main.cpp

00001 #include "mbed.h"
00002 #include "HIDScope.h"
00003 #include "MODSERIAL.h"
00004 #include "QEI.h"
00005 
00006 DigitalOut led_g(LED_GREEN);
00007 DigitalOut led_b(LED_BLUE);
00008 DigitalOut led_r(LED_RED);
00009 
00010 DigitalOut M1_Rotate(D2); // voltage only base rotation
00011 PwmOut M1_Speed(D3);      // voltage only base rotation
00012 
00013 MODSERIAL pc(USBTX, USBRX);
00014 
00015 //QEI wheel(PinName channelA, PinName channelB, PinName index, int pulsesPerRev, Encoding encoding=X2_ENCODING)
00016 QEI motor2(D10,D11,NC,8400,QEI::X4_ENCODING);
00017 QEI motor3(D12,D13,NC,8400,QEI::X4_ENCODING);
00018 
00019 //Motor control outputs
00020 DigitalOut M2_Rotate(D4);   // encoder side pot 2 translation
00021 PwmOut M2_Speed(D5);        // encoder side pot 2 translation
00022 DigitalOut M3_Rotate(D7);   // encoder side pot 1 spatel rotation
00023 PwmOut M3_Speed(D6);        // encoder side pot 1 spatel rotation
00024 
00025 //Booleans for simplifying the EMG control
00026 bool left;
00027 bool right;
00028 
00029 //EMG related inputs and outputs
00030 HIDScope    scope( 2 );
00031 AnalogIn    emg0( A0 );
00032 AnalogIn    emg1( A1 );
00033 DigitalIn   buttonCalibrate(SW3);
00034 DigitalIn   buttonCalibrateComplete(SW2);
00035 
00036 //Variables for control
00037 bool turnRight;
00038 bool turnLeft;
00039 bool turn = 0;
00040 float waiter = 0.12;
00041 float translation = 0; //initialise values at 0
00042 float degrees3 = 0;
00043 
00044 float Puls_degree = (8400/360);
00045 float wheel1 = 16;
00046 float wheel2 = 31;
00047 float wheel3 = 41;
00048 float transmission = ((wheel2/wheel1)*(wheel3/wheel1));
00049 float pi = 3.14159265359;
00050 
00051 //Global filter variables
00052 volatile float x;
00053 volatile float x_prev =0;
00054 volatile float b; // filtered 'output' of ReadAnalogInAndFilter
00055 const double a1 = -1.6475;
00056 const double a2 = 0.7009;
00057 const double b0 = 0.8371;
00058 const double b1 = -1.6742;
00059 const double b2 = 0.8371;
00060 const double c1 = -1.9645;
00061 const double c2 = 0.9651;
00062 const double d0 = 0.0001551;
00063 const double d1 = 0.0003103;
00064 const double d2 = 0.0001551;
00065 double v1_HR = 0;
00066 double v2_HR = 0;
00067 double v1_LR = 0;
00068 double v2_LR = 0;
00069 double v1_HL = 0;
00070 double v2_HL = 0;
00071 double v1_LL = 0;
00072 double v2_LL = 0;
00073 double highpassFilterLeft = 0;
00074 double lowpassFilterLeft = 0;
00075 double highpassFilterRight = 0;
00076 double lowpassFilterRight = 0;
00077 
00078 //calibration
00079 bool calibrate = false;
00080 bool calibrate_complete = false;
00081 double threshold_Left = 0;
00082 double threshold_Right= 0;
00083 
00084 
00085 //Tickers
00086 Ticker      sample_timer;
00087 Ticker      printinfo;
00088 Ticker      checkSetpointTranslation;
00089 Ticker      checkSetpointRotation;
00090 
00091 //LED for testing the code
00092 DigitalOut  led(LED1);
00093 
00094 
00095 //setpoints
00096 volatile float setpointRotation;
00097 volatile float setpointTranslation;
00098 const double Setpoint_Translation = -300;
00099 const double Setpoint_Back = 0;
00100 const double Setpoint_Rotation = pi;
00101 double M3_ControlSpeed = 0;
00102 double M2_ControlSpeed = 0;
00103 double SetpointError_Translation = 0;
00104 double SetpointError_Rotation = 0;
00105 double theta_translation;
00106 double theta_rotation;
00107 
00108 //Variables for storing previous setpoints (for calculation average setpoint over 6 previously measured values)
00109 int counter = 0;
00110 double Setpoint1 = 0;
00111 double Setpoint2 = 0;
00112 double Setpoint3 = 0;
00113 double Setpoint4 = 0;
00114 double Setpoint5 = 0;
00115 double SetpointAvg = 0;
00116 
00117 //booleans acting as 'Go-Flags'
00118 bool booltranslate = false;
00119 bool boolrotate = false;
00120 
00121 //Arm PID
00122 const double Ts = 0.001953125; //Ts=1/fs (sample frequency)
00123 const double Translation_Kp = 6.9, Translation_Ki = 0.8, Translation_Kd = 0.4;
00124 double Translation_error = 0;
00125 double Translation_e_prev = 0;
00126 
00127 //Spatel PID
00128 const double Rotation_Kp = 0.23, Rotation_Ki = 0.0429 , Rotation_Kd = 2;
00129 double Rotation_error = 0;
00130 double Rotation_e_prev = 0;
00131 
00132 //Pid calculation (reusable)
00133 double pid_control(double error, const double kp, const double ki, const double kd, double &e_int, double &e_prev)
00134 {
00135     double e_der = (error - e_prev) / Ts;
00136     e_prev = error;
00137     e_int = e_int + (Ts * error);
00138 
00139     return kp*error + ki + e_int + kd + e_der;
00140 }
00141 
00142 //biquad calculation (reusable)
00143 double biquad(double u, double&v1, double&v2, const double a1, const double a2, const double b0,
00144               const double b1, const double b2)
00145 {
00146     double v = u - a1*v1 - a2*v2;
00147     double y = b0*v + b1*v1 + b2*v2;
00148     v2 = v1;
00149     v1 = v;
00150     return y;
00151 }
00152 
00153 //sample function, samples and processes through a highpassfilter, rectifier and lowpassfilter.
00154 void filterSample()
00155 {
00156     highpassFilterLeft = fabs(biquad(emg0.read(), v1_HL, v2_HL, a1, a2, b0, b1, b2));
00157     lowpassFilterLeft = biquad(highpassFilterLeft, v1_LL, v2_LL, c1, c2, d0, d1, d2);
00158     //pc.printf("%f \n \r ", lowpassFilter);
00159     highpassFilterRight = fabs(biquad(emg1.read(), v1_HR, v2_HR, a1, a2, b0, b1, b2));
00160     lowpassFilterRight = biquad(highpassFilterRight, v1_LR, v2_LR, c1, c2, d0, d1, d2);
00161     scope.set(0, lowpassFilterLeft );
00162     scope.set(1, lowpassFilterRight );
00163     scope.send();
00164     //pc.printf("%f \n \r ", lowpassFilter);
00165 }
00166 
00167 //Getting the positions of moving parts by processing the motor angles
00168 float GetPositionM2()
00169 {
00170     float pulses2 = motor2.getPulses();
00171     float degrees2 = (pulses2/Puls_degree);
00172     float radians2 = (degrees2/360)*2*pi;
00173     float translation = ((radians2/transmission)*32.25);
00174 
00175     return translation;
00176 }
00177 float GetRotationM3()
00178 {
00179     float pulses3 = motor3.getPulses();
00180     float degrees3 = (pulses3/Puls_degree);
00181     float radians3 = (degrees3/360)*2*pi;
00182 
00183     return radians3;
00184 }
00185 
00186 // check the error at current sample, and every 50 samples sample for an average (to be used in the controllers)
00187 void CheckErrorRotation()
00188 {
00189     counter++;
00190     if (counter > 50) {
00191         theta_rotation = GetRotationM3();
00192         Setpoint5 = Setpoint4;
00193         Setpoint4 = Setpoint3;
00194         Setpoint3 = Setpoint2;
00195         Setpoint2 = Setpoint1;
00196         Setpoint1 = SetpointError_Rotation;
00197         counter = 0;
00198     }
00199     SetpointError_Rotation =  setpointRotation -theta_rotation;
00200 
00201     SetpointAvg = ((SetpointError_Rotation + Setpoint1 + Setpoint2 + Setpoint3 + Setpoint4 + Setpoint5)/6);
00202 
00203 }
00204 // check the error at current sample (Translation does well enough without averaging
00205 void CheckErrorTranslation()
00206 {
00207     theta_translation = GetPositionM2();
00208     SetpointError_Translation =  setpointTranslation -theta_translation;
00209 }
00210 
00211 //Controller function for the rotation of the spatula
00212 void motorRotation()
00213 {
00214     printf("setpoint = %f \n\r", setpointRotation);
00215     //set direction
00216     if (SetpointError_Rotation > 0) { //if rotation exceeds the setpoint, turn in the other direction
00217         M3_Rotate = 0;
00218     } else {
00219         M3_Rotate = 1;
00220 
00221     }
00222     double speedfactor = 1;
00223     //when on the way back (Setpoint is not pi, but 0) go at a lower speed for higher accuracy
00224     if (setpointRotation != Setpoint_Rotation) {
00225         speedfactor = 0.3; 
00226     }
00227     //the way back has to be more precise. On the way up it doesn't really matter at which point the spatula stops. (as long as it's somewhere high)
00228     double tolerance = 0.1;
00229     if (setpointRotation == Setpoint_Rotation){
00230         tolerance = 1;   
00231     }   
00232     
00233     //control action with 'speedfactor' 
00234     M3_ControlSpeed = speedfactor * Ts * fabs( pid_control(SetpointError_Rotation, Rotation_Kp, Rotation_Ki, Rotation_Kd, Rotation_error, Rotation_e_prev));
00235     if (fabs(SetpointAvg) < 0.1) { //when average error over the last samples is low enough, stop the motor
00236         M3_ControlSpeed = 0;
00237     }
00238     if (theta_rotation > tolerance) //if the angle is within tolerance, give the correct Go-Flag.
00239         boolrotate = true;
00240     if ((fabs(theta_rotation) < tolerance ) && (M3_ControlSpeed == 0))
00241         boolrotate = false;
00242     M3_Speed = M3_ControlSpeed;
00243 }
00244 
00245 //controller for the translation, similar to the rotation but easier because it does not use speedfactors and variable tolerances et cetera
00246 void motorTranslation()
00247 {
00248     theta_translation = GetPositionM2();
00249     SetpointError_Translation =  setpointTranslation - theta_translation;
00250 
00251     //set direction
00252     if (SetpointError_Translation < 0) {
00253         M2_Rotate = 0;
00254     } else {
00255         M2_Rotate = 1;
00256     }
00257     M2_ControlSpeed = Ts * fabs( pid_control(SetpointError_Translation, Translation_Kp, Translation_Ki, Translation_Kd, Translation_error, Translation_e_prev));
00258     if (fabs(SetpointError_Translation) < 8) {
00259         M2_ControlSpeed = 0;
00260 
00261     }
00262     if ((theta_translation < -292) && (M2_ControlSpeed == 0))
00263         booltranslate = true;
00264     if ((theta_translation > -8) && (M2_ControlSpeed == 0))
00265         booltranslate = false;
00266     M2_Speed = M2_ControlSpeed;
00267 
00268 }
00269 //from now on the position of motors can be easily controlled by calling the previous two functions and changing the setpoints.
00270 
00271 
00272 
00273 //go back, so the setpoint will change to Setpoint_Back which is 0
00274 void GoBack()
00275 {
00276     setpointTranslation = Setpoint_Back; //setting the setpoint
00277     motorTranslation(); //executing the action
00278     if (booltranslate == false) {
00279         setpointRotation = Setpoint_Back;
00280         motorRotation();
00281     }
00282     if (boolrotate == false) {
00283         turn = 0;
00284     }
00285     led_r = 1;
00286     led_b = 0;
00287 }
00288 
00289 //Same as GoBack but forward
00290 void Burgerflip()
00291 {
00292     led_r = 0;
00293     led_b = 1;
00294     setpointTranslation = Setpoint_Translation;
00295     motorTranslation();
00296     if (booltranslate == true) { //this is a Go-Flag, for when the translation is complete. When this is the case the spatula can rotate.
00297         setpointRotation = Setpoint_Rotation; //setting the setpoint
00298         motorRotation(); //executing the action
00299     }
00300 }
00301 void BurgerflipActie() //a simple function which calls on the previous two, two perform the 2 actions after each other
00302 {
00303     Burgerflip();
00304     if (boolrotate == true) {
00305         GoBack();
00306     }
00307 }
00308 void print() //a print function which can be attached to a ticker to read out the positions
00309 {
00310     pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
00311 }
00312 
00313 
00314 //Getting the directions. This means reading out the input and setting the correct booleans which are used to control the motors
00315 void GetDirections()
00316 {
00317     
00318     //booleans based on EMG input
00319     if (lowpassFilterRight < threshold_Right)
00320         right = 0;
00321     if (lowpassFilterRight > threshold_Right)
00322         right = 1;
00323     if (lowpassFilterLeft < threshold_Left)
00324         left = 0;
00325     if (lowpassFilterLeft > threshold_Left)
00326         left = 1;
00327     pc.baud(115200);
00328     
00329     //based on the EMG inputs and the boolean 'turn' (which is a go flag for the burger flip action) turnLeft and turnRight are set.
00330     //turnLeft and turnRight control the base motor which rotates the entire robot.
00331     if ((right == 1) && (left == 1) && (turn == 0)) {
00332         turnLeft = 0;
00333         turnRight = 0;
00334         turn = 1;
00335         pc.printf("start action \n \r ");
00336         wait(waiter);
00337 
00338     } else if ((right == 1) && (left == 1) && (turn == 1)) {
00339         turnLeft = 0;
00340         turnRight = 0;
00341         turn = 0;
00342         pc.printf("cancel action \n \r ");
00343         GoBack();
00344         wait(waiter);
00345     } else if ((right == 0) && (left == 0)&& (turn == 0)) {
00346 
00347     } else if ((right == 1) && (turnLeft == 0)&& (turn == 0)) {
00348         /* if the right button is pressed and the motor isn't rotating to the left,
00349         then start rotating to the right etc*/
00350         turnRight = !turnRight;
00351         pc.printf("turn right \n \r ");
00352         wait(waiter);
00353     } else if ((right == 1) && (turnLeft == 1)&& (turn == 0)) {
00354         turnLeft = 0;
00355         turnRight = !turnRight;
00356         pc.printf("turn right after left \n \r ");
00357         wait(waiter);
00358     } else if ((left == 1) && (turnRight == 0)&& (turn == 0)) {
00359         turnLeft = !turnLeft;
00360         pc.printf("turn left \n \r ");
00361         wait(waiter);
00362     } else if ((left == 1) && (turnRight == 1) && (turn == 0)) {
00363         turnRight = 0;
00364         turnLeft = !turnLeft;
00365         pc.printf("turn left after right \n \r ");
00366         wait(waiter);
00367     }
00368     wait(2*waiter);
00369 }
00370 int main()
00371 {
00372     //setting leds
00373     led_g = 1;
00374     led_b = 1;
00375     led_r = 1;
00376 
00377     /**Attach the 'sample' function to the timer 'sample_timer'.
00378     * this ensures that 'sample' is executed every... 0.001953125 seconds = 512 Hz
00379     */
00380     //sample_timer.attach(&sample, 0.001953125);
00381     sample_timer.attach(&filterSample, Ts);
00382     checkSetpointTranslation.attach(&CheckErrorTranslation,Ts);
00383     checkSetpointRotation.attach(&CheckErrorRotation,Ts);
00384 
00385     //printinfo.attach(&print, Ts);
00386     pc.baud(115200);
00387     pc.printf("please push the button to calibrate \n \r");
00388     while (1) {
00389         
00390         //The main function starts with some calibration steps
00391         if (buttonCalibrate == 0) {
00392             calibrate = true;
00393             threshold_Left = lowpassFilterLeft*0.9;  //at the moment of the button press, the current EMG values are stored and multiplied by 0.9 as a threshold value.
00394             threshold_Right = lowpassFilterRight*0.9;
00395             pc.printf("calibration complete, press to continue \n \r");
00396         }
00397         if ((buttonCalibrateComplete == 0) && (calibrate == true)) {
00398             calibrate_complete = true;
00399         }
00400         if (calibrate_complete == true) {
00401             
00402             //After calibrating, the booleans turn, turnRight and turnLeft are evaluated and based on that the motors are controlled.
00403 
00404             pc.printf("rotation is %f, setpoint %f, error = %f en translation = %f en de error %f \n \r", GetRotationM3(), Setpoint_Back, SetpointError_Rotation, GetPositionM2(), SetpointError_Translation);
00405             GetDirections();
00406             if (turnRight == true) {
00407                 M1_Speed = 0.1; //turn to the right
00408                 M1_Rotate = 1;
00409             } else if (turnLeft == true) {
00410                 M1_Speed = 0.1; //turn to the left
00411                 M1_Rotate = 0;
00412             } else if (turn == 1) {
00413                 BurgerflipActie();  //flip the burger
00414             } else if (turn == 0) {
00415                 M2_Speed = 0;   //do not flip the burger
00416                 M3_Speed = 0;
00417             }
00418             if ((turnLeft == false) && (turnRight == false)) {
00419                 M1_Speed = 0;      //do nothing
00420 
00421             }
00422 
00423         }
00424     }
00425 }