Ball and Plate

LQ control of Ball and Plate system controlled by Arduino MEGA 2560.

May 21, 2016

•

78515 views

•

53 respects

ball and plate

pid

home automation

Components and supplies

Arduino Mega 2560

Project description

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

/////////Ball and Plate///////////////////////////////
/*
BALL AND   PLATE PID CONTROL
*/
//////////////////////////////////////////////////////
///Libraries///
#include   <PID_v1.h>
#include <stdint.h>
#include "TouchScreen.h"
#include <SPI.h>
#include   <Wire.h>
#include <wiinunchuk.h>
#include<Servo.h>
// Definitions TOUCH   PINS
#define YP A0 //0
#define XM A1 //1
#define YM 3  //3
#define XP   4 //4
TouchScreen ts = TouchScreen(XP, YP, XM, YM, 300);
int buttonPushCounter   = 1;   // counter for the number of button presses
int lastButtonState = 0;     //   previous state of the button
int flag , flagZ ;


float xVal , yVal   ;
int cCount=0;
int flagC=0;
int flagK=0;
float kk=0;
int fl=0;
double   l =0.00;
unsigned int noTouchCount = 0; //viariable for noTouch
double  k=0;
//   PID values
double Setpoint, Input, Output; //for X
double Setpoint1, Input1,   Output1; //for Y
//
int Modulo;
long lastcas=0;
// servos variables
Servo   servo1; //X axis
Servo servo2; //Y axis

uint16_t homeX = 550;            //   raw data value for center of touchscreen
uint16_t homeY = 550;            //   raw data value for center of touchscreen             

float convertX = 151.0   / 955.0;  // converts raw x values to mm. found through manual calibration
float   convertY = 91.0 / 927.0;   // converts raw y values to mm. found through manual   calibration
/////TIME SAMPLE
int Ts = 50; 
unsigned long Stable=0; 
//PID   const
float Kp = 0.3;                                                     
float   Ki = 0.03;                                                      
float Kd = 0.13;

float   Kp1 = 0.3;                                                       
float Ki1 =   0.08;                                                      
float Kd1 = 0.13;
long   cas=0; 
//INIT PID
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
PID   myPID1(&Input1, &Output1, &Setpoint1,Kp1,Ki1,Kd1, DIRECT);

void setup()
{
   servo1.attach(5);
  servo2.attach(6);
  Output=95;
  Output1=95;
   servo1.write(Output);
  servo2.write(Output1);
  
  //init NUN
  nunchuk_setpowerpins();
   nunchuk_init();
  nunchuk_get_data(); 
 
  //INIT PINS
  pinMode(9,   OUTPUT);
  pinMode(8, OUTPUT);
  digitalWrite(9,LOW); //LED INIT
  digitalWrite(8,LOW);

   Serial.begin(115200);
  
  //INIT OF TOUSCHSCREEN
   TSPoint p = ts.getPoint();
   Input=120;
 Input1=65;
  //INIT SETPOINT
  Setpoint=120;
  Setpoint1=65;
   //// Make plate flat
  servo1.attach(5); 
  servo2.attach(6);
  Output=95;
   Output1=95;
  servo1.write(Output);
  servo2.write(Output1);
  
  //Zapnutie   PID
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(20, 160);
  myPID1.SetMode(AUTOMATIC);
   myPID1.SetOutputLimits(20, 160);
  // TIME SAMPLE
  myPID1.SetSampleTime(Ts);   
  myPID.SetSampleTime(Ts);  
  /////
  delay(100);
  
 
  ///
   }
 
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(5); //connect servos
      servo2.attach(6);   
      setDesiredPosition();  
      noTouchCount = 0;  
      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(9,HIGH);
                  
          }
          else
          {
              digitalWrite(9,LOW);
           }
       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=93;
     servo1.write(Output); 
     servo2.write(Output1);
     }
    if(noTouchCount == 150) //if there is no ball on plate longer
    {
      servo1.detach(); //detach servos
     servo2.detach();     
   
    }
   }
  servo1.write(Output);//control
  servo2.write(Output1);//control 
   Serial.print(Setpoint);   Serial.print(",");  Serial.print(Setpoint1);  Serial.print(",");   Serial.print(Input);Serial.print(","); Serial.println(Input1); 
     
}////END   OF REGULATION LOOP///
  
  servo1.detach();//detach servos
  servo2.detach();
   
 ///KONTROLA STABILITY////
 while(Stable==125)//if is stable
 { //still   measure actual postiion
    setDesiredPosition(); 
    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(5); //again   attach servos
      servo2.attach(6);
      digitalWrite(9,LOW);
      Stable=0;   //change STABLE state
    }
    
  }//end of STABLE LOOP
}//loop end

////////////////////////Functions//////////////////   
///// DESIRED POSITION
void setDesiredPosition()
{
 
  
 nunchuk_get_data();   
 //if zbutton is pressed, zero positions
 
 int c = nunchuk_zbutton();
   if (c != lastButtonState) {
    // if the state has changed, increment the counter
   if (c == HIGH && digitalRead(11)==0 ) {
      // if the current state is HIGH   then the button
      // wend from off to on:
      buttonPushCounter++;   
     }
  }
   lastButtonState =c;
   
   switch (buttonPushCounter)
    {
    case 1:
    Setpoint=120;
    Setpoint1=70;
    fl=1;
    break;
     case 2:
    Setpoint=52;
    Setpoint1=70;
    fl=2;
    break;
     case 3:
    Setpoint=52;
    Setpoint1=40;
    fl=3;
    break;   
    case 4:
    Setpoint=120;
    Setpoint1=40;
    buttonPushCounter=0;
     fl=4;
    break;  
   }  
    if (nunchuk_cbutton()&&fl==1)///LEMNISCATE   TRAJECOTRY
  {
    Setpoint = 85+ (50*cos(k))/(1+sin(k)*sin(k));
    Setpoint1   = 55+ (50*sin(k)*cos(k))/(1+sin(k)*sin(k));
    buttonPushCounter=0;
    k=k+0.008;   
  }
  if (nunchuk_cbutton()&&fl==2)// CIRCLE TRAJECTORY
  {
    Setpoint   = 85+ 25*cos(k);
    Setpoint1 = 55+ 25*sin(k);
    buttonPushCounter=0;
     k=k-0.02; 
  }
  if (nunchuk_cbutton()&&fl==3)/// ELLIPSE TRAJECORY
   {
    Setpoint = 85+ 40*cos(k);
    Setpoint1 = 55+ 25*sin(k);
    buttonPushCounter=0;
     k=k-0.02; 
  }
  if (nunchuk_cbutton()&&fl==4) //PENTAGRAM TRAJECOTRY
   {
    Setpoint =85+  18*cos(k)+12*cos(k*150);//
    Setpoint1 =55+ 18*sin(k)-12*sin(k*150);//
     buttonPushCounter=0;
    k=k+0.01; 
  }
}

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

/////////Ball and Plate///////////////////////////////
/*
BALL AND  PLATE PID CONTROL
*/
//////////////////////////////////////////////////////
///Libraries///
#include  <PID_v1.h>
#include <stdint.h>
#include "TouchScreen.h"
#include <SPI.h>
#include  <Wire.h>
#include <wiinunchuk.h>
#include<Servo.h>
// Definitions TOUCH  PINS
#define YP A0 //0
#define XM A1 //1
#define YM 3  //3
#define XP  4 //4
TouchScreen ts = TouchScreen(XP, YP, XM, YM, 300);
int buttonPushCounter  = 1;   // counter for the number of button presses
int lastButtonState = 0;     //  previous state of the button
int flag , flagZ ;


float xVal , yVal  ;
int cCount=0;
int flagC=0;
int flagK=0;
float kk=0;
int fl=0;
double  l =0.00;
unsigned int noTouchCount = 0; //viariable for noTouch
double  k=0;
//  PID values
double Setpoint, Input, Output; //for X
double Setpoint1, Input1,  Output1; //for Y
//
int Modulo;
long lastcas=0;
// servos variables
Servo  servo1; //X axis
Servo servo2; //Y axis

uint16_t homeX = 550;            //  raw data value for center of touchscreen
uint16_t homeY = 550;            //  raw data value for center of touchscreen             

float convertX = 151.0  / 955.0;  // converts raw x values to mm. found through manual calibration
float  convertY = 91.0 / 927.0;   // converts raw y values to mm. found through manual  calibration
/////TIME SAMPLE
int Ts = 50; 
unsigned long Stable=0; 
//PID  const
float Kp = 0.3;                                                     
float  Ki = 0.03;                                                      
float Kd = 0.13;

float  Kp1 = 0.3;                                                       
float Ki1 =  0.08;                                                      
float Kd1 = 0.13;
long  cas=0; 
//INIT PID
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
PID  myPID1(&Input1, &Output1, &Setpoint1,Kp1,Ki1,Kd1, DIRECT);

void setup()
{
  servo1.attach(5);
  servo2.attach(6);
  Output=95;
  Output1=95;
  servo1.write(Output);
  servo2.write(Output1);
  
  //init NUN
  nunchuk_setpowerpins();
  nunchuk_init();
  nunchuk_get_data(); 
 
  //INIT PINS
  pinMode(9,  OUTPUT);
  pinMode(8, OUTPUT);
  digitalWrite(9,LOW); //LED INIT
  digitalWrite(8,LOW);

  Serial.begin(115200);
  
  //INIT OF TOUSCHSCREEN
   TSPoint p = ts.getPoint();
  Input=120;
 Input1=65;
  //INIT SETPOINT
  Setpoint=120;
  Setpoint1=65;
  //// Make plate flat
  servo1.attach(5); 
  servo2.attach(6);
  Output=95;
  Output1=95;
  servo1.write(Output);
  servo2.write(Output1);
  
  //Zapnutie  PID
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(20, 160);
  myPID1.SetMode(AUTOMATIC);
  myPID1.SetOutputLimits(20, 160);
  // TIME SAMPLE
  myPID1.SetSampleTime(Ts);  
  myPID.SetSampleTime(Ts);  
  /////
  delay(100);
  
 
  ///
  }
 
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(5); //connect servos
      servo2.attach(6);  
      setDesiredPosition();  
      noTouchCount = 0;  
      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(9,HIGH);
                 
          }
          else
          {
              digitalWrite(9,LOW);
          }
       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=93;
     servo1.write(Output); 
     servo2.write(Output1);
    }
    if(noTouchCount == 150) //if there is no ball on plate longer
    {
     servo1.detach(); //detach servos
     servo2.detach();     
   
    }
  }
  servo1.write(Output);//control
  servo2.write(Output1);//control 
  Serial.print(Setpoint);   Serial.print(",");  Serial.print(Setpoint1);  Serial.print(",");  Serial.print(Input);Serial.print(","); Serial.println(Input1); 
     
}////END  OF REGULATION LOOP///
  
  servo1.detach();//detach servos
  servo2.detach();
  
 ///KONTROLA STABILITY////
 while(Stable==125)//if is stable
 { //still  measure actual postiion
    setDesiredPosition(); 
    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(5); //again  attach servos
      servo2.attach(6);
      digitalWrite(9,LOW);
      Stable=0;  //change STABLE state
    }
    
  }//end of STABLE LOOP
}//loop end

////////////////////////Functions//////////////////  
///// DESIRED POSITION
void setDesiredPosition()
{
 
  
 nunchuk_get_data();  
 //if zbutton is pressed, zero positions
 
 int c = nunchuk_zbutton();
  if (c != lastButtonState) {
    // if the state has changed, increment the counter
  if (c == HIGH && digitalRead(11)==0 ) {
      // if the current state is HIGH  then the button
      // wend from off to on:
      buttonPushCounter++;   
    }
  }
   lastButtonState =c;
   
   switch (buttonPushCounter)
   {
    case 1:
    Setpoint=120;
    Setpoint1=70;
    fl=1;
    break;
    case 2:
    Setpoint=52;
    Setpoint1=70;
    fl=2;
    break;
    case 3:
    Setpoint=52;
    Setpoint1=40;
    fl=3;
    break;  
    case 4:
    Setpoint=120;
    Setpoint1=40;
    buttonPushCounter=0;
    fl=4;
    break;  
   }  
    if (nunchuk_cbutton()&&fl==1)///LEMNISCATE  TRAJECOTRY
  {
    Setpoint = 85+ (50*cos(k))/(1+sin(k)*sin(k));
    Setpoint1  = 55+ (50*sin(k)*cos(k))/(1+sin(k)*sin(k));
    buttonPushCounter=0;
    k=k+0.008;  
  }
  if (nunchuk_cbutton()&&fl==2)// CIRCLE TRAJECTORY
  {
    Setpoint  = 85+ 25*cos(k);
    Setpoint1 = 55+ 25*sin(k);
    buttonPushCounter=0;
    k=k-0.02; 
  }
  if (nunchuk_cbutton()&&fl==3)/// ELLIPSE TRAJECORY
  {
    Setpoint = 85+ 40*cos(k);
    Setpoint1 = 55+ 25*sin(k);
    buttonPushCounter=0;
    k=k-0.02; 
  }
  if (nunchuk_cbutton()&&fl==4) //PENTAGRAM TRAJECOTRY
  {
    Setpoint =85+  18*cos(k)+12*cos(k*150);//
    Setpoint1 =55+ 18*sin(k)-12*sin(k*150);//
    buttonPushCounter=0;
    k=k+0.01; 
  }
}

Downloadable files

Scheme

Comments

Only logged in users can leave comments

davidhamor

0 Followers

•

0 Projects

Intro