main.cpp

00001 /*Code originally by Jesse Kaiser, s1355783 for control of the 2DOF Planar Table
00002 Some variables are also numbered at the end. The numbers stands for the muscle that controls it.
00003 Biceps =            1 = lower right arm(wrist flexors)
00004 Triceps =           2 = upper right arm(wrist extensors)
00005 Pectoralis Major =  3 = upper left arm(wrist extensors)
00006 Deltoid posterior = 4 = lower left arm(wrist flexors)
00007 The "x" and "y" at the end of variables stand for the X-movement or Y-movement respectivly.
00008 The code has been revised to work with the new board and also has a secondary way of controlling it using a joystick
00009 */
00010 
00011 #include "mbed.h"
00012 #include "MODSERIAL.h"
00013 #include "arm_math.h"
00014 #include "HIDScope.h"
00015 //factors proportional control
00016 #define K_Gain      150   //Gain of the filtered EMG signal
00017 #define Damp        5    //Deceleration of the motor
00018 #define Mass        1    // Mass value
00019 #define dt          0.01 //Sample frequency
00020 //threshold values to determine when the motor needs to stop
00021 #define EMG_tresh1   0.015
00022 #define EMG_tresh2   0.015
00023 #define EMG_tresh3   0.015
00024 #define EMG_tresh4   0.015
00025 
00026 //button for switching between proportional and precision speed control
00027 InterruptIn button1(PTC6);
00028 
00029 MODSERIAL pc(USBTX,USBRX);
00030 //joystick control
00031 AnalogIn X_control(A1);
00032 AnalogIn Y_control(A0);
00033 
00034 //Motor control 1
00035 DigitalOut Diry(D12);
00036 PwmOut Stepy(PTA1);
00037 DigitalOut Enabley(PTC3);
00038 
00039 //motor control 2
00040 DigitalOut Dirx(PTC17);
00041 PwmOut Stepx(PTD1);
00042 DigitalOut Enablex(D0);
00043 
00044 //Microstepping 1
00045 DigitalOut MS11(D11);
00046 DigitalOut MS21(D10);
00047 DigitalOut MS31(D9);
00048 //Microstepping 2
00049 DigitalOut MS12(PTC2);
00050 DigitalOut MS22(PTA2);
00051 DigitalOut MS32(PTB23);
00052 //initializing lights for testing and notification purposes
00053 DigitalOut Ledr(LED1);
00054 DigitalOut Ledg(LED2);
00055 DigitalOut Ledb(LED3);
00056 
00057 //EMG inputs
00058 AnalogIn emg1(A2); //biceps or wrist flexors bottom emg board
00059 AnalogIn emg2(A3); //triceps or wirst extensors
00060 AnalogIn emg3(A4); //Pectoralis major or wrist extensors
00061 AnalogIn emg4(A5); //Deltoid or wrist flexors top emg board
00062 
00063 HIDScope scope(4);
00064 Ticker   scopeTimer;
00065 Ticker emgtimer;
00066 Ticker looptimer1;
00067 Ticker looptimer2;
00068 
00069 //Variables for motor control
00070 float setpoint = 3200; //Frequentie setpoint
00071 float step_freq1 = 1;
00072 float step_freq2 = 1;
00073 
00074 //EMG filters
00075 //Lowpass1
00076 arm_biquad_casd_df1_inst_f32 lowpass1_biceps;
00077 arm_biquad_casd_df1_inst_f32 lowpass1_triceps;
00078 arm_biquad_casd_df1_inst_f32 lowpass1_pect;
00079 arm_biquad_casd_df1_inst_f32 lowpass1_deltoid;
00080 //lowpass2
00081 arm_biquad_casd_df1_inst_f32 lowpass2_biceps;
00082 arm_biquad_casd_df1_inst_f32 lowpass2_triceps;
00083 arm_biquad_casd_df1_inst_f32 lowpass2_pect;
00084 arm_biquad_casd_df1_inst_f32 lowpass2_deltoid;
00085 //highpass
00086 arm_biquad_casd_df1_inst_f32 highpass_biceps;
00087 arm_biquad_casd_df1_inst_f32 highpass_triceps;
00088 arm_biquad_casd_df1_inst_f32 highpass_pect;
00089 arm_biquad_casd_df1_inst_f32 highpass_deltoid;
00090 //used as extra filter for wrist motion
00091 //bandstop
00092 arm_biquad_casd_df1_inst_f32 bandstop_biceps;
00093 arm_biquad_casd_df1_inst_f32 bandstop_triceps;
00094 arm_biquad_casd_df1_inst_f32 bandstop_pect;
00095 arm_biquad_casd_df1_inst_f32 bandstop_deltoid;
00096 
00097 //lowpass 1 filter settings: Fc = 49 Hz, Fs = 100 Hz, Gain = -3 dB
00098 //lowpass 2 filter settings: Fc = 0.8 Hz, Fs = 100 Hz, Gain = -3 dB
00099 float lowpass1_const[] = {0.956543225556877,1.91308645111375,0.956543225556877,-1.91119706742607,-0.914975834801434};
00100 float lowpass2_const[] = {0.000609854718717299,0.00121970943743460,0.000609854718717299,1.92894226325203,-0.931381682126903};
00101 //highpass filter settings: Fc = 20 Hz, Fs = 100 Hz
00102 float highpass_const[] = {0.391335772501769,-0.782671545003538,0.391335772501769,0.369527377351241,-0.195815712655833};
00103 //bandstop filter settings Fc=[29Hz 36Hz], Fs= 100Hz
00104 float bandstop_const[] = {0.732022476556623, 0.681064744276583,0.732022476556623,-0.535944148680739,-0.713976335521464,1,0.930387749130681,1,-1.04147956553087,-0.752398361226849};
00105 
00106 /*
00107 //values are usable for triceps and biceps continuous motion, not for wrist motion.
00108 //lowpass 1 filter settings: Fc = 45 Hz, Fs = 100 Hz, Gain = -3 dB
00109 //lowpass 2 filter settings: Fc = 0.3 Hz, Fs = 100 Hz, Gain = -3 dB
00110 float lowpass1_const[] = {0.800592403464570,1.60118480692914,0.800592403464570,-1.56101807580072,-0.641351538057563};
00111 float lowpass2_const[] = {8.76555487540147e-05,  0.000175311097508029,  8.76555487540147e-05 , 1.97334424978130, -0.973694871976315};
00112 //highpass filter settings: Fc = 20 Hz, Fs = 100 Hz
00113 
00114 float highpass_const[] = {0.391335772501769 ,-0.782671545003538,  0.391335772501769,0.369527377351241,  -0.195815712655833};
00115 
00116 */
00117 
00118 
00119 //state values for filter initialization
00120 float lowpass1_biceps_states[4];
00121 float lowpass2_biceps_states[4];
00122 float highpass_biceps_states[4];
00123 float bandstop_biceps_states[8];
00124 
00125 float lowpass1_triceps_states[4];
00126 float lowpass2_triceps_states[4];
00127 float highpass_triceps_states[4];
00128 float bandstop_triceps_states[8];
00129 
00130 float lowpass1_pect_states[4];
00131 float lowpass2_pect_states[4];
00132 float highpass_pect_states[4];
00133 float bandstop_pect_states[8];
00134 
00135 float lowpass1_deltoid_states[4];
00136 float lowpass2_deltoid_states[4];
00137 float highpass_deltoid_states[4];
00138 float bandstop_deltoid_states[8];
00139 
00140 
00141 //global variables
00142 float filtered_biceps, filtered_triceps, filtered_pect, filtered_deltoid;
00143 float speed_old1, speed_old2, speed_old3, speed_old4;
00144 float acc1, acc2, acc3, acc4;
00145 float force1, force2, force3, force4;
00146 float speed1, speed2, speed3, speed4;
00147 float damping1, damping2, damping3, damping4;
00148 float emg_x, emg_y;
00149 float emg_y_abs = 0;
00150 float emg_x_abs = 0;
00151 bool proportional = 1;
00152 int count = 0;
00153 
00154 void looper_emg()//EMG filtering function
00155 {
00156     float emg_value1_f32, emg_value2_f32, emg_value3_f32, emg_value4_f32;
00157     emg_value1_f32 = emg1.read();//read out analog inputs for EMG value
00158     emg_value2_f32 = emg2.read();
00159     emg_value3_f32 = emg1.read();
00160     emg_value4_f32 = emg2.read();
00161     
00162     //Biquad process emg biceps
00163     arm_biquad_cascade_df1_f32(&bandstop_biceps, &emg_value1_f32, &filtered_biceps, 1 );//bandstop filter used for wrist motion
00164     arm_biquad_cascade_df1_f32(&lowpass1_biceps, &filtered_biceps, &filtered_biceps, 1 );//first lowpass filter
00165     arm_biquad_cascade_df1_f32(&highpass_biceps, &filtered_biceps, &filtered_biceps, 1 );//highpass filter
00166     filtered_biceps = fabs(filtered_biceps);                                                //Rectifier, The Gain is already implemented.
00167     arm_biquad_cascade_df1_f32(&lowpass2_biceps, &filtered_biceps, &filtered_biceps, 1 );    //second low pass filter
00168 
00169     //Biquad process emg triceps
00170    arm_biquad_cascade_df1_f32(&bandstop_triceps, &emg_value2_f32, &filtered_triceps, 1 );    //used for wrist motion
00171    arm_biquad_cascade_df1_f32(&lowpass1_triceps, &filtered_triceps, &filtered_triceps, 1 );
00172    arm_biquad_cascade_df1_f32(&highpass_triceps, &filtered_triceps, &filtered_triceps, 1 );
00173    filtered_triceps = fabs(filtered_triceps);
00174    arm_biquad_cascade_df1_f32(&lowpass2_triceps, &filtered_triceps, &filtered_triceps, 1 );
00175 
00176     //Biquad process emg pectoralis major
00177     arm_biquad_cascade_df1_f32(&bandstop_pect, &emg_value3_f32, &filtered_pect, 1 );//used for wrist motion
00178     arm_biquad_cascade_df1_f32(&lowpass1_pect, &filtered_pect, &filtered_pect, 1 );
00179     arm_biquad_cascade_df1_f32(&highpass_pect, &filtered_pect, &filtered_pect, 1 );
00180     filtered_pect = fabs(filtered_pect);
00181     arm_biquad_cascade_df1_f32(&lowpass2_pect, &filtered_pect, &filtered_pect, 1 );
00182 
00183     //Biquad process emg deltoid
00184     arm_biquad_cascade_df1_f32(&bandstop_deltoid, &emg_value4_f32, &filtered_deltoid, 1 );//used for wrist motion
00185     arm_biquad_cascade_df1_f32(&lowpass1_deltoid, &filtered_deltoid, &filtered_deltoid, 1 );
00186     arm_biquad_cascade_df1_f32(&highpass_deltoid, &filtered_deltoid, &filtered_deltoid, 1 );
00187     filtered_deltoid = fabs(filtered_deltoid);
00188     arm_biquad_cascade_df1_f32(&lowpass2_deltoid, &filtered_deltoid, &filtered_deltoid, 1 );
00189  
00190 // send value to PC for control
00191 
00192     scope.set(0,filtered_biceps); //Filtered EMG signals
00193     scope.set(1,filtered_triceps);
00194     scope.set(2,filtered_pect);
00195     scope.set(3,filtered_deltoid);
00196     scope.send();
00197     
00198 }
00199 
00200 //y motor control
00201 void looper_motory()
00202 {
00203 //determine which direction the motor will rotate
00204     emg_y = (filtered_biceps - filtered_triceps);
00205     //switch between proportional EMG control and pre-set speed control
00206     if(proportional==0)
00207     {
00208     //proportional control explained in the report
00209     Stepy.write(0.5);
00210     Ledr=0;//red light to make it clear which control method is used
00211     emg_y_abs = fabs(emg_y);
00212     force1 = emg_y_abs*K_Gain;
00213     force1 = force1 - damping1;
00214     acc1 = force1/Mass;
00215     speed1 = speed_old1 + (acc1 * dt);
00216     damping1 = speed1 * Damp;
00217     step_freq1 = setpoint * speed1;
00218     Stepy.period(1.0/step_freq1);
00219     speed_old1 = speed1;
00220     
00221         //Speed limit
00222     if (speed1 > 1) {
00223         speed1 = 1;
00224     }
00225 }
00226     else{
00227     //precision control
00228     Ledr=1;
00229     Stepy.period(0.000625);//frequency of 1600 Hz
00230     }
00231     
00232     if (emg_y > 0) {//downward movement
00233         Diry = 0;
00234     }
00235     if (emg_y < 0) {//upward movement
00236         Diry = 1;
00237     }
00238 
00239     //EMG treshold, determine if signal is strong enough to start the motors
00240     if (filtered_biceps < EMG_tresh1 && filtered_triceps < EMG_tresh2) {
00241         Enabley = 1; //Enable = 1 turns the motor off.
00242     }
00243     else {
00244         Enabley = 0;//Enable = 0 turns the motor on.
00245     }
00246 }
00247 //same setup as with the y motor
00248 void looper_motorx()
00249 {
00250     emg_x = (filtered_pect - filtered_deltoid);
00251     if(proportional==0)
00252     {
00253         //proportional control
00254         Stepx.write(0.5);
00255         Ledr=0;
00256         emg_x_abs = fabs(emg_x);
00257         force2 = emg_x_abs*K_Gain;
00258         force2 = force2 - damping2;
00259         acc2 = force2/Mass;
00260         speed2 = speed_old2 + (acc2 * dt);
00261         damping2 = speed2 * Damp;
00262         step_freq2 = setpoint * speed2;
00263         Stepx.period(1.0/step_freq2);
00264         speed_old2 = speed2;
00265     
00266         //speed limit
00267         if (speed2 > 1) {
00268             speed2 = 1;
00269         }   
00270 
00271     }
00272         else{
00273         //precision control 
00274         Ledr=1;
00275         Stepx.period(0.000625);//frequency of 1600 Hz
00276         }
00277         
00278     if (emg_x > 0) {//left movement
00279         Dirx = 0;
00280     }
00281     if (emg_x < 0) {//rig1ht movement
00282         Dirx = 1;
00283     }
00284 
00285     //EMG treshold
00286     if (filtered_pect < EMG_tresh3 && filtered_deltoid < EMG_tresh4) {
00287         Enablex = 1; //Enable = 1 turns the motor off.
00288     } else {
00289         Enablex = 0;
00290     }
00291 }
00292 
00293 void changecontrol(){
00294         proportional = !proportional;//code for changing speed control
00295         }
00296 
00297 
00298 int main()
00299 {
00300     pc.baud(115200);//sent data to pc for testing purposes
00301     Ledr=1;//turn of all the lights
00302     Ledg=1;
00303     Ledb=1;
00304    
00305     MS11=1;//higher microstepping in combination with a higher step frequency reduces the vibration significantly
00306     MS21=1;//it is now in 16th step mode, which seems to work really well and smooth without causing major vibrations
00307     MS31=1;
00308     MS12=1;
00309     MS22=1;
00310     MS32=1;
00311 Stepy.write(0.5);//set all motors to half load and disable them.
00312 Enabley.write(1);
00313 Stepx.write(0.5);
00314 Enablex.write(1);
00315 //Stepy.period(0.000625);//use period change for speed adjustments
00316 //Stepx.period(0.000625);//frequency of 1600 Hz
00317 
00318 
00319 /*//code for controlling the mechanism with a joystick
00320 float Left_value = 0.6;
00321 float Right_value = 0.9;
00322 float Up_value = 0.6;
00323 float Down_value = 0.9;
00324 
00325 while(1){
00326     if (X_control.read() < Left_value){
00327      Dirx.write(0);
00328      Enablex.write(0);
00329      }
00330 else if (X_control.read() > Right_value){
00331      Dirx.write(1);
00332      Enablex.write(0);
00333      }
00334 else{
00335     Enablex.write(1);
00336 }         
00337 if (Y_control.read() < Up_value){
00338      Diry.write(0);
00339      Enabley.write(0);
00340      }
00341 else if (Y_control.read() > Down_value){
00342      Diry.write(1);
00343      Enabley.write(0);
00344      }
00345 else{
00346     Enabley.write(1);
00347 }
00348 }*/
00349 //initialization of the biquad filters and appointing of names to filters.
00350     //biceps
00351     arm_biquad_cascade_df1_init_f32(&lowpass1_biceps, 1 , lowpass1_const, lowpass1_biceps_states);
00352     arm_biquad_cascade_df1_init_f32(&lowpass2_biceps, 1 , lowpass2_const, lowpass2_biceps_states);
00353     arm_biquad_cascade_df1_init_f32(&highpass_biceps, 1 , highpass_const, highpass_biceps_states);
00354     arm_biquad_cascade_df1_init_f32(&bandstop_biceps, 2 , bandstop_const, bandstop_biceps_states);
00355     //triceps
00356     arm_biquad_cascade_df1_init_f32(&lowpass1_triceps, 1 , lowpass1_const, lowpass1_triceps_states);
00357     arm_biquad_cascade_df1_init_f32(&lowpass2_triceps, 1 , lowpass2_const, lowpass2_triceps_states);
00358     arm_biquad_cascade_df1_init_f32(&highpass_triceps, 1 , highpass_const, highpass_triceps_states);
00359     arm_biquad_cascade_df1_init_f32(&bandstop_triceps, 2 , bandstop_const, bandstop_triceps_states);
00360     //pectoralis major
00361     arm_biquad_cascade_df1_init_f32(&lowpass1_pect, 1 , lowpass1_const, lowpass1_pect_states);
00362     arm_biquad_cascade_df1_init_f32(&lowpass2_pect, 1 , lowpass2_const, lowpass2_pect_states);
00363     arm_biquad_cascade_df1_init_f32(&highpass_pect, 1 , highpass_const, highpass_pect_states);
00364     arm_biquad_cascade_df1_init_f32(&bandstop_pect, 2 , bandstop_const, bandstop_pect_states);
00365 
00366     //deltoid
00367     arm_biquad_cascade_df1_init_f32(&lowpass1_deltoid, 1 , lowpass1_const, lowpass1_deltoid_states);
00368     arm_biquad_cascade_df1_init_f32(&lowpass2_deltoid, 1 , lowpass2_const, lowpass2_deltoid_states);
00369     arm_biquad_cascade_df1_init_f32(&highpass_deltoid, 1 , highpass_const, highpass_deltoid_states);
00370     arm_biquad_cascade_df1_init_f32(&bandstop_deltoid, 2 , bandstop_const, bandstop_deltoid_states);
00371 
00372     emgtimer.attach(looper_emg, 0.01);//emg signal filtering
00373 
00374     looptimer1.attach(looper_motorx, 0.01); //X motor control
00375 
00376     looptimer2.attach(looper_motory, 0.01); //Y motor control
00377 
00378     while(1){//if button is pressed the control mode changes with it.
00379         button1.fall(changecontrol);
00380         };
00381 }