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 DigitalOut M2_Rotate(D4);   // encoder side pot 2 translation
00020 PwmOut M2_Speed(D5);        // encoder side pot 2 translation
00021 
00022 DigitalOut M3_Rotate(D7);   // encoder side pot 1 spatel rotation
00023 PwmOut M3_Speed(D6);        // encoder side pot 1 spatel rotation
00024 
00025 DigitalIn links(SW3);
00026 DigitalIn rechts(SW2);
00027 
00028 AnalogIn pot1(A4); // pot 1 motor 1
00029 AnalogIn pot2(A3); // pot 2 motor 3
00030 
00031 //Define objects
00032 AnalogIn    emg0( A0 );
00033 AnalogIn    emg1( A1 );
00034 DigitalIn   buttonCalibrate(D9);
00035 
00036 bool draairechts;
00037 bool draailinks;
00038 bool turn = 0;
00039 float waiter = 0.1;
00040 float translation = 0;
00041 float degrees3 = 0;
00042 
00043 float Puls_degree = (8400/360);
00044 float wheel1 = 16;
00045 float wheel2 = 31;
00046 float wheel3 = 41;
00047 float overbrenging = ((wheel2/wheel1)*(wheel3/wheel1));
00048 float pi = 3.14159265359;
00049 
00050 volatile float x;
00051 volatile float x_prev =0;
00052 volatile float b; // filtered 'output' of ReadAnalogInAndFilter
00053 
00054 bool calibrate = false;
00055 double threshold_Left = 0;
00056 double threshold_Right= 0;
00057 Ticker      sample_timer;
00058 Ticker      sample_timer2;
00059 Ticker      printinfo;
00060 Ticker      checkSetpointTranslation;
00061 Ticker      checkSetpointRotation;
00062 HIDScope    scope( 2 );
00063 DigitalOut  led(LED1);
00064 const double a1 = -1.6475;
00065 const double a2 = 0.7009;
00066 const double b0 = 0.8371;
00067 const double b1 = -1.6742;
00068 const double b2 = 0.8371;
00069 const double c1 = -1.9645;
00070 const double c2 = 0.9651;
00071 const double d0 = 0.0001551;
00072 const double d1 = 0.0003103;
00073 const double d2 = 0.0001551;
00074 double v1_high = 0;
00075 double v2_high = 0;
00076 double v1_low = 0;
00077 double v2_low = 0;
00078 double highpassFilterLeft = 0;
00079 double lowpassFilterLeft = 0;
00080 double highpassFilterRight = 0;
00081 double lowpassFilterRight = 0;
00082 
00083 //setpoints
00084 volatile float setpointRotation;
00085 volatile float setpointTranslation;
00086 const double Setpoint_Translation = -200;
00087 const double Setpoint_Back = 0;
00088 const double Setpoint_Rotation = pi;
00089 double M3_ControlSpeed = 0;
00090 double M2_ControlSpeed = 0;
00091 double SetpointError_Translation = 0;
00092 double SetpointError_Rotation = 0;
00093 double theta_translation;
00094 double theta_rotation;
00095 //booleans for control
00096 bool booltranslate = false;
00097 bool boolrotate = false;
00098 //copied from slides
00099 //Arm PID
00100 const double Ts = 0.001953125; //Ts=1/fs (sample frequency)
00101 const double Translation_Kp = 6.9, Translation_Ki = 0.8, Translation_Kd = 0.4;
00102 double Translation_error = 0;
00103 double Translation_e_prev = 0;
00104 
00105 //Spatel PID
00106 const double Rotation_Kp = 0.0499, Rotation_Ki = 0.0429 , Rotation_Kd = 0.0429;
00107 double Rotation_error = 0;
00108 double Rotation_e_prev = 0;
00109 
00110 double pid_control(double error, const double kp, const double ki, const double kd, double &e_int, double &e_prev)
00111 {
00112     double e_der = (error - e_prev) / Ts;
00113     e_prev = error;
00114     e_int = e_int + (Ts * error);
00115 
00116     return kp*error + ki + e_int + kd + e_der;
00117 }
00118 
00119 double biquad1(double u, double&v1, double&v2, const double a1, const double a2, const double b0,
00120                const double b1, const double b2)
00121 {
00122     double v = u - a1*v1 - a2*v2;
00123     double y = b0*v + b1*v1 + b2*v2;
00124     v2 = v1;
00125     v1 = v;
00126     return y;
00127 }
00128 
00129 /** Sample function
00130  * this function samples the emg and sends it to HIDScope
00131  **/
00132 
00133 void filterSampleLeft()
00134 {
00135     highpassFilterLeft = fabs(biquad1(emg0.read(), v1_high, v2_high, a1, a2, b0, b1, b2));
00136     lowpassFilterLeft = biquad1(highpassFilterLeft, v1_low, v2_low, c1, c2, d0, d1, d2);
00137     scope.set(0, lowpassFilterLeft );
00138     scope.send();
00139     //pc.printf("%f \n \r ", lowpassFilter);
00140 }
00141 void filterSampleRight()
00142 {
00143     highpassFilterRight = fabs(biquad1(emg1.read(), v1_high, v2_high, a1, a2, b0, b1, b2));
00144     lowpassFilterRight = biquad1(highpassFilterRight, v1_low, v2_low, c1, c2, d0, d1, d2);
00145     scope.set(1, lowpassFilterRight );
00146     scope.send();
00147     //pc.printf("%f \n \r ", lowpassFilter);
00148 }
00149 
00150 void sample()
00151 {
00152     // Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope'
00153     scope.set(0, emg0.read() );
00154     scope.set(1, emg1.read() );
00155     /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
00156     *  Ensure that enough channels are available (HIDScope scope( 2 ))
00157     *  Finally, send all channels to the PC at once */
00158 
00159     x = emg0;   // Capture data        scope.set(0, x);   // store data in first element of scope memory
00160     b = (x_prev + x)/2.0;   // averaging filter
00161     x_prev = x; // Prepare for next round
00162 
00163     scope.send();
00164     // To indicate that the function is working, the LED is toggled
00165     led = !led;
00166     pc.printf("%f, %f \n \r ", x, b);
00167 }
00168 
00169 void GetDirections()
00170 {
00171     pc.baud(115200);
00172     if ((rechts == 0) && (links == 0) && (turn == 0)) {
00173         draailinks = 0;
00174         draairechts = 0;
00175         turn = 1;
00176         pc.printf("begin de actie \n \r ");
00177         wait(waiter);
00178 
00179     } else if ((rechts == 0) && (links == 0) && (turn == 1)) {
00180         draailinks = 0;
00181         draairechts = 0;
00182         turn = 0;
00183         pc.printf("breek de actie af \n \r ");
00184         wait(waiter);
00185     } else if ((rechts == 1) && (links == 1)&& (turn == 0)) {
00186 
00187     } else if ((rechts == 1) && (draailinks == 0)&& (turn == 0)) {
00188         /* if the right button is pressed and the motor isn't rotating to the left,
00189         then start rotating to the right etc*/
00190         draairechts = !draairechts;
00191         pc.printf("draai naar rechts \n \r ");
00192         wait(waiter);
00193     } else if ((rechts == 1) && (draailinks == 1)&& (turn == 0)) {
00194         draailinks = 0;
00195         draairechts = !draairechts;
00196         pc.printf("draai naar rechts na links \n \r ");
00197         wait(waiter);
00198     } else if ((links == 1) && (draairechts == 0)&& (turn == 0)) {
00199         draailinks = !draailinks;
00200         pc.printf("draai naar links \n \r ");
00201         wait(waiter);
00202     } else if ((links == 1) && (draairechts == 1) && (turn == 0)) {
00203         draairechts = 0;
00204         draailinks = !draailinks;
00205         pc.printf("draai naar links na rechts \n \r ");
00206         wait(waiter);
00207     }
00208     wait(2*waiter);
00209 }
00210 
00211 float GetPositionM2()
00212 {
00213     float pulses2 = motor2.getPulses();
00214     float degrees2 = (pulses2/Puls_degree);
00215     float radians2 = (degrees2/360)*2*pi;
00216     float translation = ((radians2/overbrenging)*32.25);
00217 
00218     return translation;
00219 }
00220 float GetRotationM3()
00221 {
00222     float pulses3 = motor3.getPulses();
00223     float degrees3 = (pulses3/Puls_degree);
00224     float radians3 = (degrees3/360)*2*pi;
00225 
00226     return radians3;
00227 }
00228 void CheckErrorRotation(){
00229     theta_rotation = GetRotationM3();
00230     SetpointError_Rotation =  setpointRotation -theta_rotation;
00231 }
00232 void CheckErrorTranslation(){
00233     theta_translation = GetPositionM2();
00234     SetpointError_Translation =  setpointTranslation -theta_translation;
00235 }
00236 void motorRotation()
00237 {
00238         printf("setpoint = %f \n\r", setpointRotation);
00239     //set direction
00240     if (SetpointError_Rotation > 0) {
00241         M3_Rotate = 0;
00242     } else {
00243         M3_Rotate = 1;
00244 
00245     }
00246     M3_ControlSpeed = Ts * fabs( pid_control(SetpointError_Rotation, Rotation_Kp, Rotation_Ki, Rotation_Kd, Rotation_error, Rotation_e_prev));
00247     if (fabs(SetpointError_Rotation) < fabs(Setpoint_Rotation*0.05)) {
00248         M3_ControlSpeed = 0;
00249     }
00250     if (theta_rotation > (Setpoint_Rotation*0.9))
00251         boolrotate = true;
00252     if ((theta_rotation < (Setpoint_Rotation*0.07) ) && (M3_Speed == 0))
00253         boolrotate = false;
00254         M3_Speed = M3_ControlSpeed;
00255 }
00256 void motorTranslation()
00257 {
00258     theta_translation = GetPositionM2();
00259     SetpointError_Translation =  setpointTranslation - theta_translation;
00260 
00261     //set direction
00262     if (SetpointError_Translation < 0) {
00263         M2_Rotate = 0;
00264     } else {
00265         M2_Rotate = 1;
00266     }
00267     M2_ControlSpeed = Ts * fabs( pid_control(SetpointError_Translation, Translation_Kp, Translation_Ki, Translation_Kd, Translation_error, Translation_e_prev));
00268     if (fabs(SetpointError_Translation) < fabs(Setpoint_Translation*0.05)) {
00269         M2_ControlSpeed = 0;
00270         
00271     }
00272     if ((theta_translation < Setpoint_Translation*0.95) && (M2_ControlSpeed == 0))
00273         booltranslate = true;
00274     if ((theta_translation > Setpoint_Translation*0.05) && (M2_ControlSpeed == 0))
00275         booltranslate = false;
00276     M2_Speed = M2_ControlSpeed;
00277   
00278 }
00279 void GoBack()
00280 {
00281     setpointTranslation = Setpoint_Back;
00282     motorTranslation();
00283     if (booltranslate == false) {
00284         setpointRotation = Setpoint_Back;
00285         motorRotation();
00286     }
00287     if (boolrotate == false) {
00288         turn = 0;
00289     }
00290     led_r = 1;
00291     led_b = 0;
00292 }
00293 
00294 void Burgerflip()
00295 {
00296     led_r = 0;
00297     led_b = 1;
00298     setpointTranslation = Setpoint_Translation;
00299     motorTranslation();
00300     if (booltranslate == true) {
00301         setpointRotation = Setpoint_Rotation;
00302         motorRotation();
00303     }
00304 }
00305 void BurgerflipActie()
00306 {
00307     Burgerflip();
00308     if (boolrotate == true) {
00309         GoBack();
00310     }
00311 }
00312 void print()
00313 {
00314     pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
00315 }
00316 int main()
00317 {
00318     //Leds
00319     led_g = 1;
00320     led_b = 1;
00321     led_r = 1;
00322 
00323     /**Attach the 'sample' function to the timer 'sample_timer'.
00324     * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
00325     */
00326     //sample_timer.attach(&sample, 0.001953125);
00327     sample_timer2.attach(&filterSampleLeft, Ts);        //512 Hz
00328     sample_timer.attach(&filterSampleRight, Ts);
00329     checkSetpointTranslation.attach(&CheckErrorTranslation,Ts);
00330     checkSetpointRotation.attach(&CheckErrorRotation,Ts);
00331     //printinfo.attach(&print, Ts);
00332     pc.baud(115200);
00333     pc.printf("please push the button to calibrate \n \r");
00334     while (1) {
00335         if (buttonCalibrate == 0) {
00336             calibrate = true;
00337             threshold_Left = lowpassFilterLeft*0.7;
00338             threshold_Right = lowpassFilterRight*0.7;
00339         }
00340         if (calibrate == true) {
00341             
00342             pc.printf("calibration complete, left = %f, right = %f \n \r", threshold_Left, threshold_Right);
00343             //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);
00344             GetDirections();
00345             if (draairechts == true) {
00346                 M1_Speed = 0.2;
00347                 M1_Rotate = 0;
00348             } else if (draailinks == true) {
00349                 M1_Speed = 0.2;
00350                 M1_Rotate = 1;
00351             } else if (turn == 1) {
00352                 BurgerflipActie();
00353             } else if (turn == 0) {
00354                 M2_Speed = 0;
00355                 M3_Speed = 0;
00356             }
00357             if ((draailinks == false) && (draairechts == false)) {
00358                 M1_Speed = 0;
00359             }
00360         }
00361     }
00362 }