/* BALL BALANCING USING A PID CONTROL with 6" resistive Touchscreen */ ////////////////////////////////////////////////////// ///Libraries/// #include <PID_v1.h> #include <stdint.h> #include <TouchScreen.h> #include <Servo.h>
//______________________________________________________________ // Definitions TOUCH PINS #define YP A1 //brown #define XM A2 //black #define YM 7 //yellow #define XP 6 //red
//______________________________________________________________ // For better pressure precision, we need to know the resistance // between XP and XM Use any multimeter to read it // For the 6" its 273 ohms across the X plate #define Rx 273 // Resistance in Ohm of Touchscreen measured in X-Direction TouchScreen ts = TouchScreen(XP, YP, XM, YM, Rx);
// Coordinates Touchscreen TSPoint p; double xmin = 146.0; //154 double xmax = 854.0; //851 double xLength = 160.0; //Width of Touchscreen in mm at 6.0" 158
double ymin = 63.0; //63.0
double ymax = 953.0; //950.0
double yLength = 96.0; //Length of Touchscreen in mm at 6.0" 94
double convertX = xLength / (xmax - xmin); // converts raw x values to mm. found through manual calibration 0.2295 0.225988 double convertY = yLength / (ymax - ymin); // converts raw y values to mm. found through manual calibration 0.1082 0.107865
//______________________________________________________________ //Center-Position double x0=74.35; double y0=42.71; //center Correct: 0.5//-7 double x0=74.35; double y0=42.71 //______________________________________________________________
int state = 0;
// servos variables Servo servoX; //X axis Servo servoY; //Y axis
/////TIME SAMPLE int Ts = 45; //Delay between taking samples/measurements
unsigned long Stable=0; unsigned int noTouchCount = 0; //variable for noTouch //______________________________________________________________
double Kpx = 0.08; //Kpx = 0.08; //Proportional (P) Get the ball to set point
double Kix = 0.02; //Kix = 0.02 0.022 //Integral (I)
double Kdx = 0.035; //Kdx = 0.035; //Derivative (D) Stop the ball at set point
double Kpy =0.08; //Kpy = 0.08
double Kiy =0.02; //Kiy = 0.02 0.022;
double Kdy =0.035; //Kdy = 0.035;
//______________________________________________________________ //INIT PID double SetpointX, InputX, OutputX; //for X double SetpointY, InputY, OutputY; //for Y PID myPIDX(&InputX, &OutputX, &SetpointX, Kpx, Kix, Kdx, REVERSE); PID myPIDY(&InputY, &OutputY, &SetpointY, Kpy, Kiy, Kdy, REVERSE); //______________________________________________________________
void setup(){ servoX.attach(9); servoY.attach(10);
OutputX=92; OutputY=100; servoX.write(OutputX); // Make Plate flat in X-Direction servoY.write(OutputY); // Make Plate flat in Y-Direction
//INIT OF TOUSCHSCREEN p = ts.getPoint();
//INIT SETPOINT, Center of Plate SetpointX = 0.0; SetpointY = 0.0;
//Setup PID Controller myPIDX.SetMode(AUTOMATIC); myPIDX.SetOutputLimits(77, 107); // 83 107 myPIDY.SetMode(AUTOMATIC); myPIDY.SetOutputLimits(85, 115); // -- 81 105
// TIME SAMPLE
myPIDX.SetSampleTime(Ts);
myPIDY.SetSampleTime(Ts);
delay(100);
state=0; setDesiredPosition();
} // end of setup
void loop()
{
while(Stable < 15) //REGULATION LOOP
{
p = ts.getPoint(); //measure pressure on plate
if (p.x > 0 ) //ball is on plate
{
servoX.attach(9); servoY.attach(10); //connect servos
setDesiredPosition();
noTouchCount = 0;
p = ts.getPoint(); // measure actual position
InputX = (p.x - xmin) * convertX; // read and convert X coordinate
InputY = (p.y - ymin) * convertY; // read and convert Y coordinate
if((InputX == SetpointX && InputY == SetpointY ))
{ Stable = Stable + 1;}
myPIDX.Compute(); // action control X compute
myPIDY.Compute(); // action control Y compute
}
else //if there is no ball on plate
{
noTouchCount++; //increment no touch count
if(noTouchCount == 150)
{
noTouchCount++;
OutputX=92; OutputY=100; //make plate flat
servoX.write(OutputX); servoY.write(OutputY);
}
if(noTouchCount == 300) //if there is no ball on plate longer, detach servos
{ servoX.detach(); servoY.detach(); }
}
servoX.write(OutputX); servoY.write(OutputY); //control
} // END OF REGULATION LOOP
servoX.detach(); servoY.detach(); //detach servos
///STABILITY//// while(Stable==15) //if is stable { //still measure actual postiion setDesiredPosition(); p = ts.getPoint(); InputX = (p.x - xmin) * convertX; // read and convert X coordinate InputY = (p.y - ymin) * convertY; // read and convert Y coordinate if(InputX < SetpointX || InputX > SetpointX || InputY > SetpointY || InputY < SetpointY ) //if ball isnt close to setpoint { servoX.attach(9); //again attach servos servoY.attach(10); Stable=0; //change STABLE state }
}//end of STABLE LOOP }//loop end
//______________________________________________________________ //#### DESIRED POSITION #### void setDesiredPosition(){ switch(state){
// Start-Sequence
case 0: SetpointX = x0;
SetpointY = y0;
p = ts.getPoint();
if (p.x > 0)
{
state = 1;
} // if ball is on plate
else if (p.x <= 0)
{
state = 0;
setDesiredPosition();
} // if no ball is on plate
break;
// Center-Position
case 1: SetpointX = x0;
SetpointY = y0;
state = 1;
break;
default : state = 0; break;
} //end of switch state } //end of setDesiredPosition()
//______________________________________________________________
/*
BALL BALANCING USING A PID CONTROL with 6" resistive Touchscreen
*/
//////////////////////////////////////////////////////
///Libraries///
#include <PID_v1.h>
#include <stdint.h>
#include <TouchScreen.h>
#include <Servo.h>
//______________________________________________________________
// Definitions TOUCH PINS
#define YP A1 //brown
#define XM A2 //black
#define YM 7 //yellow
#define XP 6 //red
//______________________________________________________________
// For better pressure precision, we need to know the resistance
// between XP and XM Use any multimeter to read it
// For the 6" its 273 ohms across the X plate
#define Rx 273 // Resistance in Ohm of Touchscreen measured in X-Direction
TouchScreen ts = TouchScreen(XP, YP, XM, YM, Rx);
// Coordinates Touchscreen
TSPoint p;
double xmin = 146.0; //154
double xmax = 854.0; //851
double xLength = 160.0; //Width of Touchscreen in mm at 6.0" 158
double ymin = 63.0; //63.0
double ymax = 953.0; //950.0
double yLength = 96.0; //Length of Touchscreen in mm at 6.0" 94
double convertX = xLength / (xmax - xmin); // converts raw x values to mm. found through manual calibration 0.2295 0.225988
double convertY = yLength / (ymax - ymin); // converts raw y values to mm. found through manual calibration 0.1082 0.107865
//______________________________________________________________
//Center-Position
double x0=74.35; double y0=42.71; //center Correct: 0.5//-7 double x0=74.35; double y0=42.71
//______________________________________________________________
int state = 0;
// servos variables
Servo servoX; //X axis
Servo servoY; //Y axis
/////TIME SAMPLE
int Ts = 45; //Delay between taking samples/measurements
unsigned long Stable=0;
unsigned int noTouchCount = 0; //variable for noTouch
//______________________________________________________________
double Kpx = 0.08; //Kpx = 0.08; //Proportional (P) Get the ball to set point
double Kix = 0.02; //Kix = 0.02 0.022 //Integral (I)
double Kdx = 0.035; //Kdx = 0.035; //Derivative (D) Stop the ball at set point
double Kpy =0.08; //Kpy = 0.08
double Kiy =0.02; //Kiy = 0.02 0.022;
double Kdy =0.035; //Kdy = 0.035;
//______________________________________________________________
//INIT PID
double SetpointX, InputX, OutputX; //for X
double SetpointY, InputY, OutputY; //for Y
PID myPIDX(&InputX, &OutputX, &SetpointX, Kpx, Kix, Kdx, REVERSE);
PID myPIDY(&InputY, &OutputY, &SetpointY, Kpy, Kiy, Kdy, REVERSE);
//______________________________________________________________
void setup(){
servoX.attach(9);
servoY.attach(10);
OutputX=92;
OutputY=100;
servoX.write(OutputX); // Make Plate flat in X-Direction
servoY.write(OutputY); // Make Plate flat in Y-Direction
//INIT OF TOUSCHSCREEN
p = ts.getPoint();
//INIT SETPOINT, Center of Plate
SetpointX = 0.0;
SetpointY = 0.0;
//Setup PID Controller
myPIDX.SetMode(AUTOMATIC);
myPIDX.SetOutputLimits(77, 107); // 83 107
myPIDY.SetMode(AUTOMATIC);
myPIDY.SetOutputLimits(85, 115); // -- 81 105
// TIME SAMPLE
myPIDX.SetSampleTime(Ts);
myPIDY.SetSampleTime(Ts);
delay(100);
state=0;
setDesiredPosition();
} // end of setup
void loop()
{
while(Stable < 15) //REGULATION LOOP
{
p = ts.getPoint(); //measure pressure on plate
if (p.x > 0 ) //ball is on plate
{
servoX.attach(9); servoY.attach(10); //connect servos
setDesiredPosition();
noTouchCount = 0;
p = ts.getPoint(); // measure actual position
InputX = (p.x - xmin) * convertX; // read and convert X coordinate
InputY = (p.y - ymin) * convertY; // read and convert Y coordinate
if((InputX == SetpointX && InputY == SetpointY ))
{ Stable = Stable + 1;}
myPIDX.Compute(); // action control X compute
myPIDY.Compute(); // action control Y compute
}
else //if there is no ball on plate
{
noTouchCount++; //increment no touch count
if(noTouchCount == 150)
{
noTouchCount++;
OutputX=92; OutputY=100; //make plate flat
servoX.write(OutputX); servoY.write(OutputY);
}
if(noTouchCount == 300) //if there is no ball on plate longer, detach servos
{ servoX.detach(); servoY.detach(); }
}
servoX.write(OutputX); servoY.write(OutputY); //control
} // END OF REGULATION LOOP
servoX.detach(); servoY.detach(); //detach servos
///STABILITY////
while(Stable==15) //if is stable
{ //still measure actual postiion
setDesiredPosition();
p = ts.getPoint();
InputX = (p.x - xmin) * convertX; // read and convert X coordinate
InputY = (p.y - ymin) * convertY; // read and convert Y coordinate
if(InputX < SetpointX || InputX > SetpointX || InputY > SetpointY || InputY < SetpointY ) //if ball isnt close to setpoint
{
servoX.attach(9); //again attach servos
servoY.attach(10);
Stable=0; //change STABLE state
}
}//end of STABLE LOOP
}//loop end
//______________________________________________________________
//#### DESIRED POSITION ####
void setDesiredPosition(){
switch(state){
// Start-Sequence
case 0: SetpointX = x0;
SetpointY = y0;
p = ts.getPoint();
if (p.x > 0)
{
state = 1;
} // if ball is on plate
else if (p.x <= 0)
{
state = 0;
setDesiredPosition();
} // if no ball is on plate
break;
// Center-Position
case 1: SetpointX = x0;
SetpointY = y0;
state = 1;
break;
default : state = 0; break;
} //end of switch state
} //end of setDesiredPosition()
//______________________________________________________________
