/////////Ball on Plate/////////////////////////////// /* BALL AND PLATE PID CONTROL with 8" resistive Touchscreen */ ////////////////////////////////////////////////////// ///Libraries/// #include #include #include #include //______________________________________________________________ // Definitions LED PINS for showing Positions int L1= 4; int L2= 7; int L3= 8; int L4= 9; int L5= 10; int L6= 11; int L7= 12; int L8= 14; int L9= 15; //______________________________________________________________ // Definitions TOUCH PINS #define YP A4 //0 #define XM A5 //1 #define YM 2 //3 #define XP 3 //4 //______________________________________________________________ // For better pressure precision, we need to know the resistance // between XP and XM Use any multimeter to read it // For the 8.0" its 227 ohms across the X plate #define Rx 227 // Resistance in Ohm of Touchscreen measured in X-Direction TouchScreen ts = TouchScreen(XP, YP, XM, YM, Rx); // Coordinates Touchscreen TSPoint p; double xmin = 60.0; double xmax = 830.0; double xm = (xmax - xmin) / 2.0; // x - Center double xLength = 174.0; //Width of Touchscreen in mm at 8.0" double ymin = 380.0; //380.0 double ymax = 900.0; //900.0 double ym = (ymax - ymin) / 2.0; // y - Center double yLength = 97.0; //Length of Touchscreen in mm at 8.0" double convertX = xLength / (xmax - xmin); // converts raw x values to mm. found through manual calibration double convertY = yLength / (ymax - ymin); // converts raw y values to mm. found through manual calibration //______________________________________________________________ //Center-Position double x0= 20; double y0= -25; //center //______________________________________________________________ //Coordinates for Square-Sequence double x1= 55; double y1= -60; //lower left double x2= -25; double y2= -60; //lower right double x3= -25; double y3= 25; //upper right double x4= 55; double y4= 25; //upper left //______________________________________________________________ //Coordinates for Cross-Sequence double x11= 15; double y11= 5; //upper middle double x12= 15; double y12= -40; //lower middle double x13= 75; double y13= -20; //left middle double x14= -40; double y14= -20; //right middle //______________________________________________________________ //Coordinates for 8 Points-Sequence double x20= 55; double y20= -60; //lower left double x21= 75; double y21= -20; //left middle double x22= 55; double y22= 28; //upper left double x23= 15; double y23= 28; //upper middle double x24= -25; double y24= 28; //upper right double x25= -40; double y25= -20; //right middle double x26= -25; double y26= -60; //lower right double x27= 20; double y27= -60; //lower middle //______________________________________________________________ long duration = 4 * 500; // duration for each position in Square-Sequence long shortduration = 1 * 1200; // duration for each position in 8 Points-Sequence long PauseTime = 3 * 1000; // Delay between Sequences double k=0; //Increment/Decrement of angle, or Speed of Ball int state = 0; long StartTime = 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.31; //Kpx = 0.35; //Proportional (P) double Kix = 0.02; //Kix = 0.03 //Integral (I) double Kdx = 0.10; //Kdx = 0.13; //Derivative (D) double Kpy = 0.33; //Kpy = 0.35; double Kiy = 0.02; //Kiy = 0.08; double Kdy = 0.11; //Kdy = 0.13; double deadX = 4; //4 double deadY = 4; //4 double xin = 0.0; double yin = 0.0; //______________________________________________________________ //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, DIRECT); PID myPIDY(&InputY, &OutputY, &SetpointY, Kpy, Kiy, Kdy, REVERSE); //______________________________________________________________ void setup(){ //Serial.begin(115200); k=0; pinMode(L1, OUTPUT); pinMode(L2, OUTPUT); pinMode(L3, OUTPUT); pinMode(L4, OUTPUT); pinMode(L5, OUTPUT); pinMode(L6, OUTPUT); pinMode(L7, OUTPUT); pinMode(L8, OUTPUT); pinMode(L9, OUTPUT); //center servoX.attach(5); servoY.attach(6); // LED blinking start sequenz for (int i = 0; i < 1; i++){ for (int i = 0; i < 3; i++){ digitalWrite(L1, HIGH); digitalWrite(L2, HIGH); digitalWrite(L3, HIGH); digitalWrite(L4, HIGH); digitalWrite(L5, HIGH); digitalWrite(L6, HIGH); digitalWrite(L7, HIGH); digitalWrite(L8, HIGH); delay (70); LEDsOff(); delay (70); } } delay(400); // Rotate Plate just for fun for(int i=0; i<=95; i++){ OutputX=90; OutputY=90; servoX.write(OutputX + 45*cos(k)); servoY.write(OutputY + 45*sin(k)); k=k-0.05; //k=k-0.004; digitalWrite(L1, HIGH); digitalWrite(L2, HIGH); digitalWrite(L3, HIGH); digitalWrite(L4, HIGH); digitalWrite(L5, HIGH); digitalWrite(L6, HIGH); digitalWrite(L7, HIGH); digitalWrite(L8, HIGH); digitalWrite(L9, HIGH); delay(25); } // LED blinking start sequenz for (int i = 0; i < 1; i++){ for (int i = 0; i < 3; i++){ digitalWrite(L1, HIGH); digitalWrite(L2, HIGH); digitalWrite(L3, HIGH); digitalWrite(L4, HIGH); digitalWrite(L5, HIGH); digitalWrite(L6, HIGH); digitalWrite(L7, HIGH); digitalWrite(L8, HIGH); delay (70); LEDsOff(); delay (70); } } // end of blinking LEDsOff(); pinMode(13, OUTPUT); // internal LED, lights up, when position is stable digitalWrite(13,LOW); // Stable-LED init OutputX=90; OutputY=90; 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(40, 140); //40, 140 myPIDY.SetMode(AUTOMATIC); myPIDY.SetOutputLimits(40, 140); //40, 140 // TIME SAMPLE myPIDX.SetSampleTime(Ts); myPIDY.SetSampleTime(Ts); delay(100); StartTime = millis(); state =0; setDesiredPosition(); } // end of setup //______________________________________________________________ void loop() { while(Stable < 80) //REGULATION LOOP (was 125) { p = ts.getPoint(); //measure pressure on plate if (p.x > 0 ) //ball is on plate { servoX.attach(5); servoY.attach(6); //connect servos setDesiredPosition(); noTouchCount = 0; p = ts.getPoint(); // measure actual position InputX = (p.x - xmin - xm) * convertX; // read and convert X coordinate InputY = (p.y - ymin - ym) * convertY; // read and convert Y coordinate if((InputX > SetpointX - deadX && InputX < SetpointX + deadX && InputY > SetpointY - deadY && InputY < SetpointY + deadY)) //if ball is close to setpoint //increment STABLE { Stable = Stable + 1; digitalWrite(13,HIGH); } else{ digitalWrite(13,LOW); } 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 == 75) { noTouchCount++; OutputX=90; OutputY=90; //make plate flat servoX.write(OutputX); servoY.write(OutputY); } if(noTouchCount == 150) //if there is no ball on plate longer, detach servos { servoX.detach(); servoY.detach(); } } servoX.write(OutputX); servoY.write(OutputY); //control //Serial.print(SetpointX); Serial.print(","); Serial.print(SetpointY); Serial.print(","); Serial.print(InputX);Serial.print(","); Serial.println(InputY); } // END OF REGULATION LOOP servoX.detach(); servoY.detach(); //detach servos ///KONTROL STABILITY//// while(Stable==80) //if is stable (was 125) { //still measure actual postiion setDesiredPosition(); p = ts.getPoint(); InputX = (p.x - xmin - xm) * convertX; // read and convert X coordinate InputY = (p.y - ymin - ym) * convertY; // read and convert Y coordinate if(InputX < SetpointX - deadX || InputX > SetpointX + deadX || InputY>SetpointY + deadY || InputY < SetpointY - deadY ) //if ball isnt close to setpoint { servoX.attach(5); //again attach servos servoY.attach(6); digitalWrite(13,LOW); Stable=0; //change STABLE state } }//end of STABLE LOOP }//loop end ////////////////////////Functions////////////////// //______________________________________________________________ //#### DESIRED POSITION #### void setDesiredPosition(){ switch(state){ // Start-Sequence, it is time to put the ball on plate case 0: SetpointX = x0; SetpointY = y0; k=0; p = ts.getPoint(); if (p.x > 0){ LEDsOff(); StartTime = millis(); state = 1; } // if ball is on plate else if (p.x <= 0){ delay (80); digitalWrite(L9, HIGH); delay (180); LEDsOff(); StartTime = millis(); state = 0; setDesiredPosition(); } // if no ball is on plate break; // Center-Position case 1: SetpointX = x0; SetpointY = y0; digitalWrite(L9, HIGH); if ( millis() - StartTime > 2000 ){ LEDsOff(); StartTime = millis(); k=0; state = 2; }; break; // Epizykloid case 2: SetpointX =(x0-5) + ( 35 * cos(k) - 30 * cos(3 *k) ) ; SetpointY =(y0+10) + ( 8 * sin(k) - 27 * sin(3 *k) ) ; k=k-0.0032; digitalWrite(L2, HIGH); digitalWrite(L6, HIGH); digitalWrite(L9, HIGH); if ( millis() - StartTime > 13800 ){ LEDsOff(); StartTime = millis(); state = 3; }; break; // Square case 3: SetpointX = x1; //lower left SetpointY = y1; digitalWrite(L5, HIGH); if (millis() - StartTime > 2500){ LEDsOff(); StartTime = millis(); state = 4; }; break; case 4: SetpointX = x2; //lower right SetpointY = y2; digitalWrite(L3, HIGH); if ( millis() - StartTime > duration){ LEDsOff(); StartTime = millis(); state = 5; }; break; case 5: SetpointX = x3; //upper right SetpointY = y3; digitalWrite(L1, HIGH); if ( millis() - StartTime > duration){ LEDsOff(); StartTime = millis(); state = 6; }; break; case 6: SetpointX = x4; //upper left SetpointY = y4; digitalWrite(L7, HIGH); if ( millis() - StartTime > duration){ LEDsOff(); StartTime = millis(); k=0; state = 7; }; break; /* // Circle case 6: SetpointX = (x0) + 45*cos(k); SetpointY = (y0) + 45*sin(k); k=k-0.0045; //k=k-0.004; digitalWrite(L1, HIGH); digitalWrite(L2, HIGH); digitalWrite(L3, HIGH); digitalWrite(L4, HIGH); digitalWrite(L5, HIGH); digitalWrite(L6, HIGH); digitalWrite(L7, HIGH); digitalWrite(L8, HIGH); if ( millis() - StartTime > 16000 ){ LEDsOff(); StartTime = millis(); state = 7; }; break; */ // Growing Circle case 7: SetpointX = x0 + ((1.4*k))*cos(k); SetpointY = y0 + ((1.4*k))*sin(k); k=k+0.007; //k=k-0.004; digitalWrite(L1, HIGH); digitalWrite(L2, HIGH); digitalWrite(L3, HIGH); digitalWrite(L4, HIGH); digitalWrite(L5, HIGH); digitalWrite(L6, HIGH); digitalWrite(L7, HIGH); digitalWrite(L8, HIGH); if ( k >= 31.8 ){ LEDsOff(); StartTime = millis(); k=0; state = 8; }; break; // Shrinking Circle case 8: SetpointX = x0 + (55+(2*k))*cos(k); SetpointY = y0 + (55+(2*k))*sin(k); k=k-0.007; //k=k-0.004; digitalWrite(L1, HIGH); digitalWrite(L2, HIGH); digitalWrite(L3, HIGH); digitalWrite(L4, HIGH); digitalWrite(L5, HIGH); digitalWrite(L6, HIGH); digitalWrite(L7, HIGH); digitalWrite(L8, HIGH); if ( k <= -24.0 ){ LEDsOff(); StartTime = millis(); k=0; state = 9; }; break; // Cross case 9: SetpointX = x11; //upper middle SetpointY = y11; digitalWrite(L1, HIGH); digitalWrite(L7, HIGH); digitalWrite(L8, HIGH); digitalWrite(L9, HIGH); if (millis() - StartTime > 500 ){ LEDsOff(); StartTime = millis(); state = 10; }; break; case 10: SetpointX = x11 + 30*cos(k); //upper middle swing SetpointY = y11; k=k-0.01; digitalWrite(L1, HIGH); digitalWrite(L7, HIGH); digitalWrite(L8, HIGH); digitalWrite(L9, HIGH); if ( millis() - StartTime > 8000 ){ LEDsOff(); StartTime = millis(); state = 11; }; break; case 11: SetpointX = x12; //lower middle SetpointY = y12; digitalWrite(L3, HIGH); digitalWrite(L4, HIGH); digitalWrite(L5, HIGH); digitalWrite(L9, HIGH); if ( millis() - StartTime > 500 ){ LEDsOff(); StartTime = millis(); state = 12; }; break; case 12: SetpointX = x12 + 30*cos(k); //lower middle swing SetpointY = y12; k=k-0.01; digitalWrite(L3, HIGH); digitalWrite(L4, HIGH); digitalWrite(L5, HIGH); digitalWrite(L9, HIGH); if ( millis() - StartTime > 8000 ){ LEDsOff(); StartTime = millis(); state = 13; }; break; // Center-Position case 13: SetpointX = x0; SetpointY = y0; k=0; digitalWrite(L2, HIGH); digitalWrite(L9, HIGH); if ( millis() - StartTime > 300 ){ LEDsOff(); StartTime = millis(); state = 14; } break; //Right to left wandering circle case 14: digitalWrite(L2, HIGH); //right to left wandering circle digitalWrite(L6, HIGH); digitalWrite(L9, HIGH); SetpointX = (x14 + 2.1*k) + 14*cos(k); //12 SetpointY = (y14) + 21*sin(k); k=k+0.017; //k=k+0.01; if ( millis() - StartTime > 7000 ){ LEDsOff(); StartTime = millis(); k=0; state = 15; }; break; //Left to right wandering circle case 15: digitalWrite(L2, HIGH); //left to right wandering circle digitalWrite(L6, HIGH); digitalWrite(L9, HIGH); SetpointX = (x13 + 2.1*k) + 14*cos(k); //12 SetpointY = (y13) + 21*sin(k); k=k-0.017; //k=k+0.01; if ( millis() - StartTime > 7000 ){ LEDsOff(); StartTime = millis(); k=0; state = 16; }; break; // Center-Position case 16: SetpointX = x0; SetpointY = y0; digitalWrite(L9, HIGH); if ( millis() - StartTime > PauseTime ){ LEDsOff(); StartTime = millis(); state = 17; }; break; // Lemniscate case 17: SetpointX = (x0) + (60*cos(k))/(1+sin(k)*sin(k)); SetpointY = (y0+5) + (110*sin(k)*cos(k))/(1+sin(k)*sin(k)); k=k+0.003; digitalWrite(L1, HIGH); digitalWrite(L2, HIGH); digitalWrite(L3, HIGH); digitalWrite(L9, HIGH); digitalWrite(L5, HIGH); digitalWrite(L6, HIGH); digitalWrite(L7, HIGH); if ( millis() - StartTime > 16300 ){ LEDsOff(); StartTime = millis(); state = 18; }; break; // Swing case 18: SetpointX = x0 + 60*cos(k); SetpointY = y0; k=k-0.005; digitalWrite(L6, HIGH); digitalWrite(L2, HIGH); if ( millis() - StartTime > 15500 ){ LEDsOff(); StartTime = millis(); k=0; state = 19; }; break; // Deltoid case 19: SetpointX = (x0-10) + ( (35* cos(k/3)) + (15*cos(2*k/3)) ); SetpointY = (y0+10) + ( (30* sin(k/3)) - (15*sin(2*k/3)) ); k=k-0.018; digitalWrite(L1, HIGH); digitalWrite(L3, HIGH); digitalWrite(L6, HIGH); if ( millis() - StartTime > 10000 ){ LEDsOff(); StartTime = millis(); state = 20; }; break; // Center-Position case 20: SetpointX = x0; SetpointY = y0; digitalWrite(L9, HIGH); if ( millis() - StartTime > PauseTime ){ LEDsOff(); StartTime = millis(); k=0; state = 21; }; break; // 8 Points case 21: SetpointX = x20; //lower left SetpointY = y20; digitalWrite(L5, HIGH); if (millis() - StartTime > shortduration){ LEDsOff(); StartTime = millis(); state = 22; }; break; case 22: SetpointX = x21; //middle left SetpointY = y21; digitalWrite(L6, HIGH); if ( millis() - StartTime > shortduration){ LEDsOff(); StartTime = millis(); state = 23; }; break; case 23: SetpointX = x22; //upper left SetpointY = y22; digitalWrite(L7, HIGH); if ( millis() - StartTime > shortduration){ LEDsOff(); StartTime = millis(); state = 24; }; break; case 24: SetpointX = x23; //upper middle SetpointY = y23; digitalWrite(L8, HIGH); if ( millis() - StartTime > shortduration){ LEDsOff(); StartTime = millis(); state = 25; }; break; case 25: SetpointX = x24; //upper right SetpointY = y24; digitalWrite(L1, HIGH); if ( millis() - StartTime > shortduration){ LEDsOff(); StartTime = millis(); state = 26; }; break; case 26: SetpointX = x25; //middle right SetpointY = y25; digitalWrite(L2, HIGH); if ( millis() - StartTime > shortduration){ LEDsOff(); StartTime = millis(); state = 27; }; break; case 27: SetpointX = x26; //lower right SetpointY = y26; digitalWrite(L3, HIGH); if ( millis() - StartTime > shortduration){ LEDsOff(); StartTime = millis(); state = 28; }; break; case 28: SetpointX = x27; //lower middle SetpointY = y27; digitalWrite(L4, HIGH); if ( millis() - StartTime > shortduration){ LEDsOff(); StartTime = millis(); state = 1; }; break; default : StartTime = millis(); k=0; state = 0; break; } //end of switch state } //end of setDesiredPosition() //______________________________________________________________ void LEDsOff(){ digitalWrite(L1, LOW); digitalWrite(L2, LOW); digitalWrite(L3, LOW); digitalWrite(L4, LOW); digitalWrite(L5, LOW); digitalWrite(L6, LOW); digitalWrite(L7, LOW); digitalWrite(L8, LOW); digitalWrite(L9, LOW); }