Downloads

Below are text files for the Arduino to perform each of the tasks necessary during the assembly of your Ball & Plate kit. Cut and paste each text file into an empty sketch in the Arduino UI, then load the Arduino code to your Kit.

Downloads

//////////////////////////////////////////////////////
//////////////////////////////////////////////////////
//////////BALL AND PLATE LED TEST BKKLLC//////////////
/////////////////////R103.01//////////////////////////
//////////////////////////////////////////////////////

int led = A2;

int led2 = A3;

int led3 = 3;

int led4 = A4;

int led5 = A5;

void setup() {

pinMode(led, OUTPUT);

pinMode(led2, OUTPUT);

pinMode(led3, OUTPUT);

pinMode(led4, OUTPUT);

pinMode(led5, OUTPUT);

}

// LOOPS FOREVER

void loop() {

{digitalWrite(led, HIGH);

delay (15);

digitalWrite(led, LOW);

delay(15);

digitalWrite(led, HIGH);

delay(15);

digitalWrite(led, LOW);

delay(15);}

{digitalWrite(led2, HIGH);

delay (15);

digitalWrite(led2, LOW);

delay(15);

digitalWrite(led2, HIGH);

delay(15);

digitalWrite(led2, LOW);

delay(15);}

{digitalWrite(led3, HIGH);

delay (15);

digitalWrite(led3, LOW);

delay(15);

digitalWrite(led3, HIGH);

delay(15);

digitalWrite(led3, LOW);

delay(15);}

{digitalWrite(led4, HIGH);

delay (15);

digitalWrite(led4, LOW);

delay(15);

digitalWrite(led4, HIGH);

delay(15);

digitalWrite(led4, LOW);

delay(15);}

{digitalWrite(led5, HIGH);

delay (15);

digitalWrite(led5, LOW);

delay(15);

digitalWrite(led5, HIGH);

delay(15);

digitalWrite(led5, LOW);

delay(15);}

{digitalWrite(led5, HIGH);

delay (15);

digitalWrite(led5, LOW);

delay(15);

digitalWrite(led5, HIGH);

delay(15);

digitalWrite(led5, LOW);

delay(15);}

{digitalWrite(led4, HIGH);

delay (15);

digitalWrite(led4, LOW);

delay(15);

digitalWrite(led4, HIGH);

delay(15);

digitalWrite(led4, LOW);

delay(15);}

{digitalWrite(led3, HIGH);

delay (15);

digitalWrite(led3, LOW);

delay(15);

digitalWrite(led3, HIGH);

delay(15);

digitalWrite(led3, LOW);

delay(15);}

{digitalWrite(led2, HIGH);

delay (15);

digitalWrite(led2, LOW);

delay(15);

digitalWrite(led2, HIGH);

delay(15);

digitalWrite(led2, LOW);

delay(15);}

{digitalWrite(led, HIGH);

delay (15);}

digitalWrite(led, LOW);

delay(15);

digitalWrite(led, HIGH);

delay(15);

digitalWrite(led, LOW);

delay(15);}

//}

Ball & Plate Servo Test

Ball & Plate PID Control

//////////////////////////////////////////////////////
//////////////////////////////////////////////////////
/////////BALL AND PLATE PID CONTROL BKKLLC////////////
////////////////////// R103.01////////////////////////
//////////////////////////////////////////////////////
///Libraries///
#include <IRremote.h>                   // insert IRremote.h library
#include <PID_v1.h>
#include <stdint.h>
#include "TouchScreen.h"
#include <SPI.h>
#include <Wire.h>
#include<Servo.h>

// touchscreen connections
#define XP 11 //pin D11 on Nano, pin 1 on touchscreen can be a "D"
#define YP A0 // pin A1 on Nano, pin 2 on touchscreen has to be an "An" was A0, change for R102, changed back for R103
#define XM A1 // pin A2 on Nano, pin 3 on touchscreen has to be an "An" was A1 change for R102, changed back for R103
#define YM 10  //pin D10 on Nano, pin 4 on touchscreen can be a "D"

// servo connection
#define ServoX 5 // D5 X-axis servo PWM
#define ServoY 6 // D6 Y-axis servo PWM

// servo tuning adjust servo arm to allow arm assembly variation
int AdjustXservo = -8;
int AdjustYservo = -8;

// IR pin connection
#define IRPin 2 // D2 IR Receiver output on D2 interrupt 1 pin

//interrupt set up
const byte interruptPin = 2;

//Blue position LED's
#define BlueLed1 A2  // blue LED in position A2 pin, A0 for R102, A2 for R103
#define BlueLed3 A3  // blue LED in position A3 pin 
#define BlueLed5 3  // blue LED in position D3 pin 
#define BlueLed7 A4  // blue LED in position A4 pin 
#define BlueLed9 A5  // blue LED in position A5 pin on 6 pin header

//IR Remote button codes
#define IRButtonCH 16736925
#define IRButton1 16724175
#define IRButton3 16743045
#define IRButton5 16726215
#define IRButton7 16728765
#define IRButton9 16732845
#define RepeatValue 4294967295  //value broadcast by remote when button is held down

//baud rate
#define BaudRate 115200  //was 115200

//touchscreen
TouchScreen ts = TouchScreen(XP, YP, XM, YM, 640);// was 300
uint16_t homeX = 500;            // was 550 raw data value for center of touchscreen
uint16_t homeY = 500;            // was 550 raw data value for center of touchscreen

float convertX = .158;  // converts raw x values to mm. found through manual calibration was 151 955
float convertY = .098;   // converts raw y values to mm. found through manual calibration was 91 927
unsigned int noTouchCount = 0; //viariable for noTouch

//position variables
volatile int CHButtonCounter = 0 ;   // was 1 counter for the number of ch button presses volatile because it is in interrupt routine
volatile long LastButtonValue = 0;     // previous state of the button, holds value so it can compare if the remote button is held down volatile because it is in interrupt routine
int flag , flagZ ;
float xVal , yVal ;
int cCount = 0;
int flagC = 0;
int flagK = 0;
float kk = 0;
double l = 0.00;
volatile double k = 0;  //integer for trajectory calculation

// PID values
volatile double Setpoint, Input, Output; //for X
volatile double Setpoint1, Input1, Output1; //for Y
//PID const
float Kp = 0.3;       //Proportional (P)
float Ki = 0.03;      //Integral (I)
float Kd = 0.13;      //Derivative (D)

float Kp1 = 0.3;       //Proportional (P)
float Ki1 = 0.03;      //Integral (I)
float Kd1 = 0.13;      //Derivative (D)
long cas = 0;
//INIT PID
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
PID myPID1(&Input1, &Output1, &Setpoint1, Kp1, Ki1, Kd1, DIRECT);

// control
int Modulo;
long lastcas = 0;
/////TIME SAMPLE
int Ts = 50; //was 50
unsigned long Stable = 0;

// servos variables
Servo servo1; //X axis
Servo servo2; //Y axis

// IR Remote
IRrecv irrecv(IRPin);                 //define IR PIN for infrared receiver
volatile decode_results results;                  //define variable results to save the result of infrared receiver added volatile because it is interrupt routine
volatile int SkipCHcount = 0;

//LED control
volatile char LedID = BlueLed5;

void setup()
{
  servo1.attach(ServoX);
  servo2.attach(ServoY);
  Output = 95; //was 90 this is servo arm center position
  Output1 = 95; //was 90
  servo1.write(Output + AdjustXservo); //servo arm with adjustment so that plane is perfectly flat
  servo2.write(Output1 + AdjustYservo);

//IR receiver
  irrecv.enableIRIn();                      //Boot infrared decoding
  pinMode(IRPin, INPUT);                    // set pin to receive

//INIT PINS
  pinMode(BlueLed1, OUTPUT);  //these LEDS are to show searching and stable on the arduino
  pinMode(BlueLed3, OUTPUT);
  pinMode(BlueLed5, OUTPUT);
  pinMode(BlueLed7, OUTPUT);
  pinMode(BlueLed9, OUTPUT);
  digitalWrite(LedID, LOW);
  Serial.begin(BaudRate);

//INIT OF TOUSCHSCREEN
  TSPoint p = ts.getPoint();
  Input = 84; //was 120 defualt center position for ball
  Input1 = 53; //was 65
  //INIT SETPOINT
  Setpoint = 84; //was 120 this sets the setpoint target to the default position for the ball
  Setpoint1 = 53; //was 65
  //// Make plate flat
  servo1.attach(ServoX); //was(5)
  servo2.attach(ServoY); //was(6)
  Output = 95;
  Output1 = 95;
  servo1.write(Output + AdjustXservo);
  servo2.write(Output1 + AdjustYservo);

//Zapnutie PID
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(20, 160);  //was(20, 160) this limits servo arm movement to the set range
  myPID1.SetMode(AUTOMATIC);
  myPID1.SetOutputLimits(20, 160);  //was (20, 160)
  // TIME SAMPLE
  myPID1.SetSampleTime(Ts);
  myPID.SetSampleTime(Ts);
  /////
  delay(100);  //was 100
  //interupt setup
  attachInterrupt(digitalPinToInterrupt(interruptPin), IRinput, CHANGE);
}  //end setup

void loop() {
  while (Stable < 125) //REGULATION LOOP
  {
    TSPoint p = ts.getPoint();   //measure pressure on plate
    if (p.z > ts.pressureThreshhold) //ball is on plate
    {
      servo1.attach(ServoX); //connect servos
      servo2.attach(ServoY);
      setTrajectory();
      noTouchCount = 0;  //timer value for ball not present
      TSPoint p = ts.getPoint(); // measure actual position
      Input = (p.x * convertX); // read and convert X coordinate
      Input1 = (p.y * convertY); // read and convert Y coordinate
      if ((Input > Setpoint - 2 && Input < Setpoint + 2 && Input1 > Setpoint1 - 2 && Input1 < Setpoint1 + 2)) //if ball is close to setpoint
      {
        Stable = Stable + 1; //increment STABLE
        digitalWrite(LedID, HIGH); // if its stable lights led 9
      }
      else
      {
        digitalWrite(LedID, LOW);  //if not stable turns off led 9
      }
      myPID.Compute();  //action control X compute
      myPID1.Compute(); //   action control  Y compute
    }
    else //if there is no ball on plate
    {
      noTouchCount++; //increment no touch count
      if (noTouchCount == 75)
      {
        noTouchCount++;
        Output = 95; //make plate flat
        Output = 95;
        servo1.write(Output + AdjustXservo);
        servo2.write(Output1 + AdjustYservo);
        digitalWrite(LedID, HIGH);
      }
      if (noTouchCount == 150) //if there is no ball on plate longer
      {
        servo1.detach(); //detach servos
        servo2.detach();
      }
    }
    servo1.write(Output + AdjustXservo);//control
    servo2.write(Output1 + AdjustYservo);//control
    //  Serial.print(Setpoint);   Serial.print(",");  Serial.print(Setpoint1);  Serial.print(",");  Serial.print(Input);Serial.print(","); Serial.println(Input1);
    //Serial.println(CHButtonCounter);     // turn off and on to print value for trouble shooting, plot output to see representation of control
  }
  ////END OF REGULATION LOOP///

servo1.detach();//detach servos
  servo2.detach();
  ///KONTROLA STABILITY////
  while (Stable == 125) //if is stable
  { //still measure actual postiion
    setTrajectory();
    TSPoint p = ts.getPoint();
    Input = (p.x * convertX); //read X
    Input1 = (p.y * convertY); //read Y
    if (Input < Setpoint - 2 || Input > Setpoint + 2 || Input1 > Setpoint1 + 2 || Input1 < Setpoint1 - 2  ) //if ball isnt close to setpoint
    {
      servo1.attach(ServoX); //again attach servos/
      servo2.attach(ServoY);
      digitalWrite(LedID, LOW);
      Stable = 0; //change STABLE state
    }
  }//end of STABLE LOOP
}//loop end

////////////////////////Functions//////////////////

//get IR input
void IRinput() {
  if (irrecv.decode(&results)) {
    if (results.value == RepeatValue)
      LastButtonValue = LastButtonValue;
    else
      LastButtonValue = results.value;
    irrecv.resume(); // Receive the next value
  }
  SkipCHcount = SkipCHcount + 1;
  if (SkipCHcount > 50) SkipCHcount = 1;
  if (SkipCHcount = 1) {
    //if CH is pressed, increment up until 4 which doesnt exist so resets to 1
    if (LastButtonValue == IRButtonCH) {  // if CH is pushed
      CHButtonCounter = CHButtonCounter + 1; //Ch button counter is incremented
      LastButtonValue = 0;
    } //end if
    if (CHButtonCounter > 4) CHButtonCounter = 1;
  } //end if
  switch (LastButtonValue) {
    case IRButton1: //remote button 1
      Setpoint = 109;
      Setpoint1 = 38;
      CHButtonCounter = 0;
      LedID = BlueLed1;
      digitalWrite(BlueLed3, LOW);
      digitalWrite(BlueLed5, LOW);
      digitalWrite(BlueLed7, LOW);
      digitalWrite(BlueLed9, LOW);
      LastButtonValue = 0;
      break;
    case IRButton3: //remote button 3
      Setpoint = 54;
      Setpoint1 = 38;
      CHButtonCounter = 0;
      LedID = BlueLed3;
      digitalWrite(BlueLed1, LOW);
      digitalWrite(BlueLed5, LOW);
      digitalWrite(BlueLed7, LOW);
      digitalWrite(BlueLed9, LOW);
      LastButtonValue = 0;
      break;
    case IRButton5: //remote button 5
      Setpoint = 84;
      Setpoint1 = 53;
      CHButtonCounter = 0;
      LedID = BlueLed5;
      digitalWrite(BlueLed3, LOW);
      digitalWrite(BlueLed1, LOW);
      digitalWrite(BlueLed7, LOW);
      digitalWrite(BlueLed9, LOW);
      LastButtonValue = 0;
      break;
    case IRButton7: //remote button 7
      Setpoint = 109;
      Setpoint1 = 63;
      CHButtonCounter = 0;
      LedID = BlueLed7;
      digitalWrite(BlueLed3, LOW);
      digitalWrite(BlueLed5, LOW);
      digitalWrite(BlueLed1, LOW);
      digitalWrite(BlueLed9, LOW);
      LastButtonValue = 0;
      break;
    case IRButton9: //remote button 9
      Setpoint = 54;
      Setpoint1 = 63;
      CHButtonCounter = 0;
      LedID = BlueLed9;
      digitalWrite(BlueLed3, LOW);
      digitalWrite(BlueLed5, LOW);
      digitalWrite(BlueLed7, LOW);
      digitalWrite(BlueLed1, LOW);
      LastButtonValue = 0;
      break;
  }
}
///// DESIRED POSITION
void setTrajectory() {
  // shape trajectories using channel button

switch (CHButtonCounter) {
    case 1: //LEMNISCATE (figure 8) TRAJECTORY
      Setpoint = 84 + (30 * cos(k)) / (1 + sin(k) * sin(k));//was 35
      Setpoint1 = 53 + (30 * sin(k) * cos(k)) / (1 + sin(k) * sin(k));//was35
      k = k + 0.010;//was.008
      digitalWrite(BlueLed3, LOW);
      digitalWrite(BlueLed5, HIGH);
      digitalWrite(BlueLed7, LOW);
      digitalWrite(BlueLed9, LOW);
      digitalWrite(BlueLed1, HIGH);
      break;
    case 2: // CIRCLE TRAJECTORY
      Setpoint = 84 + 30 * cos(k); //BIGGER SMALLER
      Setpoint1 = 53 + 30 * sin(k);
      k = k - 0.02; //WAS 0.012  //FASTER SLOWER.026 very fast
      digitalWrite(BlueLed5, HIGH);
      digitalWrite(BlueLed7, LOW);
      digitalWrite(BlueLed9, LOW);
      digitalWrite(BlueLed1, HIGH);
      digitalWrite(BlueLed3, HIGH);
      break;
    case 3: // ELLIPSE TRAJECORY
      Setpoint = 84 + 40 * cos(k);
      Setpoint1 = 53 + 25 * sin(k);
      k = k - 0.02;
      digitalWrite(BlueLed5, HIGH);
      digitalWrite(BlueLed9, LOW);
      digitalWrite(BlueLed1, HIGH);
      digitalWrite(BlueLed3, HIGH);
      digitalWrite(BlueLed7, HIGH);
      break;
    case 4: //PENTAGRAM TRAJECOTRY
      Setpoint = 84 +  18 * cos(k) + 12 * cos(k * 150); //
      Setpoint1 = 53 + 18 * sin(k) - 12 * sin(k * 150); //
      k = k + 0.01;
      digitalWrite(BlueLed5, HIGH);
      digitalWrite(BlueLed1, HIGH);
      digitalWrite(BlueLed3, HIGH);
      digitalWrite(BlueLed7, HIGH);
      digitalWrite(BlueLed9, HIGH);
      break;
      LastButtonValue = 0;
  }
}

Ball & Plate LED Test

Home Products Downloads Contact