Components and supplies
Arduino Mega 2560
Project description
Code
Code
arduino
Code for PID control
1/////////Ball and Plate/////////////////////////////// 2/* 3BALL AND PLATE PID CONTROL 4*/ 5////////////////////////////////////////////////////// 6///Libraries/// 7#include <PID_v1.h> 8#include <stdint.h> 9#include "TouchScreen.h" 10#include <SPI.h> 11#include <Wire.h> 12#include <wiinunchuk.h> 13#include<Servo.h> 14// Definitions TOUCH PINS 15#define YP A0 //0 16#define XM A1 //1 17#define YM 3 //3 18#define XP 4 //4 19TouchScreen ts = TouchScreen(XP, YP, XM, YM, 300); 20int buttonPushCounter = 1; // counter for the number of button presses 21int lastButtonState = 0; // previous state of the button 22int flag , flagZ ; 23 24 25float xVal , yVal ; 26int cCount=0; 27int flagC=0; 28int flagK=0; 29float kk=0; 30int fl=0; 31double l =0.00; 32unsigned int noTouchCount = 0; //viariable for noTouch 33double k=0; 34// PID values 35double Setpoint, Input, Output; //for X 36double Setpoint1, Input1, Output1; //for Y 37// 38int Modulo; 39long lastcas=0; 40// servos variables 41Servo servo1; //X axis 42Servo servo2; //Y axis 43 44uint16_t homeX = 550; // raw data value for center of touchscreen 45uint16_t homeY = 550; // raw data value for center of touchscreen 46 47float convertX = 151.0 / 955.0; // converts raw x values to mm. found through manual calibration 48float convertY = 91.0 / 927.0; // converts raw y values to mm. found through manual calibration 49/////TIME SAMPLE 50int Ts = 50; 51unsigned long Stable=0; 52//PID const 53float Kp = 0.3; 54float Ki = 0.03; 55float Kd = 0.13; 56 57float Kp1 = 0.3; 58float Ki1 = 0.08; 59float Kd1 = 0.13; 60long cas=0; 61//INIT PID 62PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT); 63PID myPID1(&Input1, &Output1, &Setpoint1,Kp1,Ki1,Kd1, DIRECT); 64 65void setup() 66{ 67 servo1.attach(5); 68 servo2.attach(6); 69 Output=95; 70 Output1=95; 71 servo1.write(Output); 72 servo2.write(Output1); 73 74 //init NUN 75 nunchuk_setpowerpins(); 76 nunchuk_init(); 77 nunchuk_get_data(); 78 79 //INIT PINS 80 pinMode(9, OUTPUT); 81 pinMode(8, OUTPUT); 82 digitalWrite(9,LOW); //LED INIT 83 digitalWrite(8,LOW); 84 85 Serial.begin(115200); 86 87 //INIT OF TOUSCHSCREEN 88 TSPoint p = ts.getPoint(); 89 Input=120; 90 Input1=65; 91 //INIT SETPOINT 92 Setpoint=120; 93 Setpoint1=65; 94 //// Make plate flat 95 servo1.attach(5); 96 servo2.attach(6); 97 Output=95; 98 Output1=95; 99 servo1.write(Output); 100 servo2.write(Output1); 101 102 //Zapnutie PID 103 myPID.SetMode(AUTOMATIC); 104 myPID.SetOutputLimits(20, 160); 105 myPID1.SetMode(AUTOMATIC); 106 myPID1.SetOutputLimits(20, 160); 107 // TIME SAMPLE 108 myPID1.SetSampleTime(Ts); 109 myPID.SetSampleTime(Ts); 110 ///// 111 delay(100); 112 113 114 /// 115 } 116 117void loop() 118{ 119 while(Stable<125) //REGULATION LOOP 120 { 121 TSPoint p = ts.getPoint(); //measure pressure on plate 122 if (p.z > ts.pressureThreshhold) //ball is on plate 123 { 124 servo1.attach(5); //connect servos 125 servo2.attach(6); 126 setDesiredPosition(); 127 noTouchCount = 0; 128 TSPoint p = ts.getPoint(); // measure actual position 129 Input=(p.x * convertX); // read and convert X coordinate 130 Input1=(p.y * convertY); // read and convert Y coordinate 131 132 if((Input>Setpoint-2 && Input<Setpoint+2 && Input1>Setpoint1-2 && Input1<Setpoint1+2))//if ball is close to setpoint 133 { 134 Stable=Stable+1; //increment STABLE 135 digitalWrite(9,HIGH); 136 137 } 138 else 139 { 140 digitalWrite(9,LOW); 141 } 142 myPID.Compute(); //action control X compute 143 myPID1.Compute(); // action control Y compute 144 145 } 146 else //if there is no ball on plate 147 { 148 noTouchCount++; //increment no touch count 149 150 if(noTouchCount == 75) 151 { 152 noTouchCount++; 153 Output=95; //make plate flat 154 Output=93; 155 servo1.write(Output); 156 servo2.write(Output1); 157 } 158 if(noTouchCount == 150) //if there is no ball on plate longer 159 { 160 servo1.detach(); //detach servos 161 servo2.detach(); 162 163 } 164 } 165 servo1.write(Output);//control 166 servo2.write(Output1);//control 167 Serial.print(Setpoint); Serial.print(","); Serial.print(Setpoint1); Serial.print(","); Serial.print(Input);Serial.print(","); Serial.println(Input1); 168 169}////END OF REGULATION LOOP/// 170 171 servo1.detach();//detach servos 172 servo2.detach(); 173 174 ///KONTROLA STABILITY//// 175 while(Stable==125)//if is stable 176 { //still measure actual postiion 177 setDesiredPosition(); 178 TSPoint p = ts.getPoint(); 179 Input=(p.x * convertX); //read X 180 Input1=(p.y * convertY); //read Y 181 if(Input<Setpoint-2 || Input>Setpoint+2 || Input1>Setpoint1+2 || Input1<Setpoint1-2 ) //if ball isnt close to setpoint 182 { 183 servo1.attach(5); //again attach servos 184 servo2.attach(6); 185 digitalWrite(9,LOW); 186 Stable=0; //change STABLE state 187 } 188 189 }//end of STABLE LOOP 190}//loop end 191 192////////////////////////Functions////////////////// 193///// DESIRED POSITION 194void setDesiredPosition() 195{ 196 197 198 nunchuk_get_data(); 199 //if zbutton is pressed, zero positions 200 201 int c = nunchuk_zbutton(); 202 if (c != lastButtonState) { 203 // if the state has changed, increment the counter 204 if (c == HIGH && digitalRead(11)==0 ) { 205 // if the current state is HIGH then the button 206 // wend from off to on: 207 buttonPushCounter++; 208 } 209 } 210 lastButtonState =c; 211 212 switch (buttonPushCounter) 213 { 214 case 1: 215 Setpoint=120; 216 Setpoint1=70; 217 fl=1; 218 break; 219 case 2: 220 Setpoint=52; 221 Setpoint1=70; 222 fl=2; 223 break; 224 case 3: 225 Setpoint=52; 226 Setpoint1=40; 227 fl=3; 228 break; 229 case 4: 230 Setpoint=120; 231 Setpoint1=40; 232 buttonPushCounter=0; 233 fl=4; 234 break; 235 } 236 if (nunchuk_cbutton()&&fl==1)///LEMNISCATE TRAJECOTRY 237 { 238 Setpoint = 85+ (50*cos(k))/(1+sin(k)*sin(k)); 239 Setpoint1 = 55+ (50*sin(k)*cos(k))/(1+sin(k)*sin(k)); 240 buttonPushCounter=0; 241 k=k+0.008; 242 } 243 if (nunchuk_cbutton()&&fl==2)// CIRCLE TRAJECTORY 244 { 245 Setpoint = 85+ 25*cos(k); 246 Setpoint1 = 55+ 25*sin(k); 247 buttonPushCounter=0; 248 k=k-0.02; 249 } 250 if (nunchuk_cbutton()&&fl==3)/// ELLIPSE TRAJECORY 251 { 252 Setpoint = 85+ 40*cos(k); 253 Setpoint1 = 55+ 25*sin(k); 254 buttonPushCounter=0; 255 k=k-0.02; 256 } 257 if (nunchuk_cbutton()&&fl==4) //PENTAGRAM TRAJECOTRY 258 { 259 Setpoint =85+ 18*cos(k)+12*cos(k*150);// 260 Setpoint1 =55+ 18*sin(k)-12*sin(k*150);// 261 buttonPushCounter=0; 262 k=k+0.01; 263 } 264} 265 266
Code
arduino
Code for PID control
1/////////Ball and Plate/////////////////////////////// 2/* 3BALL AND PLATE PID CONTROL 4*/ 5////////////////////////////////////////////////////// 6///Libraries/// 7#include <PID_v1.h> 8#include <stdint.h> 9#include "TouchScreen.h" 10#include <SPI.h> 11#include <Wire.h> 12#include <wiinunchuk.h> 13#include<Servo.h> 14// Definitions TOUCH PINS 15#define YP A0 //0 16#define XM A1 //1 17#define YM 3 //3 18#define XP 4 //4 19TouchScreen ts = TouchScreen(XP, YP, XM, YM, 300); 20int buttonPushCounter = 1; // counter for the number of button presses 21int lastButtonState = 0; // previous state of the button 22int flag , flagZ ; 23 24 25float xVal , yVal ; 26int cCount=0; 27int flagC=0; 28int flagK=0; 29float kk=0; 30int fl=0; 31double l =0.00; 32unsigned int noTouchCount = 0; //viariable for noTouch 33double k=0; 34// PID values 35double Setpoint, Input, Output; //for X 36double Setpoint1, Input1, Output1; //for Y 37// 38int Modulo; 39long lastcas=0; 40// servos variables 41Servo servo1; //X axis 42Servo servo2; //Y axis 43 44uint16_t homeX = 550; // raw data value for center of touchscreen 45uint16_t homeY = 550; // raw data value for center of touchscreen 46 47float convertX = 151.0 / 955.0; // converts raw x values to mm. found through manual calibration 48float convertY = 91.0 / 927.0; // converts raw y values to mm. found through manual calibration 49/////TIME SAMPLE 50int Ts = 50; 51unsigned long Stable=0; 52//PID const 53float Kp = 0.3; 54float Ki = 0.03; 55float Kd = 0.13; 56 57float Kp1 = 0.3; 58float Ki1 = 0.08; 59float Kd1 = 0.13; 60long cas=0; 61//INIT PID 62PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT); 63PID myPID1(&Input1, &Output1, &Setpoint1,Kp1,Ki1,Kd1, DIRECT); 64 65void setup() 66{ 67 servo1.attach(5); 68 servo2.attach(6); 69 Output=95; 70 Output1=95; 71 servo1.write(Output); 72 servo2.write(Output1); 73 74 //init NUN 75 nunchuk_setpowerpins(); 76 nunchuk_init(); 77 nunchuk_get_data(); 78 79 //INIT PINS 80 pinMode(9, OUTPUT); 81 pinMode(8, OUTPUT); 82 digitalWrite(9,LOW); //LED INIT 83 digitalWrite(8,LOW); 84 85 Serial.begin(115200); 86 87 //INIT OF TOUSCHSCREEN 88 TSPoint p = ts.getPoint(); 89 Input=120; 90 Input1=65; 91 //INIT SETPOINT 92 Setpoint=120; 93 Setpoint1=65; 94 //// Make plate flat 95 servo1.attach(5); 96 servo2.attach(6); 97 Output=95; 98 Output1=95; 99 servo1.write(Output); 100 servo2.write(Output1); 101 102 //Zapnutie PID 103 myPID.SetMode(AUTOMATIC); 104 myPID.SetOutputLimits(20, 160); 105 myPID1.SetMode(AUTOMATIC); 106 myPID1.SetOutputLimits(20, 160); 107 // TIME SAMPLE 108 myPID1.SetSampleTime(Ts); 109 myPID.SetSampleTime(Ts); 110 ///// 111 delay(100); 112 113 114 /// 115 } 116 117void loop() 118{ 119 while(Stable<125) //REGULATION LOOP 120 { 121 TSPoint p = ts.getPoint(); //measure pressure on plate 122 if (p.z > ts.pressureThreshhold) //ball is on plate 123 { 124 servo1.attach(5); //connect servos 125 servo2.attach(6); 126 setDesiredPosition(); 127 noTouchCount = 0; 128 TSPoint p = ts.getPoint(); // measure actual position 129 Input=(p.x * convertX); // read and convert X coordinate 130 Input1=(p.y * convertY); // read and convert Y coordinate 131 132 if((Input>Setpoint-2 && Input<Setpoint+2 && Input1>Setpoint1-2 && Input1<Setpoint1+2))//if ball is close to setpoint 133 { 134 Stable=Stable+1; //increment STABLE 135 digitalWrite(9,HIGH); 136 137 } 138 else 139 { 140 digitalWrite(9,LOW); 141 } 142 myPID.Compute(); //action control X compute 143 myPID1.Compute(); // action control Y compute 144 145 } 146 else //if there is no ball on plate 147 { 148 noTouchCount++; //increment no touch count 149 150 if(noTouchCount == 75) 151 { 152 noTouchCount++; 153 Output=95; //make plate flat 154 Output=93; 155 servo1.write(Output); 156 servo2.write(Output1); 157 } 158 if(noTouchCount == 150) //if there is no ball on plate longer 159 { 160 servo1.detach(); //detach servos 161 servo2.detach(); 162 163 } 164 } 165 servo1.write(Output);//control 166 servo2.write(Output1);//control 167 Serial.print(Setpoint); Serial.print(","); Serial.print(Setpoint1); Serial.print(","); Serial.print(Input);Serial.print(","); Serial.println(Input1); 168 169}////END OF REGULATION LOOP/// 170 171 servo1.detach();//detach servos 172 servo2.detach(); 173 174 ///KONTROLA STABILITY//// 175 while(Stable==125)//if is stable 176 { //still measure actual postiion 177 setDesiredPosition(); 178 TSPoint p = ts.getPoint(); 179 Input=(p.x * convertX); //read X 180 Input1=(p.y * convertY); //read Y 181 if(Input<Setpoint-2 || Input>Setpoint+2 || Input1>Setpoint1+2 || Input1<Setpoint1-2 ) //if ball isnt close to setpoint 182 { 183 servo1.attach(5); //again attach servos 184 servo2.attach(6); 185 digitalWrite(9,LOW); 186 Stable=0; //change STABLE state 187 } 188 189 }//end of STABLE LOOP 190}//loop end 191 192////////////////////////Functions////////////////// 193///// DESIRED POSITION 194void setDesiredPosition() 195{ 196 197 198 nunchuk_get_data(); 199 //if zbutton is pressed, zero positions 200 201 int c = nunchuk_zbutton(); 202 if (c != lastButtonState) { 203 // if the state has changed, increment the counter 204 if (c == HIGH && digitalRead(11)==0 ) { 205 // if the current state is HIGH then the button 206 // wend from off to on: 207 buttonPushCounter++; 208 } 209 } 210 lastButtonState =c; 211 212 switch (buttonPushCounter) 213 { 214 case 1: 215 Setpoint=120; 216 Setpoint1=70; 217 fl=1; 218 break; 219 case 2: 220 Setpoint=52; 221 Setpoint1=70; 222 fl=2; 223 break; 224 case 3: 225 Setpoint=52; 226 Setpoint1=40; 227 fl=3; 228 break; 229 case 4: 230 Setpoint=120; 231 Setpoint1=40; 232 buttonPushCounter=0; 233 fl=4; 234 break; 235 } 236 if (nunchuk_cbutton()&&fl==1)///LEMNISCATE TRAJECOTRY 237 { 238 Setpoint = 85+ (50*cos(k))/(1+sin(k)*sin(k)); 239 Setpoint1 = 55+ (50*sin(k)*cos(k))/(1+sin(k)*sin(k)); 240 buttonPushCounter=0; 241 k=k+0.008; 242 } 243 if (nunchuk_cbutton()&&fl==2)// CIRCLE TRAJECTORY 244 { 245 Setpoint = 85+ 25*cos(k); 246 Setpoint1 = 55+ 25*sin(k); 247 buttonPushCounter=0; 248 k=k-0.02; 249 } 250 if (nunchuk_cbutton()&&fl==3)/// ELLIPSE TRAJECORY 251 { 252 Setpoint = 85+ 40*cos(k); 253 Setpoint1 = 55+ 25*sin(k); 254 buttonPushCounter=0; 255 k=k-0.02; 256 } 257 if (nunchuk_cbutton()&&fl==4) //PENTAGRAM TRAJECOTRY 258 { 259 Setpoint =85+ 18*cos(k)+12*cos(k*150);// 260 Setpoint1 =55+ 18*sin(k)-12*sin(k*150);// 261 buttonPushCounter=0; 262 k=k+0.01; 263 } 264} 265 266
Downloadable files
Scheme
Scheme
Comments
Only logged in users can leave comments
davidhamor
0 Followers
•0 Projects
53
0
Ball and Plate | Arduino Project Hub