Diff: main.cpp

Revision:

14:fb5d8064830d

Parent:

13:0e25698dce40

Child:

15:6ad7abc5c691

--- a/main.cpp	Thu Nov 01 18:56:23 2018 +0000
+++ b/main.cpp	Thu Nov 01 19:34:05 2018 +0000
@@ -186,6 +186,9 @@
     emg2_filtered = lowp2.step(emg2_abs);
     emg3_filtered = lowp3.step(emg3_abs);
     emg4_filtered = lowp4.step(emg4_abs);
+    
+
+
 }
 
 void CalibrationEMG()
@@ -239,13 +242,15 @@
         led1=1;
         led2=1;
         led3=1;
+
+
     }
-/*
-    pc.printf("Highest value right biceps= %f \r\n", temp_highest_emg1);
-    pc.printf("Highest value right triceps= %f \r\n", temp_highest_emg2);
-    pc.printf("Highest value left biceps= %f \r\n", temp_highest_emg3);
-    pc.printf("Highest value left triceps= %f \r\n", temp_highest_emg4);
-*/
+
+    //pc.printf("Highest value right biceps= %f \r\n", temp_highest_emg1);
+    //pc.printf("Highest value right triceps= %f \r\n", temp_highest_emg2);
+    //pc.printf("Highest value left biceps= %f \r\n", temp_highest_emg3);
+    //pc.printf("Highest value left triceps= %f \r\n", temp_highest_emg4);
+
 
     threshold1 = temp_highest_emg1*biceps_p_t;  //Right Biceps
     threshold2 = temp_highest_emg2*triceps_p_t; //Right Triceps
@@ -288,10 +293,11 @@
     pc.printf("Biceps Left = %i", bicepsL);
     pc.printf("Triceps Left = %i", tricepsL);
     */
+
+
 }
 
 
-
 // ~~~~~~~~~~~~~~ROBOT KINEMATICS ~~~~~~~~~~~~~~~~~~
 
 // functie x positie
@@ -441,7 +447,7 @@
 void Motor_mover()
 {
     float px = positionx(bicepsR,bicepsL);  // EMG: +x, -x
-   float py = positiony(tricepsL,tricepsL);  // EMG: +y, -y
+   float py = positiony(tricepsR,tricepsL);  // EMG: +y, -y
         
     double motor_position = encoder1.getPulses(); //output in counts
     double reference_rotation = hoek1(px, py);
@@ -460,27 +466,21 @@
     double error_3 = reference_rotation3 - motor_position3*(2*PI)/8400;
     double u_3 = PID_controller(error_3);
     moter3_control(u_3);
-
-
 }
 
 //Activate ticker for Movement state, filtering and Threshold checking
-void emg_sample_ticker(){
-     sample_ticker.attach(&emgsample, ts);
-     //sample_ticker.attach(&Motor_mover, ts);
-     
-     }
 void movement_ticker_activator()
 {
     sample_ticker.attach(&emgsample, ts);
-    sample_ticker.attach(&threshold_check, ts);
+    threshold_check_ticker.attach(&threshold_check, ts);
 }
 void movement_ticker_deactivator()
 {
     sample_ticker.detach();
-    //sample_ticker_ticker.detach();
+    threshold_check_ticker.detach();
 }
 
+
 //-------------------- STATE MACHINE --------------------------
 enum states {MOTORS_OFF,CALIBRATION,HOMING,DEMO,MOVEMENT,CLICK};
 states currentState = MOTORS_OFF; //Chosen startingposition for states
@@ -488,7 +488,6 @@
 
 void ProcessStateMachine(void)
 {
-    
     switch (currentState) {
         case MOTORS_OFF:
             // Actions
@@ -527,8 +526,9 @@
                 timer_calibration.reset();
                 timer_calibration.start();
 
-
+                sample_ticker.attach(&emgsample, ts); 
                 CalibrationEMG();
+                sample_ticker.detach();
                 timer_calibration.stop();
 
 
@@ -580,7 +580,7 @@
                 led1 = 0;
                 led2 = 1;
                 led3 = 0;
-                //wait (1);
+                wait (1);
 
                 stateChanged = false;
             }
@@ -637,7 +637,7 @@
                 stateChanged = true;
             }
             // here ends the idle checking mode
-            else {
+            else{
                 //For every muscle a different colour if threshold is passed
                 if(bicepsR==1) {
                     led1 = 0;
@@ -675,6 +675,8 @@
                     led2 = 1;
                     led3 = 1;
                 }
+                //currentState = MOVEMENT  ;
+                //stateChanged = false;
             }
 
             break;
@@ -729,9 +731,9 @@
     led3 = 1;
     
     pwmpin1.period_us(60); // setup motor
-    //ref_rot.attach(Motor_mover, 0.01f);// HAS TO GO TO STATE MACHINE
+    ref_rot.attach(Motor_mover, 0.01f);// HAS TO GO TO STATE MACHINE
     //movement_ticker_activator();
-    emg_sample_ticker();
+    //emg_sample_ticker();
     while (true) {
         ProcessStateMachine();