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.095f;                            // 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 = 1024.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 
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.002f);
00040     pwm0.write(0.5f);
00041 
00042     pwm1.period(0.002f);
00043     pwm1.write(0.5f);
00044 
00045     pwm2.period(0.002f);
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     x = 0.0f;
00056     y = 0.0f;
00057 
00058     float previousValueCounter1 = counter1.read();
00059     float previousValueCounter2 = counter2.read();
00060     float previousValueCounter3 = counter3.read();
00061 
00062     speedFilter1.setPeriod(PERIOD);
00063     speedFilter1.setFrequency(LOWPASS_FILTER_FREQUENCY);
00064 
00065     speedFilter2.setPeriod(PERIOD);
00066     speedFilter2.setFrequency(LOWPASS_FILTER_FREQUENCY);
00067 
00068     speedFilter3.setPeriod(PERIOD);
00069     speedFilter3.setFrequency(LOWPASS_FILTER_FREQUENCY);
00070 
00071     previousValueCounter1 = 0.0f;
00072     previousValueCounter2 = 0.0f;
00073     previousValueCounter3 = 0.0f;
00074 
00075     // start thread and timer interrupt
00076 
00077     thread.start(callback(this, &Controller::run));
00078     //ticker.attach(callback(this, &Controller::sendSignal), PERIOD);
00079 }
00080 
00081 /**
00082  * Delete the robot controller object and release all allocated resources.
00083  */
00084 Controller::~Controller()
00085 {
00086 
00087     //ticker.detach();
00088 }
00089 
00090 // set --------------------------------
00091 void Controller::setGammaX(float gammaX)
00092 {
00093 
00094     this->gammaX = gammaX;
00095 }
00096 
00097 
00098 void Controller::setGammaY(float gammaY)
00099 {
00100 
00101     this->gammaY = gammaY;
00102 }
00103 
00104 
00105 void Controller::setGammaZ(float gammaZ)
00106 {
00107 
00108     this->gammaZ = gammaZ;
00109 }
00110 
00111 
00112 void Controller::setPhiX(float phiX)
00113 {
00114 
00115     this->phiX = phiX;
00116 }
00117 
00118 void Controller::setPhiY(float phiY)
00119 {
00120 
00121     this->phiY = phiY;
00122 }
00123 
00124 void Controller::setX(float x)
00125 {
00126 
00127     this->x = x;
00128 }
00129 
00130 /**
00131  * Sets the actual y coordinate of the robots position.
00132  * @param y the y coordinate of the position, given in [m].
00133  */
00134 void Controller::setY(float y)
00135 {
00136 
00137     this->y = y;
00138 }
00139 
00140 // get --------------------------------
00141 
00142 float Controller::getPhiX()
00143 {
00144 
00145     return phiX;
00146 }
00147 
00148 float Controller::getPhiY()
00149 {
00150 
00151     return phiY;
00152 }
00153 
00154 /**
00155  * Gets the actual x coordinate of the robots position.
00156  * @return the x coordinate of the position, given in [m].
00157  */
00158 float Controller::getX()
00159 {
00160 
00161     return x;
00162 }
00163 
00164 /**
00165  * Gets the actual y coordinate of the robots position.
00166  * @return the y coordinate of the position, given in [m].
00167  */
00168 float Controller::getY()
00169 {
00170 
00171     return y;
00172 }
00173 
00174 /**
00175  * This method is called by the ticker timer interrupt service routine.
00176  * It sends a signal to the thread to make it run again.
00177  */
00178 //void Controller::sendSignal() {
00179 
00180 //    thread.signal_set(signal);
00181 //}
00182 
00183 /**
00184  * This <code>run()</code> method contains an infinite loop with the run logic.
00185  */
00186 void Controller::run()
00187 {
00188 
00189     float integratedGammaX = 0.0f;
00190     float integratedGammaY = 0.0f;
00191     float integratedGammaZ = 0.0f;
00192     float integratedPhiX = 0.0f;
00193     float integratedPhiY = 0.0f;
00194 
00195     float previousGammaX = 0.0;
00196     float previousGammaY = 0.0;
00197     float previousGammaZ = 0.0;
00198     float previousPhiX = 0.0;
00199     float previousPhiY = 0.0;
00200 
00201     // K matrix
00202     float K[3][15] = {
00203         {0.1010, 0.0000, 2.4661, 0.0000, 0.0639, 0.0621, 0.0000, 0.3066, 0.0000, 0.0237, 0.0816, 0.0000, 0.0000, 0.0000, 0.0577},
00204         {-0.0505, 0.0875, -1.2330, 2.1357, 0.0639, -0.0311, 0.0538, -0.1533, 0.2655, 0.0237, -0.0408, 0.0707, 0.0000, 0.0000, 0.0577},
00205         {-0.0505, -0.0875, -1.2330, -2.1357, 0.0639, -0.0311, -0.0538, -0.1533, -0.2655, 0.0237, -0.0408, -0.0707, 0.0000, 0.0000, 0.0577}
00206     };
00207 
00208 
00209     while (true) {
00210 
00211         gammaX = imu.getGammaX();
00212         gammaY = imu.getGammaY();
00213         gammaZ = imu.getGammaZ();
00214         d_gammaX = imu.getDGammaX();
00215         d_gammaY = imu.getDGammaY();
00216         d_gammaZ = imu.getDGammaZ();
00217 
00218         // wait for the periodic signal
00219 
00220         // thread.signal_wait(signal);
00221 
00222         // Read encoder data
00223         float w1 = counter1.read()/1024.0f *1/(2*PI); // Angle wheel 1
00224         float w2 = counter2.read()/1024.0f *1/(2*PI); // Angle wheel 2
00225         float w3 = counter3.read()/1024.0f *1/(2*PI); // Angle wheel 3
00226 
00227         // Calculate Ball angle in [rad] using the kinematic model
00228         this->phiX = ((w1 - w2 - w3) - sin(ALPHA)*RB/RW*gammaZ)/(2*cos(ALPHA))+gammaX;
00229         this->phiY = (w2 - w3)/(SQRT_3*cos(ALPHA))+gammaY;
00230 
00231         // Integration states
00232         integratedGammaX = integratedGammaX + (gammaX-previousGammaX)*PERIOD;
00233         integratedGammaY = integratedGammaY + (gammaY-previousGammaY)*PERIOD;
00234         integratedGammaZ = integratedGammaZ + (gammaZ-previousGammaZ)*PERIOD;
00235         integratedPhiX = integratedPhiX + (phiX-previousPhiX)*PERIOD;
00236         integratedPhiY = integratedPhiY + (phiY-previousPhiY)*PERIOD;
00237 
00238         // Calculate Ball Velocities
00239         short valueCounter1 = counter1.read();
00240         short valueCounter2 = counter2.read();
00241         short valueCounter3 = counter3.read();
00242 
00243         short countsInPastPeriod1 = valueCounter1-previousValueCounter1;
00244         short countsInPastPeriod2 = valueCounter2-previousValueCounter2;
00245         short countsInPastPeriod3 = valueCounter3-previousValueCounter3;
00246 
00247         previousValueCounter1 = valueCounter1;
00248         previousValueCounter2 = valueCounter2;
00249         previousValueCounter3 = valueCounter3;
00250 
00251         actualSpeed1 = speedFilter1.filter((float)countsInPastPeriod1/COUNTS_PER_TURN/PERIOD/(2*PI)); // actual speed motor1 in [rad/s]
00252         actualSpeed2 = speedFilter2.filter((float)countsInPastPeriod2/COUNTS_PER_TURN/PERIOD/(2*PI)); // actual speed motor1 in [rad/s]
00253         actualSpeed3 = speedFilter3.filter((float)countsInPastPeriod3/COUNTS_PER_TURN/PERIOD/(2*PI)); // actual speed motor1 in [rad/s]
00254 
00255         float d_phiX = ((actualSpeed1 - actualSpeed2 - actualSpeed3) - sin(ALPHA)*RB/RW*d_gammaZ)/(2*cos(ALPHA))+d_gammaX;
00256         float d_phiY = (actualSpeed2 - actualSpeed3)/(SQRT_3*cos(ALPHA))+d_gammaY;
00257 
00258         // Calculate Torque motors
00259 
00260         //[M1,M2,M3]=K*x
00261 
00262         float M1 = 0.0f;
00263         float M2 = 0.0f;
00264         float M3 = 0.0f;
00265 
00266         float x[15][1] = {
00267             {gammaX},{gammaY},{gammaZ},{phiX},{phiY},{d_gammaX},{d_gammaY},{d_gammaZ},{d_phiX},{d_phiY},{integratedGammaX},{integratedGammaY},{integratedGammaZ},{integratedPhiX},{integratedPhiY}
00268         };
00269 
00270         for(int i=0; i<14; i++) {
00271             M1 = M1 + K[0][i]*x[i][1];
00272             M2 = M2 + K[1][i]*x[i][1];
00273             M3 = M3 + K[2][i]*x[i][1];
00274         };
00275 
00276         // Calculate duty cycles from desired Torque, limit and set duty cycles
00277         float I1 = M1*1000.0f/KI;
00278         float I2 = M2*1000.0f/KI;
00279         float I3 = M3*1000.0f/KI;
00280                 
00281         float dutyCycle1 = 0.5f/6.0f*I1 + 0.5f;
00282         if (dutyCycle1 < MIN_DUTY_CYCLE) dutyCycle1 = MIN_DUTY_CYCLE;
00283         else if (dutyCycle1 > MAX_DUTY_CYCLE) dutyCycle1 = MAX_DUTY_CYCLE;
00284         //pwm0.write(dutyCycle1);
00285         
00286         float dutyCycle2 = 0.5f/6.0f*I2 + 0.5f;
00287         if (dutyCycle2 < MIN_DUTY_CYCLE) dutyCycle2 = MIN_DUTY_CYCLE;
00288         else if (dutyCycle2 > MAX_DUTY_CYCLE) dutyCycle2 = MAX_DUTY_CYCLE;
00289         //pwm1.write(dutyCycle2);
00290         
00291         float dutyCycle3 = 0.5f/6.0f*I3 + 0.5f;
00292         if (dutyCycle3 < MIN_DUTY_CYCLE) dutyCycle3 = MIN_DUTY_CYCLE;
00293         else if (dutyCycle3 > MAX_DUTY_CYCLE) dutyCycle3 = MAX_DUTY_CYCLE;
00294         //pwm2.write(dutyCycle3);
00295                 
00296         // actual robot pose
00297         this->x = phiY*RB;
00298         this->y = phiX*RB;
00299         
00300         // set new gamma's, phi's
00301         previousGammaX = gammaX;
00302         previousGammaY = gammaY;
00303         previousGammaZ = gammaZ;
00304         previousPhiX = phiX;
00305         previousPhiY = phiY;
00306     }
00307 };