Diff: main.cpp

Revision:

13:0e25698dce40

Parent:

12:99e29b8d4155

Child:

14:fb5d8064830d

--- a/main.cpp	Thu Nov 01 17:51:37 2018 +0000
+++ b/main.cpp	Thu Nov 01 18:56:23 2018 +0000
@@ -196,7 +196,7 @@
             led1=!led1;
             if(emg1_filtered>temp_highest_emg1) {
                 temp_highest_emg1= emg1_filtered;
-                pc.printf("Temp1 = %f \r\n",temp_highest_emg1);
+                //pc.printf("Temp1 = %f \r\n",temp_highest_emg1);
             }
         }
         if(timer_calibration>10 && timer_calibration<15) {
@@ -208,7 +208,7 @@
             led2=!led2;
             if(emg2_filtered>temp_highest_emg2) {
                 temp_highest_emg2= emg2_filtered;
-                pc.printf("Temp2 = %f \r\n",temp_highest_emg2);
+                //pc.printf("Temp2 = %f \r\n",temp_highest_emg2);
             }
         }
         if(timer_calibration>25 && timer_calibration<30) {
@@ -220,7 +220,7 @@
             led3=!led3;
             if(emg3_filtered>temp_highest_emg3) {
                 temp_highest_emg3= emg3_filtered;
-                pc.printf("Temp3 = %f \r\n",temp_highest_emg3);
+                //pc.printf("Temp3 = %f \r\n",temp_highest_emg3);
             }
         }
         if(timer_calibration>40 && timer_calibration<45) {
@@ -233,7 +233,7 @@
             led3=!led3;
             if(emg4_filtered>temp_highest_emg4) {
                 temp_highest_emg4= emg4_filtered;
-                pc.printf("Temp4 = %f \r\n",temp_highest_emg4);
+                //pc.printf("Temp4 = %f \r\n",temp_highest_emg4);
             }
         }
         led1=1;
@@ -290,17 +290,7 @@
     */
 }
 
-//Activate ticker for Movement state, filtering and Threshold checking
-void movement_ticker_activator()
-{
-    sample_ticker.attach(&emgsample, ts);
-    threshold_check_ticker.attach(&threshold_check, ts);
-}
-void movement_ticker_deactivator()
-{
-    sample_ticker.detach();
-    threshold_check_ticker.detach();
-}
+
 
 // ~~~~~~~~~~~~~~ROBOT KINEMATICS ~~~~~~~~~~~~~~~~~~
 
@@ -450,6 +440,9 @@
 // CONTROLLING THE MOTOR
 void Motor_mover()
 {
+    float px = positionx(bicepsR,bicepsL);  // EMG: +x, -x
+   float py = positiony(tricepsL,tricepsL);  // EMG: +y, -y
+        
     double motor_position = encoder1.getPulses(); //output in counts
     double reference_rotation = hoek1(px, py);
     double error = reference_rotation - motor_position*(2*PI)/8400;
@@ -471,7 +464,22 @@
 
 }
 
-
+//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);
+}
+void movement_ticker_deactivator()
+{
+    sample_ticker.detach();
+    //sample_ticker_ticker.detach();
+}
 
 //-------------------- STATE MACHINE --------------------------
 enum states {MOTORS_OFF,CALIBRATION,HOMING,DEMO,MOVEMENT,CLICK};
@@ -480,6 +488,7 @@
 
 void ProcessStateMachine(void)
 {
+    
     switch (currentState) {
         case MOTORS_OFF:
             // Actions
@@ -518,9 +527,8 @@
                 timer_calibration.reset();
                 timer_calibration.start();
 
-                sample_ticker.attach(&emgsample, ts);
+
                 CalibrationEMG();
-                sample_ticker.detach();
                 timer_calibration.stop();
 
 
@@ -572,7 +580,7 @@
                 led1 = 0;
                 led2 = 1;
                 led3 = 0;
-                wait (1);
+                //wait (1);
 
                 stateChanged = false;
             }
@@ -697,6 +705,7 @@
 
 int main()
 {
+   
     //BiQuad Chain add
     highp1.add( &highp1_1 ).add( &highp1_2 );
     notch1.add( &notch1_1 ).add( &notch1_2 );
@@ -720,11 +729,13 @@
     led3 = 1;
     
     pwmpin1.period_us(60); // setup motor
-    ref_rot.attach(Motor_mover, 0.01f);// HAS TO GO TO STATE MACHINE
-    movement_ticker_activator();
+    //ref_rot.attach(Motor_mover, 0.01f);// HAS TO GO TO STATE MACHINE
+    //movement_ticker_activator();
+    emg_sample_ticker();
     while (true) {
-        //ProcessStateMachine();
-        
+        ProcessStateMachine();
+
+/*      
         if (button2 == false) {
             wait(0.01f);