Diff: main.cpp

Revision:

8:69b7085f5343

Parent:

7:20757784f5bf

--- a/main.cpp	Thu Jan 21 09:51:38 2016 +0000
+++ b/main.cpp	Sun Jan 24 10:47:09 2016 +0000
@@ -17,19 +17,20 @@
 #define Damp        5    //Deceleration of the motor
 #define Mass        1    // Mass value
 #define dt          0.01 //Sample frequency
-//thresholds
+//threshold values to determine when the motor needs to stop
 #define EMG_tresh1   0.015
 #define EMG_tresh2   0.015
 #define EMG_tresh3   0.015
 #define EMG_tresh4   0.015
 
-//button for control switching
+//button for switching between proportional and precision speed control
 InterruptIn button1(PTC6);
 
 MODSERIAL pc(USBTX,USBRX);
 //joystick control
 AnalogIn X_control(A1);
 AnalogIn Y_control(A0);
+
 //Motor control 1
 DigitalOut Diry(D12);
 PwmOut Stepy(PTA1);
@@ -48,7 +49,7 @@
 DigitalOut MS12(PTC2);
 DigitalOut MS22(PTA2);
 DigitalOut MS32(PTB23);
-
+//initializing lights for testing and notification purposes
 DigitalOut Ledr(LED1);
 DigitalOut Ledg(LED2);
 DigitalOut Ledb(LED3);
@@ -61,32 +62,33 @@
 
 HIDScope scope(4);
 Ticker   scopeTimer;
-    Ticker emgtimer;
-    Ticker looptimer1;
-    Ticker looptimer2;
+Ticker emgtimer;
+Ticker looptimer1;
+Ticker looptimer2;
 
 //Variables for motor control
 float setpoint = 3200; //Frequentie setpoint
 float step_freq1 = 1;
 float step_freq2 = 1;
 
-//EMG filter
+//EMG filters
+//Lowpass1
 arm_biquad_casd_df1_inst_f32 lowpass1_biceps;
 arm_biquad_casd_df1_inst_f32 lowpass1_triceps;
 arm_biquad_casd_df1_inst_f32 lowpass1_pect;
 arm_biquad_casd_df1_inst_f32 lowpass1_deltoid;
-
+//lowpass2
 arm_biquad_casd_df1_inst_f32 lowpass2_biceps;
 arm_biquad_casd_df1_inst_f32 lowpass2_triceps;
 arm_biquad_casd_df1_inst_f32 lowpass2_pect;
 arm_biquad_casd_df1_inst_f32 lowpass2_deltoid;
-
+//highpass
 arm_biquad_casd_df1_inst_f32 highpass_biceps;
 arm_biquad_casd_df1_inst_f32 highpass_triceps;
 arm_biquad_casd_df1_inst_f32 highpass_pect;
 arm_biquad_casd_df1_inst_f32 highpass_deltoid;
-
 //used as extra filter for wrist motion
+//bandstop
 arm_biquad_casd_df1_inst_f32 bandstop_biceps;
 arm_biquad_casd_df1_inst_f32 bandstop_triceps;
 arm_biquad_casd_df1_inst_f32 bandstop_pect;
@@ -114,7 +116,7 @@
 */
 
 
-//state values
+//state values for filter initialization
 float lowpass1_biceps_states[4];
 float lowpass2_biceps_states[4];
 float highpass_biceps_states[4];
@@ -144,32 +146,25 @@
 float speed1, speed2, speed3, speed4;
 float damping1, damping2, damping3, damping4;
 float emg_x, emg_y;
-float cx = 0;
-float cy = 0;
-float errorx = 0.2;
-float errory = 0.2;
-float Ps_x = 0;
-float Ps_y = 0;
-float hstep_freqx = 1;
-float hstep_freqy = 1;
 float emg_y_abs = 0;
 float emg_x_abs = 0;
 bool proportional = 1;
+int count = 0;
 
-void looper_emg()
+void looper_emg()//EMG filtering function
 {
     float emg_value1_f32, emg_value2_f32, emg_value3_f32, emg_value4_f32;
-    emg_value1_f32 = emg1.read();
+    emg_value1_f32 = emg1.read();//read out analog inputs for EMG value
     emg_value2_f32 = emg2.read();
-    emg_value3_f32 = emg3.read();
-    emg_value4_f32 = emg4.read();
+    emg_value3_f32 = emg1.read();
+    emg_value4_f32 = emg2.read();
     
     //Biquad process emg biceps
-    arm_biquad_cascade_df1_f32(&bandstop_biceps, &emg_value1_f32, &filtered_biceps, 1 );//used for wrist motion
-    arm_biquad_cascade_df1_f32(&lowpass1_biceps, &filtered_biceps, &filtered_biceps, 1 );
-    arm_biquad_cascade_df1_f32(&highpass_biceps, &filtered_biceps, &filtered_biceps, 1 );
+    arm_biquad_cascade_df1_f32(&bandstop_biceps, &emg_value1_f32, &filtered_biceps, 1 );//bandstop filter used for wrist motion
+    arm_biquad_cascade_df1_f32(&lowpass1_biceps, &filtered_biceps, &filtered_biceps, 1 );//first lowpass filter
+    arm_biquad_cascade_df1_f32(&highpass_biceps, &filtered_biceps, &filtered_biceps, 1 );//highpass filter
     filtered_biceps = fabs(filtered_biceps);                                                //Rectifier, The Gain is already implemented.
-    arm_biquad_cascade_df1_f32(&lowpass2_biceps, &filtered_biceps, &filtered_biceps, 1 );    //low pass filter
+    arm_biquad_cascade_df1_f32(&lowpass2_biceps, &filtered_biceps, &filtered_biceps, 1 );    //second low pass filter
 
     //Biquad process emg triceps
    arm_biquad_cascade_df1_f32(&bandstop_triceps, &emg_value2_f32, &filtered_triceps, 1 );    //used for wrist motion
@@ -202,16 +197,17 @@
     
 }
 
- 
+//y motor control
 void looper_motory()
 {
-
+//determine which direction the motor will rotate
     emg_y = (filtered_biceps - filtered_triceps);
-    
-    switch(proportional)
+    //switch between proportional EMG control and pre-set speed control
+    if(proportional==0)
     {
-        case 0:
-    //proportional control
+    //proportional control explained in the report
+    Stepy.write(0.5);
+    Ledr=0;//red light to make it clear which control method is used
     emg_y_abs = fabs(emg_y);
     force1 = emg_y_abs*K_Gain;
     force1 = force1 - damping1;
@@ -225,15 +221,14 @@
         //Speed limit
     if (speed1 > 1) {
         speed1 = 1;
-        step_freq1 = setpoint;
     }
-        break;
-    case 1:
+}
+    else{
     //precision control
+    Ledr=1;
     Stepy.period(0.000625);//frequency of 1600 Hz
-    break;
-    default:
     }
+    
     if (emg_y > 0) {//downward movement
         Diry = 0;
     }
@@ -241,22 +236,22 @@
         Diry = 1;
     }
 
-    //EMG treshold
+    //EMG treshold, determine if signal is strong enough to start the motors
     if (filtered_biceps < EMG_tresh1 && filtered_triceps < EMG_tresh2) {
         Enabley = 1; //Enable = 1 turns the motor off.
-    } else {
-        Enabley = 0;
+    }
+    else {
+        Enabley = 0;//Enable = 0 turns the motor on.
     }
 }
-
+//same setup as with the y motor
 void looper_motorx()
 {
-
     emg_x = (filtered_pect - filtered_deltoid);
-    switch(proportional)
+    if(proportional==0)
     {
-        case 0:
         //proportional control
+        Stepx.write(0.5);
         Ledr=0;
         emg_x_abs = fabs(emg_x);
         force2 = emg_x_abs*K_Gain;
@@ -271,21 +266,19 @@
         //speed limit
         if (speed2 > 1) {
             speed2 = 1;
-            step_freq2 = setpoint;
         }   
-        break;
-    
-        case 1:
+
+    }
+        else{
         //precision control 
         Ledr=1;
         Stepx.period(0.000625);//frequency of 1600 Hz
-        break;
-        default:
         }
+        
     if (emg_x > 0) {//left movement
         Dirx = 0;
     }
-    if (emg_x < 0) {//right movement
+    if (emg_x < 0) {//rig1ht movement
         Dirx = 1;
     }
 
@@ -304,8 +297,8 @@
 
 int main()
 {
-    pc.baud(115200);
-    Ledr=1;
+    pc.baud(115200);//sent data to pc for testing purposes
+    Ledr=1;//turn of all the lights
     Ledg=1;
     Ledb=1;
    
@@ -315,9 +308,9 @@
     MS12=1;
     MS22=1;
     MS32=1;
-Stepy.write(0.5f);
+Stepy.write(0.5);//set all motors to half load and disable them.
 Enabley.write(1);
-Stepx.write(0.5f);
+Stepx.write(0.5);
 Enablex.write(1);
 //Stepy.period(0.000625);//use period change for speed adjustments
 //Stepx.period(0.000625);//frequency of 1600 Hz
@@ -352,9 +345,8 @@
 else{
     Enabley.write(1);
 }
-        pc.printf("X value is %f and Y value is %f\n", X_control.read(), Y_control.read());
 }*/
-
+//initialization of the biquad filters and appointing of names to filters.
     //biceps
     arm_biquad_cascade_df1_init_f32(&lowpass1_biceps, 1 , lowpass1_const, lowpass1_biceps_states);
     arm_biquad_cascade_df1_init_f32(&lowpass2_biceps, 1 , lowpass2_const, lowpass2_biceps_states);
@@ -377,13 +369,13 @@
     arm_biquad_cascade_df1_init_f32(&highpass_deltoid, 1 , highpass_const, highpass_deltoid_states);
     arm_biquad_cascade_df1_init_f32(&bandstop_deltoid, 2 , bandstop_const, bandstop_deltoid_states);
 
-    emgtimer.attach(looper_emg, 0.01);
+    emgtimer.attach(looper_emg, 0.01);//emg signal filtering
 
-    looptimer1.attach(looper_motorx, 0.01); //X-Spindle motor
+    looptimer1.attach(looper_motorx, 0.01); //X motor control
 
-    looptimer2.attach(looper_motory, 0.01); //Y-Spindle motor
+    looptimer2.attach(looper_motory, 0.01); //Y motor control
 
-    while(1){
+    while(1){//if button is pressed the control mode changes with it.
         button1.fall(changecontrol);
         };
 }
\ No newline at end of file