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