main.cpp

00001 /***************************************/
00002 /*                                     */
00003 /*   BRONCODE GROEP 5, MODULE 9, 2014  */
00004 /*       *****-THE SLAP-******         */
00005 /*                                     */
00006 /* -Dominique Clevers                  */
00007 /* -Rianne van Dommelen                */
00008 /* -Daan de Muinck Keizer              */
00009 /* -David den Houting                  */
00010 /* -Marjolein Thijssen                 */
00011 /***************************************/
00012 #include "mbed.h"
00013 #include "arm_math.h"
00014 #include "encoder.h"
00015 #include "MODSERIAL.h"
00016 //include "TextLCD.h"
00017 
00018 #define M2_PWM PTC8 //kleine motor
00019 #define M2_DIR PTC9 //kleine motor
00020 #define M1_PWM PTA5 //grote motor
00021 #define M1_DIR PTA4 //grote motor
00022 #define TSAMP 0.005  // Sampletijd, 200Hz
00023 #define K_P_KM (0.01)
00024 #define K_I_KM (0.0003  *TSAMP)
00025 #define K_D_KM (0.0 /TSAMP)
00026 #define K_P_GM (0.0022)
00027 #define K_I_GM (0.0001  *TSAMP)
00028 #define K_D_GM (0.00000001 /TSAMP)
00029 #define I_LIMIT 1.
00030 #define RADTICKGM 0.003927
00031 #define THETADOT0 6.85
00032 #define THETADOT1 7.77
00033 #define THETADOT2 9.21
00034 
00035 //TextLCD pc(PTE5, PTE3, PTE2, PTB11, PTB10, PTB9); // rs, e, d4-d7 CONTROLEREN!! (Pinnen wel vrij :) )! //Textpc pc(p15, p16, p17, p18, p19, p20, Textpc::pc16x4); // rs, e, d4-d7 ok
00036 
00037 Encoder motor2(PTD2,PTD0); //geel,wit kleine motor MOTOR 2
00038 Encoder motor1(PTD5,PTA13);//geel,wit
00039 PwmOut pwm_motor1(M1_PWM);
00040 PwmOut pwm_motor2(M2_PWM);
00041 DigitalOut motordir2(M2_DIR);
00042 DigitalOut motordir1(M1_DIR);
00043 AnalogIn emg0(PTB0); //Biceps
00044 AnalogIn emg1(PTB1); //deltoid
00045 
00046 MODSERIAL pc(USBTX,USBRX,64,1024);
00047 
00048 
00049 float emg0_value_f32,filtered_emg0_notch,filtered_emg0_notch_highpass,filtered_emg0_notch_highpass_lowpass,filtered_emg0_eindsignaal_abs,envelop_emg0,pwm_to_motor1,max_value_biceps,min_value_biceps; //variable to store value in for biceps
00050 float emg1_value_f32,filtered_emg1_notch,filtered_emg1_notch_highpass,filtered_emg1_notch_highpass_lowpass,filtered_emg1_eindsignaal_abs,envelop_emg1,pwm_to_motor2,max_value_deltoid,min_value_deltoid,metingstatus; //variable to store value in for deltoid
00051 
00052 arm_biquad_casd_df1_inst_f32 notch_biceps;
00053 arm_biquad_casd_df1_inst_f32 notch_deltoid;
00054 // constants for 50 Hz notch (bandbreedte 2 Hz)
00055 float notch_const[] = {0.9695312529087462, -0.0, 0.9695312529087462, 0.0, -0.9390625058174924}; //constants for 50Hz notch
00056 //state values
00057 float notch_biceps_states[4];
00058 float notch_deltoid_states[4];
00059 
00060 arm_biquad_casd_df1_inst_f32 highpass_biceps;
00061 arm_biquad_casd_df1_inst_f32 highpass_deltoid;
00062 //constants for 20Hz highpass
00063 float highpass_const[] = {0.638945525159022, -1.277891050318045, 0.638945525159022, 1.142980502539901, -0.412801598096189};
00064 //state values
00065 float highpass_biceps_states[4];
00066 float highpass_deltoid_states[4];
00067 
00068 arm_biquad_casd_df1_inst_f32 lowpass_biceps;
00069 arm_biquad_casd_df1_inst_f32 lowpass_deltoid;
00070 //constants for 80Hz lowpass
00071 float lowpass_const[] = {0.638945525159022, 1.277891050318045, 0.638945525159022, -1.142980502539901, -0.412801598096189};
00072 //state values
00073 float lowpass_biceps_states[4];
00074 float lowpass_deltoid_states[4];
00075 
00076 arm_biquad_casd_df1_inst_f32 envelop_biceps;
00077 arm_biquad_casd_df1_inst_f32 envelop_deltoid;
00078 //constants for envelop
00079 float envelop_const[] = {0.005542711916075981, 0.011085423832151962, 0.005542711916075981, 1.7786300789392977, -0.8008009266036016};
00080 // state values
00081 float envelop_biceps_states[4];
00082 float envelop_deltoid_states[4];
00083 
00084 enum slapstates {RUST,KALIBRATIE,RICHTEN,SLAAN,HOME}; //verschillende stadia definieren voor gebruik in CASES
00085 uint8_t state=RUST;
00086 
00087 volatile bool looptimerflag;
00088 void setlooptimerflag(void)
00089 {
00090     looptimerflag = true;
00091 }
00092 
00093 void clamp(float * in, float min, float max)
00094 {
00095 *in > min ? *in < max? : *in = max: *in = min;
00096 }
00097 
00098 float pidkm(float setpointkm, float measurementkm) //PID Regelaar kleine motor
00099 {
00100     float error_km;
00101     static float prev_error_km = 0;
00102     float           out_p_km = 0;
00103     static float    out_i_km = 0;              //static, want dan wordt vorige waarde onthouden
00104     float           out_d_km = 0;
00105     error_km  = setpointkm-measurementkm;
00106     out_p_km  = error_km*K_P_KM;
00107     out_i_km += error_km*K_I_KM;
00108     out_d_km  = (error_km-prev_error_km)*K_D_KM;
00109     clamp(&out_i_km,-I_LIMIT,I_LIMIT);
00110     prev_error_km = error_km;
00111     return out_p_km + out_i_km + out_d_km;
00112 }
00113 
00114 float pidgm(float setpointgm, float measurementgm) //PID Regelaar grote motor
00115 {
00116     float error_gm;
00117     static float prev_error_gm = 0;
00118     float           out_p_gm = 0;
00119     static float    out_i_gm = 0;
00120     float           out_d_gm = 0;
00121     error_gm  = setpointgm-measurementgm;
00122     out_p_gm  = error_gm*K_P_GM;
00123     out_i_gm += error_gm*K_I_GM;
00124     out_d_gm  = (error_gm-prev_error_gm)*K_D_GM;
00125     clamp(&out_i_gm,-I_LIMIT,I_LIMIT);
00126     prev_error_gm = error_gm;
00127     return out_p_gm + out_i_gm + out_d_gm;
00128 }
00129 void emgmeten()
00130 {
00131     /*put raw emg value in emg_value*/
00132     emg0_value_f32 = emg0.read();
00133     emg1_value_f32 = emg1.read();
00134 
00135     //process emg biceps
00136     arm_biquad_cascade_df1_f32(&notch_biceps, &emg0_value_f32, &filtered_emg0_notch, 1 );
00137     arm_biquad_cascade_df1_f32(&highpass_biceps, &filtered_emg0_notch, &filtered_emg0_notch_highpass, 1 );
00138     arm_biquad_cascade_df1_f32(&lowpass_biceps, &filtered_emg0_notch_highpass, &filtered_emg0_notch_highpass_lowpass, 1 );
00139     filtered_emg0_eindsignaal_abs = fabs(filtered_emg0_notch_highpass_lowpass);  //gelijkrichter
00140     arm_biquad_cascade_df1_f32(&envelop_biceps, &filtered_emg0_eindsignaal_abs, &envelop_emg0, 1 );
00141 
00142     //process emg deltoid
00143     arm_biquad_cascade_df1_f32(&notch_deltoid, &emg1_value_f32, &filtered_emg1_notch, 1 );
00144     arm_biquad_cascade_df1_f32(&highpass_deltoid, &filtered_emg1_notch, &filtered_emg1_notch_highpass, 1 );
00145     arm_biquad_cascade_df1_f32(&lowpass_deltoid, &filtered_emg1_notch_highpass, &filtered_emg1_notch_highpass_lowpass, 1 );
00146     filtered_emg1_eindsignaal_abs = fabs(filtered_emg1_notch_highpass_lowpass);  //gelijkrichter
00147     arm_biquad_cascade_df1_f32(&envelop_deltoid, &filtered_emg1_eindsignaal_abs, &envelop_emg1, 1 );
00148 }
00149 
00150 
00151 int main()
00152 {
00153     pc.baud(38400); //PC baud rate is 38400 bits/seconde
00154     Ticker emg_timer;
00155     emg_timer.attach(emgmeten, TSAMP);
00156     Ticker looptimer;
00157     looptimer.attach(setlooptimerflag,TSAMP);
00158     Timer tijdtimer;
00159     Timer tijdslaan;
00160     tijdtimer.start();
00161     tijdslaan.start();
00162     arm_biquad_cascade_df1_init_f32(&notch_biceps,1 , notch_const, notch_biceps_states);
00163     arm_biquad_cascade_df1_init_f32(&highpass_biceps,1 ,highpass_const,highpass_biceps_states);
00164     arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 ,lowpass_const,lowpass_biceps_states);
00165     arm_biquad_cascade_df1_init_f32(&notch_deltoid,1 , notch_const, notch_deltoid_states);
00166     arm_biquad_cascade_df1_init_f32(&highpass_deltoid,1 ,highpass_const,highpass_deltoid_states);
00167     arm_biquad_cascade_df1_init_f32(&lowpass_deltoid,1 ,lowpass_const,lowpass_deltoid_states);
00168     arm_biquad_cascade_df1_init_f32(&envelop_deltoid,1 ,envelop_const,envelop_deltoid_states);
00169     arm_biquad_cascade_df1_init_f32(&envelop_biceps,1 ,envelop_const,envelop_biceps_states);
00170 
00171     while(true) {
00172         switch(state) {
00173             case RUST: {                            //Aanzetten
00174                 pc.printf("--THE SLAP -- GROEP 5");        //pc scherm
00175                 wait(5);
00176                 state = KALIBRATIE;
00177                 break;
00178             }
00179 
00180             case KALIBRATIE: {                                  //kalibreren met maximale inspanning
00181                 max_value_biceps=0;
00182                 max_value_deltoid=0;
00183                 //maximale inspanning biceps
00184                 pc.printf("Kalibratie1: Span Biceps!"); //pc scherm
00185                 wait(5);
00186                 pc.printf("Meting loopt"); //pc scherm
00187                 tijdtimer.reset();
00188                 tijdtimer.start();
00189                 while (tijdtimer.read() <= 3) {
00190                     if (envelop_emg0 > max_value_biceps) {
00191                         max_value_biceps = envelop_emg0;
00192                     }
00193                 }
00194                 tijdtimer.stop();
00195                 tijdtimer.reset();
00196                 pc.printf("max value %f\n\r", max_value_biceps);
00197                 wait(5);
00198 
00199                 //maximale inspanning deltoid
00200                 pc.printf("Kalibratie2: Span deltoid!"); //pc scherm
00201                 wait(5);
00202                 tijdtimer.start();
00203                 pc.printf("Meting loopt"); //pc scherm
00204                 while (tijdtimer.read() <= 3) {
00205                     if (envelop_emg1 > max_value_deltoid) {
00206                         max_value_deltoid = envelop_emg1;
00207                     }
00208                 }
00209                 // tijdtimer.stop();
00210                 tijdtimer.reset();
00211                 pc.printf("max value %f\n\r", max_value_deltoid);
00212                 wait(5);
00213 
00214                 state = RICHTEN;
00215                 break;
00216             }// einde kalibratie case
00217 
00218             case RICHTEN: {                                  //batje richten (gebruik biceps en deltoid)
00219                 wait(3);
00220                 pc.printf("Richten");                  //regel 1 LCD scherm
00221                 pc.printf("Kies goal!");               //regel 2 LCD scherm
00222                 float setpointkm;
00223                 float new_pwm_km;
00224                 wait(5);
00225                 pc.printf("Meting loopt");
00226                 float kalibratiewaarde_deltoid;
00227                 kalibratiewaarde_deltoid=(envelop_emg1/max_value_deltoid);
00228                 pc.printf("deltoid %f\n\r", kalibratiewaarde_deltoid);        //WEGHALEN LATER
00229                 if (kalibratiewaarde_deltoid >= 0.35) {
00230                     setpointkm = -127;          //11,12graden naar links
00231                     pc.printf("links");
00232                 } else if (kalibratiewaarde_deltoid>0.1 && kalibratiewaarde_deltoid<=0.35) {
00233                     setpointkm = 0;        //11,12graden naar rechts
00234                     pc.printf("midden");
00235                 } else {
00236                     setpointkm = 127;
00237                     pc.printf("rechts");
00238                 }
00239                 //MOTOR 2 LATEN BEWEGEN NAAR setpointkm
00240                 tijdtimer.reset();
00241                 while (tijdtimer.read() <= 3) {
00242                     while(looptimerflag == false);
00243                     looptimerflag = false;
00244                     new_pwm_km = pidkm(setpointkm, motor2.getPosition());   //bewegen naar setpoint
00245                     clamp(&new_pwm_km, -1,1);
00246                     if(new_pwm_km < 0)
00247                         motordir2 = 1;  //links
00248                     else
00249                         motordir2 = 0;  //rechts
00250                     pwm_motor2.write(abs(new_pwm_km));
00251                 }
00252                 wait(2);
00253                 state = SLAAN;
00254                 break;
00255             }
00256 
00257             case SLAAN: {
00258                 wait(3);
00259                 pc.printf("Slaan");                  //regel 1 LCD scherm
00260                 pc.printf("Kies goal");               //regel 2 LCD scherm
00261                 float thetadot;
00262                 float setpointgm;
00263                 float new_pwm_gm;
00264                 float setpointkm;
00265                 float new_pwm_km;
00266                 wait(5);
00267                 pc.printf("Meting loopt");
00268                 float kalibratiewaarde_biceps;
00269                 kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps);
00270                 pc.printf("biceps %f\n\r", kalibratiewaarde_biceps);      //WEGHALEN LATER
00271                 if (kalibratiewaarde_biceps <= 0.2) { //kalibratiewaarde_biceps<0.3 goal onderin
00272                     thetadot=THETADOT0;
00273                     pc.printf("ONDERSTE GOAL");
00274                 } else if (kalibratiewaarde_biceps>0.2 && kalibratiewaarde_biceps<=0.4) { //0.3<kalibratiewaarde_biceps<0.6 goal midden
00275                     thetadot=THETADOT1;
00276                     pc.printf("MIDDELSTE GOAL");
00277                 } else { //goal bovenin
00278                     thetadot=THETADOT2;
00279                     pc.printf("BOVENSTE GOAL");
00280                 }
00281                 wait(3);
00282                 pc.printf("Daar gaat ie!");
00283 
00284                 //MOTOR 1 LATEN BEWEGEN met snelheid thetadot
00285                 tijdtimer.reset();
00286                 tijdslaan.reset();
00287                 while (tijdtimer.read() <=3) {
00288                     while(looptimerflag == false);
00289                     looptimerflag = false;
00290                     if (motor1.getPosition()>= 444 ) {
00291                         setpointgm = 444;
00292                     } else {
00293                         setpointgm = ((thetadot*tijdslaan.read())/RADTICKGM);
00294                     }
00295                     new_pwm_gm = pidgm(setpointgm, motor1.getPosition());
00296                     clamp(&new_pwm_gm, -1,1);
00297                     if(new_pwm_gm < 0) {
00298                         motordir1 = 1;  //links
00299                     } else {
00300                         motordir1 = 0;  //rechts
00301                     }
00302                     pwm_motor1.write(abs(new_pwm_gm));
00303                 }
00304                 pc.printf("pos %d %f\n\r", motor1.getPosition(),setpointgm);
00305 
00306                 //MOTOR 2 OP POSITIE HOUDEN
00307                 new_pwm_km = pidkm(setpointkm, motor2.getPosition());   //bewegen naar setpoint
00308                 clamp(&new_pwm_km, -1,1);
00309                 if(new_pwm_km < 0)
00310                     motordir2 = 1;  //links
00311                 else
00312                     motordir2 = 0;  //rechts
00313                 pwm_motor2.write(abs(new_pwm_km));
00314 
00315             }
00316             state = HOME;
00317             break;
00318 
00319             case HOME: {
00320                 pc.printf("HOMESTATE");
00321                 float setpointgm;
00322                 float new_pwm_gm;
00323                 float setpointkm;
00324                 float new_pwm_km;
00325 
00326                 //MOTOR 1 naar home
00327                 tijdtimer.reset();
00328                 tijdslaan.reset();
00329                 while (tijdtimer.read() <=6) {
00330                     while(looptimerflag == false);
00331                     looptimerflag = false;
00332                     setpointgm = 0;
00333                     new_pwm_gm = pidgm(setpointgm, motor1.getPosition());
00334                     clamp(&new_pwm_gm, -1,1);
00335                     if(new_pwm_gm < 0)
00336                         motordir1 = 1;  //links
00337                     else
00338                         motordir1 = 0;  //rechts
00339                     pwm_motor1.write(abs(new_pwm_gm));
00340 
00341                     //MOTOR 2 naar home
00342                     setpointkm=0;
00343                     new_pwm_km = pidkm(setpointkm, motor2.getPosition());   //bewegen naar setpoint
00344                     clamp(&new_pwm_km, -1,1);
00345                     if(new_pwm_km < 0)
00346                         motordir2 = 1;  //links
00347                     else
00348                         motordir2 = 0;  //rechts
00349                     pwm_motor2.write(abs(new_pwm_km));
00350                 }
00351                 wait(10);
00352                 state = RICHTEN;      //optioneel
00353                 break;
00354             }
00355             default: {
00356                 state = RUST;
00357             }
00358         }
00359     }
00360 }