BMT K9 groep 9 2015-2016 / Mbed 2 deprecated frdm_PIDControllertest

Goed werkende PID, vanaf nu dit script gebruiken.

Dependencies:   Encoder HIDScope MODSERIAL mbed

main.cpp@5:cd1c63ffdc1a, 2015-10-23 (annotated)

Committer:
roosalyn
Date:
Fri Oct 23 08:24:04 2015 +0000
Revision:
5:cd1c63ffdc1a
Parent:
4:6d88a281192f 
motor2 aansturen;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
roosalyn 0:50d492ea0fd0 1 #include "mbed.h"
roosalyn 0:50d492ea0fd0 2 #include "encoder.h"
roosalyn 0:50d492ea0fd0 3 #include "MODSERIAL.h"
roosalyn 0:50d492ea0fd0 4 #include "math.h"
roosalyn 0:50d492ea0fd0 5 #include "HIDScope.h"
roosalyn 0:50d492ea0fd0 6 #define M_PI 3.14159265358979323846
roosalyn 0:50d492ea0fd0 7
roosalyn 0:50d492ea0fd0 8 DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield)
roosalyn 0:50d492ea0fd0 9 PwmOut motor2speed(D5);
roosalyn 0:50d492ea0fd0 10 DigitalOut motor1direction(D7); //D6 en D7 voor motor 1 (op het motorshield)
roosalyn 0:50d492ea0fd0 11 PwmOut motor1speed(D6);
roosalyn 0:50d492ea0fd0 12 AnalogIn potmeter1(A3); //blauw draaiknopje, nr1
roosalyn 0:50d492ea0fd0 13 AnalogIn potmeter2(A2); //nr 2
roosalyn 0:50d492ea0fd0 14 Encoder encoder2(D13,D12,true);
roosalyn 0:50d492ea0fd0 15 Encoder encoder1(D11,D10,true);
roosalyn 0:50d492ea0fd0 16 MODSERIAL pc(USBTX,USBRX);
roosalyn 0:50d492ea0fd0 17
roosalyn 0:50d492ea0fd0 18 Ticker myControllerTicker1, myControllerTicker2, countStepTickerX, countStepTickerY, buttonStateCheckX, buttonStateCheckY;
roosalyn 0:50d492ea0fd0 19 bool motor1_PID_Controller_go = false;
roosalyn 0:50d492ea0fd0 20 bool countStepX_go = false;
roosalyn 0:50d492ea0fd0 21 bool countStepY_go = false;
roosalyn 0:50d492ea0fd0 22 bool checkButtonStateX_go = false;
roosalyn 0:50d492ea0fd0 23 bool checkButtonStateY_go = false;
roosalyn 0:50d492ea0fd0 24
roosalyn 0:50d492ea0fd0 25 DigitalIn telknopX(SW2);
roosalyn 0:50d492ea0fd0 26 DigitalIn telknopY(SW3);
roosalyn 0:50d492ea0fd0 27 DigitalOut ledX(D2);
roosalyn 0:50d492ea0fd0 28 DigitalOut ledY(D3);
roosalyn 0:50d492ea0fd0 29 HIDScope scope(2);
roosalyn 0:50d492ea0fd0 30
roosalyn 0:50d492ea0fd0 31 double m1_Kp = 0.0005, m1_Ki = 0.0, m1_Kd = 0.0;
roosalyn 0:50d492ea0fd0 32 const double m1_Ts = 0.01;
roosalyn 0:50d492ea0fd0 33 double m1_err_int = 0, m1_prev_err = 0;
roosalyn 0:50d492ea0fd0 34 double m1_u_prev = 0;
roosalyn 0:50d492ea0fd0 35
roosalyn 4:6d88a281192f 36 double m2_Kp = 0.0005, m2_Ki = 0.0, m2_Kd = 0.0;
roosalyn 4:6d88a281192f 37 const double m2_Ts = 0.01;
roosalyn 4:6d88a281192f 38 double m2_err_int = 0, m2_prev_err = 0;
roosalyn 4:6d88a281192f 39 double m2_u_prev = 0;
roosalyn 0:50d492ea0fd0 40
roosalyn 0:50d492ea0fd0 41 /*
roosalyn 0:50d492ea0fd0 42 Gain = 0.002356
roosalyn 0:50d492ea0fd0 43 B = [ 1.00000000000, 2.00000000000, 1.00000000000]
roosalyn 0:50d492ea0fd0 44 A = [ 1.00000000000, -1.85808607200, 0.86750940236]
roosalyn 0:50d492ea0fd0 45 */
roosalyn 0:50d492ea0fd0 46
roosalyn 0:50d492ea0fd0 47 const double m1_f_a1 = -1.94042975320, m1_f_a2 = 0.94215346226, m1_f_b0 = 1.0*0.000431, m1_f_b1 = 2.0*0.000431, m1_f_b2 = 1.0*0.000431;
roosalyn 0:50d492ea0fd0 48 //dit zijn de constanten die bij de filter horen (de a0 t/m b2) a0 is altijd 1
roosalyn 4:6d88a281192f 49 double m1_f_v1 = 0, m1_f_v2 = 0; //de variabelen van de filter
roosalyn 4:6d88a281192f 50
roosalyn 4:6d88a281192f 51 const double m2_f_a1 = -1.94042975320, m2_f_a2 = 0.94215346226, m2_f_b0 = 1.0*0.000431, m2_f_b1 = 2.0*0.000431, m2_f_b2 = 1.0*0.000431;
roosalyn 4:6d88a281192f 52 //dit zijn de constanten die bij de filter horen (de a0 t/m b2) a0 is altijd 1
roosalyn 4:6d88a281192f 53 double m2_f_v1 = 0, m2_f_v2 = 0; //de variabelen van de filter
roosalyn 0:50d492ea0fd0 54
roosalyn 0:50d492ea0fd0 55 int reference1;
roosalyn 0:50d492ea0fd0 56 int reference2;
roosalyn 0:50d492ea0fd0 57 int difference1;
roosalyn 0:50d492ea0fd0 58 int difference2;
roosalyn 0:50d492ea0fd0 59 int buttonStateX = 1;
roosalyn 0:50d492ea0fd0 60 int buttonStateY = 1;
roosalyn 0:50d492ea0fd0 61 int motorDirectionX = 1;
roosalyn 0:50d492ea0fd0 62 int motorDirectionY = 1;
roosalyn 0:50d492ea0fd0 63 int n;
roosalyn 0:50d492ea0fd0 64 int countsX=0;
roosalyn 0:50d492ea0fd0 65 int countsY=0;
roosalyn 0:50d492ea0fd0 66 int countStateX = 1;
roosalyn 0:50d492ea0fd0 67 int countStateY = 1;
roosalyn 0:50d492ea0fd0 68
roosalyn 0:50d492ea0fd0 69 float x = 4.0; // voorbeeld, moet eigenlijk bepaald worden door EMG signaal
roosalyn 0:50d492ea0fd0 70 float y = 4.0; // is het 4de vakje van rechtsonderin de hoek geteld
roosalyn 0:50d492ea0fd0 71 float xdist_from_board = 4.0; //er moet 2 van afgetrokken worden. Als de as 6cm van het bord staat moet er 4 worden ingevuld. Telt namelijk
roosalyn 0:50d492ea0fd0 72 float ydist_from_board = 4.0; //vanaf het midden van een vakje.
roosalyn 0:50d492ea0fd0 73 double constante1;
roosalyn 0:50d492ea0fd0 74 double constante2;
roosalyn 0:50d492ea0fd0 75 double constante3;
roosalyn 0:50d492ea0fd0 76
roosalyn 0:50d492ea0fd0 77 bool buttonReleasedX(DigitalIn& button)
roosalyn 0:50d492ea0fd0 78 {
roosalyn 0:50d492ea0fd0 79 if (button.read() == 1 && buttonStateX == 0) {
roosalyn 0:50d492ea0fd0 80 buttonStateX = 1;
roosalyn 0:50d492ea0fd0 81 return true;
roosalyn 0:50d492ea0fd0 82 }
roosalyn 0:50d492ea0fd0 83 buttonStateX = button.read();
roosalyn 0:50d492ea0fd0 84 return false;
roosalyn 0:50d492ea0fd0 85 }
roosalyn 0:50d492ea0fd0 86
roosalyn 0:50d492ea0fd0 87 bool buttonReleasedY(DigitalIn& button)
roosalyn 0:50d492ea0fd0 88 {
roosalyn 0:50d492ea0fd0 89 if (button.read() == 1 && buttonStateY == 0) {
roosalyn 0:50d492ea0fd0 90 buttonStateY = 1;
roosalyn 0:50d492ea0fd0 91 return true;
roosalyn 0:50d492ea0fd0 92 }
roosalyn 0:50d492ea0fd0 93 buttonStateY = button.read();
roosalyn 0:50d492ea0fd0 94 return false;
roosalyn 0:50d492ea0fd0 95 }
roosalyn 0:50d492ea0fd0 96
roosalyn 0:50d492ea0fd0 97 bool buttonHold(DigitalIn& button)
roosalyn 0:50d492ea0fd0 98 {
roosalyn 0:50d492ea0fd0 99 return (button.read() == 0);
roosalyn 0:50d492ea0fd0 100 }
roosalyn 0:50d492ea0fd0 101
roosalyn 0:50d492ea0fd0 102 float bereken_hoek1(float x, float y)
roosalyn 0:50d492ea0fd0 103 {
roosalyn 0:50d492ea0fd0 104 float langste_zijde = sqrt(pow(x*4,2) + pow(y*4,2)); //pythagoras
roosalyn 0:50d492ea0fd0 105 float alpha = acos((langste_zijde/2)/30);
roosalyn 0:50d492ea0fd0 106 return (atan((4*y)/(4*x)) + alpha)*4200/(2*M_PI); // omrekenen van radialen naar counts = *4200/(2*M_PI), dit moet nog omgerekend worden met de gear ratio van de extra tandwielen.
roosalyn 0:50d492ea0fd0 107 }
roosalyn 0:50d492ea0fd0 108
roosalyn 0:50d492ea0fd0 109 float bereken_hoek2(float x, float y)
roosalyn 0:50d492ea0fd0 110 {
roosalyn 0:50d492ea0fd0 111 float langste_zijde = sqrt(pow(x*4,2) + pow(y*4,2)); //pythagoras
roosalyn 0:50d492ea0fd0 112 float alpha = acos((langste_zijde/2)/30);
roosalyn 0:50d492ea0fd0 113 float beta = 0.5*M_PI - alpha;
roosalyn 0:50d492ea0fd0 114 return (2*beta)*4200/(2*M_PI); // x wordt hoek voor eerste arm t.o.v. x-as (CW), y hoek voor tweede arm t.o.v. eerste arm (CW)
roosalyn 0:50d492ea0fd0 115 }
roosalyn 0:50d492ea0fd0 116
roosalyn 0:50d492ea0fd0 117 //-------------------------- MOTORCONTROLLERS --------------------------//
roosalyn 0:50d492ea0fd0 118
roosalyn 0:50d492ea0fd0 119 void constanteBepalen()
roosalyn 0:50d492ea0fd0 120 {
roosalyn 0:50d492ea0fd0 121 constante1 = potmeter1.read();
roosalyn 0:50d492ea0fd0 122 constante2 = potmeter2.read();
roosalyn 0:50d492ea0fd0 123 m1_Kd = constante2*0.001; //loopt nu van 0 tot 1
roosalyn 0:50d492ea0fd0 124 m1_Ki = constante1*0.001;
roosalyn 0:50d492ea0fd0 125 }
roosalyn 0:50d492ea0fd0 126
roosalyn 0:50d492ea0fd0 127 double biquad(double u, double &v1, double &v2, const double a1, const double a2,
roosalyn 0:50d492ea0fd0 128 const double b0, const double b1, const double b2){
roosalyn 0:50d492ea0fd0 129 double v = u-a1*v1-a2*v2; //formule uit slides van biquad filter
roosalyn 0:50d492ea0fd0 130 double y = b0*v+b1*v1+b2*v2; //zie ook slides
roosalyn 0:50d492ea0fd0 131 v2 = v1; v1 =v;
roosalyn 0:50d492ea0fd0 132 return y;
roosalyn 0:50d492ea0fd0 133 }
roosalyn 0:50d492ea0fd0 134
roosalyn 0:50d492ea0fd0 135 double PID(double e, const double Kp, const double Ki, const double Kd, double Ts,
roosalyn 0:50d492ea0fd0 136 double &e_int, double &e_prev, double &f_v1, double &f_v2, const double f_a1,
roosalyn 0:50d492ea0fd0 137 const double f_a2, const double f_b0, const double f_b1, const double f_b2){ //In de slides staat f_b3, maar volgens mij moet het f_b2 zijn
roosalyn 0:50d492ea0fd0 138 //Bereken eerst afgeleide van de error
roosalyn 0:50d492ea0fd0 139 double e_der = (e-e_prev)/Ts;
roosalyn 0:50d492ea0fd0 140 double e_der2 = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2); //de y die de biquad returnt is de gefilterde e_der
roosalyn 0:50d492ea0fd0 141 e_prev = e;
roosalyn 0:50d492ea0fd0 142 //Bereken nu de geintegreerde versie van de error
roosalyn 0:50d492ea0fd0 143 e_int = e_int + Ts*e;
roosalyn 0:50d492ea0fd0 144 //PID
roosalyn 0:50d492ea0fd0 145
roosalyn 0:50d492ea0fd0 146 return Kp*e+Ki*e_int+Kd*e_der2; //formule uit slides
roosalyn 0:50d492ea0fd0 147 }
roosalyn 0:50d492ea0fd0 148
roosalyn 0:50d492ea0fd0 149 void motor1_PID_Controller()
roosalyn 0:50d492ea0fd0 150 {
roosalyn 0:50d492ea0fd0 151 //reference1 = potmeter1.read()*4200; //draaiknop uitlezen, tussen 0 en 1
roosalyn 0:50d492ea0fd0 152 float hoek1 = bereken_hoek1(countsX,countsY);
roosalyn 0:50d492ea0fd0 153 reference1=hoek1;
roosalyn 0:50d492ea0fd0 154 double position =(encoder1.getPosition()); // waarde tussen 0 en 4200
roosalyn 0:50d492ea0fd0 155
roosalyn 0:50d492ea0fd0 156 scope.set(0,reference1);
roosalyn 0:50d492ea0fd0 157 scope.set(1,position);
roosalyn 0:50d492ea0fd0 158 scope.send();
roosalyn 0:50d492ea0fd0 159
roosalyn 0:50d492ea0fd0 160 difference1 = reference1 - position;
roosalyn 0:50d492ea0fd0 161 constanteBepalen();
roosalyn 0:50d492ea0fd0 162 double u = PID( reference1 - position, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int,m1_prev_err, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2 );
roosalyn 0:50d492ea0fd0 163 if (u < 0) {
roosalyn 0:50d492ea0fd0 164 motor1direction = 1; //directie bepalen. Waardes zijn tegenovergesteld vergeleken met motor 2
roosalyn 0:50d492ea0fd0 165 } else if (u>=0) {
roosalyn 0:50d492ea0fd0 166 motor1direction = 0;
roosalyn 0:50d492ea0fd0 167 }
roosalyn 0:50d492ea0fd0 168 motor1speed = fabs(u);
roosalyn 0:50d492ea0fd0 169 pc.printf("u = %.2f \r\n", u);
roosalyn 4:6d88a281192f 170 }
roosalyn 4:6d88a281192f 171
roosalyn 4:6d88a281192f 172 void motor2_PID_Controller()
roosalyn 4:6d88a281192f 173 {
roosalyn 4:6d88a281192f 174 //reference2 = potmeter2.read()*4200; //draaiknop uitlezen, tussen 0 en 1
roosalyn 4:6d88a281192f 175 float hoek2 = bereken_hoek2(countsX,countsY);
roosalyn 4:6d88a281192f 176 reference2=hoek2;
roosalyn 5:cd1c63ffdc1a 177 if (reference2 <500) {
roosalyn 5:cd1c63ffdc1a 178 reference2=500;
roosalyn 5:cd1c63ffdc1a 179 }
roosalyn 5:cd1c63ffdc1a 180 if (reference2 > 2100) {
roosalyn 5:cd1c63ffdc1a 181 reference2 = 2100;
roosalyn 5:cd1c63ffdc1a 182 }
roosalyn 4:6d88a281192f 183 double position =(encoder2.getPosition()); // waarde tussen 0 en 4200
roosalyn 0:50d492ea0fd0 184
roosalyn 4:6d88a281192f 185 scope.set(2,reference2);
roosalyn 4:6d88a281192f 186 scope.set(3,position);
roosalyn 4:6d88a281192f 187
roosalyn 4:6d88a281192f 188 difference2 = reference2 - position;
roosalyn 4:6d88a281192f 189 constanteBepalen();
roosalyn 4:6d88a281192f 190 double u = PID( reference2 - position, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int,m2_prev_err, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2, m2_f_b0, m2_f_b1, m2_f_b2 );
roosalyn 4:6d88a281192f 191 if (u < 0) {
roosalyn 4:6d88a281192f 192 motor2direction = 1; //directie bepalen. Waardes zijn tegenovergesteld vergeleken met motor 2
roosalyn 4:6d88a281192f 193 } else if (u>=0) {
roosalyn 4:6d88a281192f 194 motor2direction = 0;
roosalyn 4:6d88a281192f 195 }
roosalyn 4:6d88a281192f 196 motor2speed = fabs(u);
roosalyn 4:6d88a281192f 197 pc.printf("u = %.2f \r\n", u);
roosalyn 0:50d492ea0fd0 198 }
roosalyn 0:50d492ea0fd0 199
roosalyn 0:50d492ea0fd0 200 //-------------------------- POSITIEBEPALING --------------------------//
roosalyn 0:50d492ea0fd0 201
roosalyn 0:50d492ea0fd0 202 void checkButtonStateX()
roosalyn 0:50d492ea0fd0 203 {
roosalyn 0:50d492ea0fd0 204 if (buttonReleasedX(telknopX)) {
roosalyn 0:50d492ea0fd0 205 countStateX = -countStateX;
roosalyn 0:50d492ea0fd0 206 }
roosalyn 0:50d492ea0fd0 207 }
roosalyn 0:50d492ea0fd0 208
roosalyn 0:50d492ea0fd0 209 void checkButtonStateY()
roosalyn 0:50d492ea0fd0 210 {
roosalyn 0:50d492ea0fd0 211 if (buttonReleasedY(telknopY)) {
roosalyn 0:50d492ea0fd0 212 countStateY = -countStateY;
roosalyn 0:50d492ea0fd0 213 }
roosalyn 0:50d492ea0fd0 214 }
roosalyn 0:50d492ea0fd0 215
roosalyn 0:50d492ea0fd0 216 void countStepX()
roosalyn 0:50d492ea0fd0 217 {
roosalyn 0:50d492ea0fd0 218 if (buttonHold(telknopX)) {
roosalyn 0:50d492ea0fd0 219 countsX+= countStateX; //hier moet de richting dus veranderen
roosalyn 0:50d492ea0fd0 220 if(countsX >= 10) {
roosalyn 0:50d492ea0fd0 221 countsX = 10;
roosalyn 0:50d492ea0fd0 222 } else if(countsX<=0) {
roosalyn 0:50d492ea0fd0 223 countsX = 0;
roosalyn 0:50d492ea0fd0 224 }
roosalyn 0:50d492ea0fd0 225 }
roosalyn 0:50d492ea0fd0 226 }
roosalyn 0:50d492ea0fd0 227
roosalyn 0:50d492ea0fd0 228 void countStepY()
roosalyn 0:50d492ea0fd0 229 {
roosalyn 0:50d492ea0fd0 230 if (buttonHold(telknopY)) { //X en Y kunnen niet tegelijk optellen, alleen als de ander niet ingedrukt is
roosalyn 0:50d492ea0fd0 231 countsY+= countStateY;
roosalyn 0:50d492ea0fd0 232 if(countsY >= 10) {
roosalyn 0:50d492ea0fd0 233 countsY = 10;
roosalyn 0:50d492ea0fd0 234 } else if(countsY<=0) {
roosalyn 0:50d492ea0fd0 235 countsY = 0;
roosalyn 0:50d492ea0fd0 236 }
roosalyn 0:50d492ea0fd0 237 }
roosalyn 0:50d492ea0fd0 238 }
roosalyn 0:50d492ea0fd0 239
roosalyn 5:cd1c63ffdc1a 240 void motor2_PID_Controller_activate(){
roosalyn 5:cd1c63ffdc1a 241 motor2_PID_Controller_go = true;
roosalyn 0:50d492ea0fd0 242 }
roosalyn 0:50d492ea0fd0 243
roosalyn 0:50d492ea0fd0 244 void countStepX_activate(){
roosalyn 0:50d492ea0fd0 245 countStepX_go = true;
roosalyn 0:50d492ea0fd0 246 }
roosalyn 0:50d492ea0fd0 247
roosalyn 0:50d492ea0fd0 248 void checkButtonStateX_activate(){
roosalyn 0:50d492ea0fd0 249 checkButtonStateX_go = true;
roosalyn 0:50d492ea0fd0 250 }
roosalyn 0:50d492ea0fd0 251
roosalyn 0:50d492ea0fd0 252 void countStepY_activate(){
roosalyn 0:50d492ea0fd0 253 countStepY_go = true;
roosalyn 0:50d492ea0fd0 254 }
roosalyn 0:50d492ea0fd0 255
roosalyn 0:50d492ea0fd0 256 void checkButtonStateY_activate(){
roosalyn 0:50d492ea0fd0 257 checkButtonStateY_go = true;
roosalyn 0:50d492ea0fd0 258 }
roosalyn 0:50d492ea0fd0 259
roosalyn 0:50d492ea0fd0 260 int main()
roosalyn 0:50d492ea0fd0 261 {
roosalyn 0:50d492ea0fd0 262 pc.baud(9600);
roosalyn 0:50d492ea0fd0 263 encoder1.setPosition(0);
roosalyn 0:50d492ea0fd0 264 encoder2.setPosition(0);
roosalyn 0:50d492ea0fd0 265 /*myControllerTicker1.attach( &motor1_PID_Controller, 0.005f ); //Ticker roept elke 0.01 s de motor2 controller aan. = 100Hz
roosalyn 0:50d492ea0fd0 266 myControllerTicker2.attach( &motor2_PI_Controller, 0.01f);
roosalyn 0:50d492ea0fd0 267 countStepTickerX.attach( &countStepX,1.0f );
roosalyn 0:50d492ea0fd0 268 countStepTickerY.attach( &countStepY,1.0f);
roosalyn 0:50d492ea0fd0 269 buttonStateCheckX.attach( &checkButtonStateX,0.001f);
roosalyn 0:50d492ea0fd0 270 buttonStateCheckY.attach( &checkButtonStateY,0.001f);
roosalyn 0:50d492ea0fd0 271 */
roosalyn 5:cd1c63ffdc1a 272 myControllerTicker1.attach( &motor2_PID_Controller_activate, 0.01f); // Function@ hz
roosalyn 0:50d492ea0fd0 273 countStepTickerX.attach( &countStepX_activate, 1.0f); // Function@ hz
roosalyn 0:50d492ea0fd0 274 countStepTickerY.attach( &countStepY_activate, 1.04f); // Function@ hz
roosalyn 0:50d492ea0fd0 275 buttonStateCheckX.attach( &checkButtonStateX_activate, 0.001f); // Function@ hz
roosalyn 0:50d492ea0fd0 276 buttonStateCheckY.attach( &checkButtonStateY_activate, 0.001f); // Function@ hz
roosalyn 0:50d492ea0fd0 277
roosalyn 0:50d492ea0fd0 278
roosalyn 0:50d492ea0fd0 279 while (true) {
roosalyn 0:50d492ea0fd0 280
roosalyn 0:50d492ea0fd0 281 pc.printf(" 1: pos: %d, ref: %d, dif: %d, Ki: %.5f, Kd: %.5f, countsX: %d, countsY: %d \r\n", encoder1.getPosition(), reference1,difference1,m1_Kd,m1_Ki,countsX,countsY);
roosalyn 0:50d492ea0fd0 282 //pc.printf(" 2: pos: %d, ref: %d, dif: %d \r\n", encoder2.getPosition(), reference2,difference2);
roosalyn 0:50d492ea0fd0 283
roosalyn 5:cd1c63ffdc1a 284 if(motor2_PID_Controller_go) {
roosalyn 5:cd1c63ffdc1a 285 motor2_PID_Controller_go = false;
roosalyn 5:cd1c63ffdc1a 286 motor2_PID_Controller();
roosalyn 0:50d492ea0fd0 287 }
roosalyn 0:50d492ea0fd0 288 if(countStepX_go) {
roosalyn 0:50d492ea0fd0 289 countStepX_go = false;
roosalyn 0:50d492ea0fd0 290 countStepX();
roosalyn 0:50d492ea0fd0 291 }
roosalyn 0:50d492ea0fd0 292 if(countStepY_go) {
roosalyn 0:50d492ea0fd0 293 countStepY_go = false;
roosalyn 0:50d492ea0fd0 294 countStepY();
roosalyn 0:50d492ea0fd0 295 }
roosalyn 0:50d492ea0fd0 296 if(checkButtonStateX_go) {
roosalyn 0:50d492ea0fd0 297 checkButtonStateX_go = false;
roosalyn 0:50d492ea0fd0 298 checkButtonStateX();
roosalyn 0:50d492ea0fd0 299 }
roosalyn 0:50d492ea0fd0 300 if(checkButtonStateY_go) {
roosalyn 0:50d492ea0fd0 301 checkButtonStateY_go = false;
roosalyn 0:50d492ea0fd0 302 checkButtonStateY();
roosalyn 0:50d492ea0fd0 303 }
roosalyn 0:50d492ea0fd0 304 }
roosalyn 0:50d492ea0fd0 305 }