Diff: main.cpp

Revision:

16:1be144329f05

Parent:

15:849e0fc5d3a8

Child:

17:f87e5d6c87f4

--- a/main.cpp	Tue Oct 29 20:21:54 2019 +0000
+++ b/main.cpp	Tue Oct 29 20:34:49 2019 +0000
@@ -286,13 +286,20 @@
     void motor1_controller(void)
     {
         error1_motor1 = (Yref_motor1 - positie_motor1);
-        motor1_pwm();
+        if (motor1_calibrated==true&&motor2_calibrated==true)
+            {
+                motor1_pwm();
+            }
+        
     }
 
     void motor2_controller(void)
     {
         error1_motor2 = (Yref_motor2 - positie_motor2);
-        motor2_pwm();
+        if (motor1_calibrated==true&&motor2_calibrated==true)
+            {
+                motor2_pwm();
+            }
     }    
     
     // 4.3 State-Machine *******************************************************
@@ -327,35 +334,28 @@
             led_red.write(1);
             led_green.write(1);
             
-            Yref_motor1=5000;
-            Yref_motor2=2000;
-            if(button1==0) {State=EMGCalibration;}
-            if(button2==0) {State=Demo;} 
-            break;             
+            State=EMGCalibration;
+            break;  
+                       
         case EMGCalibration: 
    //         pc.printf("\r\n State: EMGCalibration");
             led_blue.write(1);
             led_red.write(1);
             led_green.write(0);
 
+            Yref_motor1=5000;
+            Yref_motor2=2000;
             State=Homing; 
             break; 
         case Homing: 
        //     pc.printf("\r\n State: Homing");
-            
+            led_green.write(1);
             led2=1;
-            motor1_controller();
-            motor2_controller();
-            // State=Operating; 
+            if(button1==0) {State=Operating;}
+            if(button2==0) {State=Demo;} 
+
             break; 
-        case Demo:
-           pc.printf("\r\n State: Demo");
-            led_blue.write(1);
-            led_red.write(1);
-            led_green.write(0); 
-
-            
-            break;  
+  
         case Operating:
   /*          pc.printf("\r\n State: Operating");
             led_blue.write(1);
@@ -365,6 +365,16 @@
             led_green.write(1);
             wait(0.5); */
             break; 
+        
+        case Demo:
+           pc.printf("\r\n State: Demo");
+            led_blue.write(1);
+            led_red.write(1);
+            led_green.write(0); 
+
+            
+            break;
+            
         case EmergencyMode:
             pc.printf("\r\n State: EMERGENCY MODE! Press RESET to restart");
             
@@ -403,6 +413,9 @@
 fencoder_motor2() ;
 PID_controller_motor1(error_integral_motor1, error1_prev_motor1);
 PID_controller_motor2(error_integral_motor2, error1_prev_motor2);
+motor1_controller();
+motor2_controller();
+
 state_machine() ;
 
 // 5.1 Measure Analog and Digital input signals ********************************
@@ -434,7 +447,7 @@
     ticker_hidscope.attach(&HIDScope, 0.001); //Ticker for Hidscope, different frequency compared to motors
     ticker_mainloop.attach(&main_loop,0.001); // change back to 0.001f //Run the function main_loop 1000 times per second       
     
-    //motor_calibration();
+    motor_calibration();
     
     
 // 6.2 While loop in main function**********************************************