Controller.cpp

00001 /*
00002  * Controller.cpp
00003  * Copyright (c) 2018, ZHAW
00004  * All rights reserved.
00005  *
00006  *  Created on: 27.03.2018
00007  *      Author: BaBoRo Development Team
00008  */
00009 
00010 #include <cmath>
00011 #include "Controller.h"
00012 
00013 using namespace std;
00014 
00015 const float Controller::PERIOD = 0.001f;                        // the period of the timer interrupt, given in [s]
00016 const float Controller::ALPHA = 0.785398163397448f;             // alpha = 45° in [rad]
00017 const float Controller::RB = 0.0925f;                            // Ball Radius in [m]
00018 const float Controller::RW = 0.024f;                            // Wheel Radius in [m]
00019 const float Controller::PI = 3.141592653589793f;                // PI
00020 const float Controller::SQRT_3 = 1.732050807568877f;            // Square root of 3
00021 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f;      // frequency of lowpass filter for actual speed values, given in [rad/s]
00022 const float Controller::COUNTS_PER_TURN = 4096.0f;
00023 const float Controller::KI = 58.5f;                             // torque constant [mNm/A]
00024 const float Controller::MIN_DUTY_CYCLE = 0.1f;                  // minimum allowed value for duty cycle (10%)
00025 const float Controller::MAX_DUTY_CYCLE = 0.9f;                  // maximum allowed value for duty cycle (90%)
00026 const float Controller::MAX_ACC_M = 2.0f;
00027 /**
00028  * Create and initialize a robot controller object.
00029  * @param pwm0 a pwm output object to set the duty cycle for the first motor.
00030  * @param pwm1 a pwm output object to set the duty cycle for the second motor.
00031  * @param pwm2 a pwm output object to set the duty cycle for the third motor.
00032  * @param counter1
00033  */
00034 Controller::Controller(PwmOut& pwm0, PwmOut& pwm1, PwmOut& pwm2, EncoderCounter& counter1, EncoderCounter& counter2, EncoderCounter& counter3, IMU& imu) : pwm0(pwm0), pwm1(pwm1), pwm2(pwm2), counter1(counter1), counter2(counter2), counter3(counter3), imu(imu), thread(osPriorityHigh, STACK_SIZE)
00035 {
00036 
00037     // initialize local values
00038 
00039     pwm0.period(0.0005f);// 0.5ms 2KHz
00040     pwm0.write(0.5f);
00041 
00042     pwm1.period(0.0005f);
00043     pwm1.write(0.5f);
00044 
00045     pwm2.period(0.0005f);
00046     pwm2.write(0.5f);
00047 
00048     gammaX = imu.getGammaX();
00049     gammaY = imu.getGammaY();
00050     gammaZ = imu.getGammaZ();
00051 
00052     phiX = 0.0f;
00053     phiY = 0.0f;
00054 
00055     d_phiX = 0.0f;
00056     d_phiY = 0.0f;
00057 
00058     x = 0.0f;
00059     y = 0.0f;
00060 
00061     float previousValueCounter1 = counter1.read();
00062     float previousValueCounter2 = counter2.read();
00063     float previousValueCounter3 = -counter3.read();
00064 
00065     speedFilter1.setPeriod(PERIOD);
00066     speedFilter1.setFrequency(LOWPASS_FILTER_FREQUENCY);
00067 
00068     speedFilter2.setPeriod(PERIOD);
00069     speedFilter2.setFrequency(LOWPASS_FILTER_FREQUENCY);
00070 
00071     speedFilter3.setPeriod(PERIOD);
00072     speedFilter3.setFrequency(LOWPASS_FILTER_FREQUENCY);
00073 
00074     d_phiXFilter.setPeriod(PERIOD);
00075     d_phiXFilter.setFrequency(10.0f);
00076 
00077     d_phiYFilter.setPeriod(PERIOD);
00078     d_phiYFilter.setFrequency(10.0f);
00079 
00080     gammaXFilter.setPeriod(PERIOD);
00081     gammaXFilter.setFrequency(100.0f);
00082     gammaYFilter.setPeriod(PERIOD);
00083     gammaYFilter.setFrequency(100.0f);
00084     d_gammaXFilter.setPeriod(PERIOD);
00085     d_gammaXFilter.setFrequency(100.0f);
00086     d_gammaYFilter.setPeriod(PERIOD);
00087     d_gammaYFilter.setFrequency(100.0f);
00088 
00089     M1Filter.setPeriod(PERIOD);
00090     M1Filter.setFrequency(50.0f);
00091 
00092     M2Filter.setPeriod(PERIOD);
00093     M2Filter.setFrequency(50.0f);
00094 
00095     M3Filter.setPeriod(PERIOD);
00096     M3Filter.setFrequency(50.0f);
00097 
00098     previousValueCounter1 = 0.0f;
00099     previousValueCounter2 = 0.0f;
00100     previousValueCounter3 = 0.0f;
00101 
00102     actualSpeed1 = 0.0f;
00103     actualSpeed2 = 0.0f;
00104     actualSpeed3 = 0.0f;
00105 
00106     gammaXref = 0.0f;
00107     gammaYref = 0.0f;
00108     gammaZref = 0.0f;
00109     phiXref = 0.0f;
00110     phiYref = 0.0f;
00111 
00112     M1motion.setProfileVelocity(0.4f);
00113     M1motion.setProfileAcceleration(MAX_ACC_M);
00114     M1motion.setProfileDeceleration(MAX_ACC_M);
00115 
00116     M2motion.setProfileVelocity(0.4f);
00117     M2motion.setProfileAcceleration(MAX_ACC_M);
00118     M2motion.setProfileDeceleration(MAX_ACC_M);
00119 
00120     M3motion.setProfileVelocity(0.4f);
00121     M3motion.setProfileAcceleration(MAX_ACC_M);
00122     M3motion.setProfileDeceleration(MAX_ACC_M);
00123 
00124     d_phiXMotion.setProfileVelocity(0.5f);
00125     d_phiXMotion.setProfileAcceleration(1.0f);
00126     d_phiXMotion.setProfileDeceleration(2.0f);
00127 
00128     d_phiYMotion.setProfileVelocity(0.5f);
00129     d_phiYMotion.setProfileAcceleration(1.0f);
00130     d_phiYMotion.setProfileDeceleration(2.0f);
00131 
00132     // start thread and timer interrupt
00133 
00134     thread.start(callback(this, &Controller::run));
00135     //ticker.attach(callback(this, &Controller::sendSignal), PERIOD);
00136 }
00137 
00138 /**
00139  * Delete the robot controller object and release all allocated resources.
00140  */
00141 Controller::~Controller()
00142 {
00143 
00144     //ticker.detach();
00145 }
00146 
00147 // set --------------------------------
00148 void Controller::setGammaX(float gammaX)
00149 {
00150 
00151     this->gammaX = gammaX;
00152 }
00153 
00154 
00155 void Controller::setGammaY(float gammaY)
00156 {
00157 
00158     this->gammaY = gammaY;
00159 }
00160 
00161 
00162 void Controller::setGammaZ(float gammaZ)
00163 {
00164 
00165     this->gammaZ = gammaZ;
00166 }
00167 
00168 
00169 void Controller::setPhiX(float phiX)
00170 {
00171 
00172     this->phiX = phiX;
00173 }
00174 
00175 void Controller::setPhiY(float phiY)
00176 {
00177 
00178     this->phiY = phiY;
00179 }
00180 
00181 void Controller::setX(float x)
00182 {
00183 
00184     this->x = x;
00185 }
00186 
00187 /**
00188  * Sets the actual y coordinate of the robots position.
00189  * @param y the y coordinate of the position, given in [m].
00190  */
00191 void Controller::setY(float y)
00192 {
00193 
00194     this->y = y;
00195 }
00196 
00197 // get --------------------------------
00198 
00199 float Controller::getPhiX()
00200 {
00201 
00202     return phiX;
00203 }
00204 
00205 float Controller::getPhiY()
00206 {
00207 
00208     return phiY;
00209 }
00210 
00211 /**
00212  * Gets the actual x coordinate of the robots position.
00213  * @return the x coordinate of the position, given in [m].
00214  */
00215 float Controller::getX()
00216 {
00217 
00218     return x;
00219 }
00220 
00221 /**
00222  * Gets the actual y coordinate of the robots position.
00223  * @return the y coordinate of the position, given in [m].
00224  */
00225 float Controller::getY()
00226 {
00227 
00228     return y;
00229 }
00230 
00231 /**
00232  * This method is called by the ticker timer interrupt service routine.
00233  * It sends a signal to the thread to make it run again.
00234  */
00235 //void Controller::sendSignal() {
00236 
00237 //    thread.signal_set(signal);
00238 //}
00239 
00240 /**
00241  * This <code>run()</code> method contains an infinite loop with the run logic.
00242  */
00243 void Controller::run()
00244 {
00245     Serial pc1(USBTX, USBRX); // tx, rx
00246     pc1.baud(100000);
00247 
00248     float integratedGammaX = 0.0f;
00249     float integratedGammaY = 0.0f;
00250     float integratedGammaZ = 0.0f;
00251     float integratedPhiX = 0.0f;
00252     float integratedPhiY = 0.0f;
00253 
00254     float previousGammaX = 0.0;
00255     float previousGammaY = 0.0;
00256     float previousGammaZ = 0.0;
00257     float previousPhiX = 0.0;
00258     float previousPhiY = 0.0;
00259 
00260     // K matrix
00261     /*
00262     float K[3][10] = {
00263         {0.002327,0.000000,2.166704,0.000000,0.055133,0.032581,0.000000,0.517927,0.000000,0.031336},
00264         {-0.001164,0.002015,-1.083352,1.876421,0.055133,-0.016291,0.028216,-0.258963,0.448538,0.031336},
00265         {-0.001164,-0.002015,-1.083352,-1.876421,0.055133,-0.016291,-0.028216,-0.258963,-0.448538,0.031336}
00266     };
00267     */
00268 
00269     float K[3][10] = {
00270         {0.002327,0.000000,2.362528,-0.000000,0.055133,0.009269,0.000000,0.499429,0.000000,0.031336},
00271         {-0.001164,0.002015,-1.181264,2.046009,0.055133,-0.004635,0.008027,-0.249714,0.432518,0.031336},
00272         {-0.001164,-0.002015,-1.181264,-2.046009,0.055133,-0.004635,-0.008027,-0.249714,-0.432518,0.031336}
00273     };
00274 
00275 
00276     float rad1 = 0.0f;
00277     float rad2 = 0.0f;
00278     float rad3 = 0.0f;
00279 
00280     float rad1contr = 0.0f;
00281 
00282     long int t = 0;
00283 
00284     // messung
00285     int * T = new int[2000];
00286     //float * PX = new float[5000];
00287     //float * PY = new float[5000];
00288     float * GX = new float[2000];
00289     float * GY = new float[2000];
00290     float * GZ = new float[2000];
00291     //float * dPX = new float[5000];
00292     //float * dPY = new float[5000];
00293     float * dGX = new float[2000];
00294     float * dGY = new float[2000];
00295     float * dGZ = new float[2000];
00296     float * W1 = new float[2000];
00297     float * W2 = new float[2000];
00298     float * W3 = new float[2000];
00299 
00300     int a = 0;
00301 
00302     AnalogIn M3_AOUT1(PA_7);
00303     
00304     pwm0.write(0.6);
00305     pwm1.write(0.6);
00306     pwm2.write(0.6);
00307 
00308     while (true) {
00309         
00310         /*
00311         if(t==21000) {
00312             pc1.printf("invio dati:\r\n\n");
00313             for(int j=0; j<2000; j++) {
00314                 //pc1.printf("%d %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f\r\n",*(T+j),*(PX+j),*(PY+j),*(GX+j),*(GY+j),*(GZ+j),*(dGX+j),*(dGY+j),*(dGZ+j),*(dPX+j),*(dPY+j),*(W1+j),*(W2+j),*(W3+j));
00315                 pc1.printf("%d %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f\r\n",*(T+j),*(GX+j),*(GY+j),*(GZ+j),*(dGX+j),*(dGY+j),*(dGZ+j),*(W1+j),*(W2+j),*(W3+j));
00316             }
00317             pc1.printf("fine dati:\r\n\n");
00318             delete T;
00319             //delete PX;
00320             //delete PY;
00321             delete GX;
00322             delete GY;
00323             delete GZ;
00324             //delete dPX;
00325             //delete dPY;
00326             delete dGX;
00327             delete dGY;
00328             delete dGZ;
00329             delete W1;
00330             delete W2;
00331             delete W3;
00332 
00333         }
00334         */
00335 
00336 
00337 
00338 
00339         mutex.lock();
00340         //printf("Controller start\r\n");
00341 
00342         gammaX = imu.getGammaX();
00343         gammaY = imu.getGammaY();
00344         gammaZ = imu.getGammaZ();
00345         d_gammaX = imu.getDGammaX();
00346         d_gammaY = imu.getDGammaY();
00347         d_gammaZ = imu.getDGammaZ();
00348         
00349         
00350 
00351         // wait for the periodic signal
00352 
00353         // thread.signal_wait(signal);
00354 
00355         //printf("%d %d %d\r\n",counter1.read(),counter2.read(),counter3.read());
00356         //pwm0.write(0.6f);
00357         //pwm1.write(0.6f);
00358         //pwm2.write(0.6f);
00359 
00360 
00361 
00362 
00363         // Calculate Ball Velocities
00364         short valueCounter1 = counter1.read();
00365         short valueCounter2 = counter2.read();
00366         short valueCounter3 = -counter3.read();
00367 
00368         short countsInPastPeriod1 = valueCounter1-previousValueCounter1;
00369         short countsInPastPeriod2 = valueCounter2-previousValueCounter2;
00370         short countsInPastPeriod3 = valueCounter3-previousValueCounter3;
00371 
00372         previousValueCounter1 = valueCounter1;
00373         previousValueCounter2 = valueCounter2;
00374         previousValueCounter3 = valueCounter3;
00375 
00376         actualSpeed1 = speedFilter1.filter((2*PI)*(float)countsInPastPeriod1/COUNTS_PER_TURN/PERIOD); // actual speed motor1 in [rad/s]
00377         actualSpeed2 = speedFilter2.filter((2*PI)*(float)countsInPastPeriod2/COUNTS_PER_TURN/PERIOD); // actual speed motor1 in [rad/s]
00378         actualSpeed3 = speedFilter3.filter((2*PI)*(float)countsInPastPeriod3/COUNTS_PER_TURN/PERIOD); // actual speed motor1 in [rad/s]
00379 
00380         rad1 += actualSpeed1*PERIOD;
00381         rad2 += actualSpeed2*PERIOD;
00382         rad3 += actualSpeed3*PERIOD;
00383 
00384         //printf("rad1 = %.7f rad2 = %.7f rad3 = %.7f\r\n",rad1,rad2,rad3);
00385 
00386         //printf("%.7f %.7f %.7f %.7f %.7f %.7f\r\n",(2*PI)*(float)countsInPastPeriod1/COUNTS_PER_TURN/PERIOD,actualSpeed1,(2*PI)*(float)countsInPastPeriod2/COUNTS_PER_TURN/PERIOD,actualSpeed2,(2*PI)*(float)countsInPastPeriod3/COUNTS_PER_TURN/PERIOD,actualSpeed3);
00387 
00388 
00389         //printf("%.7f %.7f %.7f\r\n",actualSpeed1/2*PI,actualSpeed2/2*PI,actualSpeed3/2*PI);
00390 
00391         float actualDPhiX = d_phiXFilter.filter(RW/RB*((actualSpeed1 - actualSpeed2 - actualSpeed3) - sin(ALPHA)*RB/RW*0.0f)/(2*cos(ALPHA))+d_gammaX);
00392         float actualDPhiY = d_phiYFilter.filter((RW/RB*(actualSpeed2 - actualSpeed3))/(SQRT_3*cos(ALPHA))+d_gammaY); //float actualDPhiY = d_phiYFilter.filter((RW/RB*(actualSpeed2 - actualSpeed3 - actualSpeed1)+cos(ALPHA)*(actualDPhiX-gammaX)+sin(ALPHA)*0.0f)/(SQRT_3*cos(ALPHA))+d_gammaY);
00393 
00394         //printf("%.7f %.7f %.7f\r\n",d_gammaX,d_gammaY,d_gammaZ);
00395 
00396         //this->d_phiX = RB/RW*(((actualSpeed1 - actualSpeed2 - actualSpeed3) - sin(ALPHA)*RB/RW*d_gammaZ)/(2*cos(ALPHA))+d_gammaX);
00397         //this->d_phiY = RB/RW*((actualSpeed2 - actualSpeed3)/(SQRT_3*cos(ALPHA))+d_gammaY);
00398 
00399         this->d_phiX = actualDPhiX;
00400         this->d_phiY = actualDPhiY;
00401 
00402         this->phiX = phiX + actualDPhiX*PERIOD;
00403         this->phiY = phiY + actualDPhiY*PERIOD;
00404 
00405 
00406 
00407 
00408         //printf("phiX = %.5f phiY = %.5f\r\n",phiX/PI*180.0f,phiY/PI*180.0f);
00409         //printf("%.5f %.5f\r\n",phiX,phiY);
00410         //printf("%.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f\r\n", phiX,phiY,gammaX,gammaY,gammaZ,d_phiX,d_phiY,d_gammaX,d_gammaY,d_gammaZ,actualSpeed1,actualSpeed2,actualSpeed3);
00411         //printf("%.7f %.7f %.7f %.7f %.7f\r\n", phiX,phiY,gammaX,gammaY,gammaZ);
00412 
00413         // calculate actual error
00414         float errorGammaX = -gammaXref + gammaX;
00415         float errorGammaY = -gammaYref + gammaY;
00416         float errorGammaZ = -gammaZref + gammaZ;
00417         float errorPhiX = -phiXref + phiX;
00418         float errorPhiY = -phiYref + phiY;
00419 
00420         // Integration states
00421         integratedGammaX = integratedGammaX + (errorGammaX-previousGammaX)*PERIOD;
00422         integratedGammaY = integratedGammaY + (errorGammaY-previousGammaY)*PERIOD;
00423         integratedGammaZ = integratedGammaZ + (errorGammaZ-previousGammaZ)*PERIOD;
00424         integratedPhiX = integratedPhiX + (errorPhiX-previousPhiX)*PERIOD;
00425         integratedPhiY = integratedPhiY + (errorPhiY-previousPhiY)*PERIOD;
00426 
00427         // set new gamma's, phi's
00428         previousGammaX = errorGammaX;
00429         previousGammaY = errorGammaY;
00430         previousGammaZ = errorGammaZ;
00431         previousPhiX = errorPhiX;
00432         previousPhiY = errorPhiY;
00433 
00434         // Calculate Torque motors
00435 
00436         //[M1,M2,M3]=K*x
00437 
00438         float M1 = 0.0f;
00439         float M2 = 0.0f;
00440         float M3 = 0.0f;
00441 
00442         d_phiXMotion.incrementToVelocity(d_phiX,PERIOD);
00443         d_phiYMotion.incrementToVelocity(d_phiY,PERIOD);
00444 
00445         float x[10][1] = {
00446             {0.0f},{0.0f},{(gammaX)},{(gammaY)},{0.0f},{0.0f},{0.0f},{d_gammaX},{d_gammaY},{0.0f}
00447         };
00448 
00449         /*
00450         if(gammaX<0.02f && gammaX>-0.02f) {
00451             x[2][0]=0.0f;
00452         }
00453         if(gammaY<0.02f && gammaY>-0.02f) {
00454             x[3][0]=0.0f;
00455         }
00456         if(d_gammaX<0.02f && gammaX>-0.02f) {
00457             x[7][0]=0.0f;
00458         }
00459         if(d_gammaY<0.02f && gammaY>-0.02f) {
00460             x[8][0]=0.0f;
00461         }
00462         */
00463 
00464         for(int i=0; i<10; i++) {
00465             M1 = M1 + K[0][i]*x[i][0];
00466             M2 = M2 + K[1][i]*x[i][0];
00467             M3 = M3 + K[2][i]*x[i][0];
00468         };
00469         
00470         printf("%.7f %.7f %.7f %.7f %.7f %.7f %.7f\r\n",gammaX,gammaY,d_gammaX,d_gammaY);
00471         
00472         M1motion.incrementToPosition(M1, PERIOD);
00473         M2motion.incrementToPosition(M2, PERIOD);
00474         M3motion.incrementToPosition(M3, PERIOD);
00475 
00476         if(t<20001) {
00477             if (t % 10 == 0) {
00478                 *(T+a) = t;
00479                 //*(PX+a) = phiX;
00480                 //*(PY+a) = phiY;
00481                 *(GX+a) = gammaXFilter.filter(gammaX);
00482                 *(GY+a) = gammaYFilter.filter(gammaY);
00483                 *(GZ+a) = gammaZ;
00484                 //*(dPX+a) = d_phiX;
00485                 //*(dPY+a) = d_phiY;
00486                 *(dGX+a) = d_gammaXFilter.filter(d_gammaX);
00487                 *(dGY+a) = d_gammaYFilter.filter(d_gammaY);
00488                 *(dGZ+a) = d_gammaZ;
00489                 *(W1+a) = M1;
00490                 *(W2+a) = M2;
00491                 *(W3+a) = M3;
00492                 a++;
00493             }
00494         }
00495 
00496         //pc1.printf("%.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f\r\n", phiX,phiY,gammaX,gammaY,gammaZ,d_phiX,d_phiY,d_gammaX,d_gammaY,d_gammaZ,M1,M2,M3);
00497         //if(t<2000) {
00498         //messung[0][t]=t/100.0f;
00499         //messung[1][t]=gammaX;
00500         //messung[2][t]=gammaY;
00501         //pc1.printf("%.2f\r\n", t/100.0f);
00502         //}
00503 
00504         //pc1.printf("%.2f %.2f %.2f\r\n", t/100.0f,gammaX,gammaY);
00505         //if(t>2100 && t<4000) {
00506         //pc1.printf("%.2f %.7f %.7f\r\n",1.0,1.0,1.0);//messung[0][t-2100], messung[1][t-2100], messung[2][t-2100]
00507         //}
00508 
00509         //printf("Mi %.7f %.7f %.7f motion Mi %.7f %.7f %.7f\r\n", M1,M2,M3,M1motion.velocity,M2motion.velocity,M3motion.velocity);
00510         //printf("%.7f %.7f\r\n", gammaX,gammaY);
00511 
00512         //printf("x:\r\n %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f\r\n M:\r\n %.7f %.7f %.7f\r\n", phiX,phiY,gammaX,gammaY,gammaZ,d_phiX,d_phiY,d_gammaX,d_gammaY,d_gammaZ,integratedPhiX,integratedPhiY,integratedGammaX,integratedGammaY,integratedGammaZ,M1,M2,M3);
00513 
00514         // Calculate duty cycles from desired Torque, limit and set duty cycles
00515         //float I1 = -M1*1000.0f/KI;
00516         //float I2 = -M2*1000.0f/KI;
00517         //float I3 = -M3*1000.0f/KI;
00518         float I1 = -M1motion.velocity*1000.0f/KI; //cos(0.1f*(float)t)
00519         float I2 = -M2motion.velocity*1000.0f/KI;
00520         float I3 = -M3motion.velocity*1000.0f/KI;
00521 
00522         //printf("%.7f %.7f\r\n",M3_AOUT1.read()* 3.3f *4.0f - 6.0f,I3);
00523         //printf("%.7f %.7f %.7f\r\n",I1,I2,I3);
00524         //printf("%.7f %.7f %.7f\r\n",phiX/PI*180.0f,phiY/PI*180.0f,phiY/PI*180.0f);
00525 
00526         //pc1.printf("%.7f %.7f\r\n", M1,M1motion.velocity);
00527 
00528         float dutyCycle1 = 0.4f/18.0f*I1 + 0.5f;
00529         if (dutyCycle1 < MIN_DUTY_CYCLE) {
00530             dutyCycle1 = MIN_DUTY_CYCLE;
00531         } else if (dutyCycle1 > MAX_DUTY_CYCLE) {
00532             dutyCycle1 = MAX_DUTY_CYCLE;
00533         }
00534         //pwm0.write(dutyCycle1);
00535 
00536         float dutyCycle2 = 0.4f/18.0f*I2 + 0.5f;
00537         if (dutyCycle2 < MIN_DUTY_CYCLE) dutyCycle2 = MIN_DUTY_CYCLE;
00538         else if (dutyCycle2 > MAX_DUTY_CYCLE) dutyCycle2 = MAX_DUTY_CYCLE;
00539         //pwm1.write(dutyCycle2);
00540 
00541         float dutyCycle3 = 0.4f/18.0f*I3 + 0.5f;
00542         if (dutyCycle3 < MIN_DUTY_CYCLE) dutyCycle3 = MIN_DUTY_CYCLE;
00543         else if (dutyCycle3 > MAX_DUTY_CYCLE) dutyCycle3 = MAX_DUTY_CYCLE;
00544         //pwm2.write(dutyCycle3);
00545 
00546         //printf("pwm1 = %.5f pwm2 = %.5f pwm3 = %.5f\r\n",dutyCycle1,dutyCycle2,dutyCycle3);
00547 
00548         // actual robot pose
00549         this->x = phiY*RB;
00550         this->y = phiX*RB;
00551 
00552 
00553         t++;
00554 
00555 
00556         //printf("Controller end\r\n");
00557 
00558         mutex.unlock();
00559 
00560         thread.wait(1.0);
00561     }
00562 };