Diff: main.cpp

Revision:

8:3cfc8be293d3

Parent:

7:676a83def149

Child:

9:298469a70280

--- a/main.cpp	Tue Oct 29 16:35:45 2019 +0000
+++ b/main.cpp	Tue Oct 29 16:42:46 2019 +0000
@@ -85,7 +85,7 @@
 HIDScope scope(6);                              //Going to send x channels of data. To access data go to 'http:/localhost:18082/' after starting HIDScope application.
 
 //State maschine
-enum Motor_States{motor_wait , motor_cal1_start , motor_cal1 , motor_cal2_start , motor_cal2 , motor_encoderset};
+enum Motor_States{motor_wait , motor_encoderset};
 Motor_States motor_curr_state;
 bool motor_state_changed = true;
 bool motor_cal1_done = false;
@@ -139,105 +139,24 @@
     countsPrev2 = counts2;    
 }
 
-void do_motor_cal1_start(){
-    // Entry function
-    if ( motor_state_changed == true ) {
-        motor_state_changed = false;
-        // More functions
-    }
-    
-    //Do stuff    
-    motor1angle = (counts1 * factorin / gearratio); // Angle of motor shaft in rad
-    omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
-    controlsignal1=0.10f;
-    motor1Power.write(abs(controlsignal1));
-    motor1Direction=0;
-
-    // State transition guard
-    if ( abs(omega1) >= 1.5f ){
-        motor_curr_state = motor_cal1;
-        motor_state_changed = true;
-        // More functions
-    }
-    
-}
-
-void do_motorCalibration1() {
+void do_motor_wait(){
     // Entry function
     if ( motor_state_changed == true ) {
         motor_state_changed = false;
         // More functions
     }
 
-    // Do stuff until end condition is met
-    motor1angle = (counts1 * factorin / gearratio); // Angle of motor shaft in rad
-    omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
-    controlsignal1=0.0980f;
-    motor1Power.write(abs(controlsignal1));
-    motor1Direction=0;
-
-    // State transition guard
-    if ( abs(omega1) <= 0.5f ) {
-        motor_curr_state = motor_cal2_start;
-        motor_state_changed = true;
-        // More functions
-    }
-}
-
-void do_motor_cal2_start(){
-    // Entry function
-    if ( motor_state_changed == true ) {
-        motor_state_changed = false;
-        // More functions
-    }    
-    //stuff
-    motor1angle = (counts1 * factorin / gearratio); // Angle of motor shaft in rad
-    omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
-    controlsignal1=0.10f;
-    motor1Power.write(abs(controlsignal1));
-    motor1Direction=0;
+    // Do nothing until end condition is met 
     
-    motor2angle = (counts2 * factorin / gearratio); // Angle of motor shaft in rad
-    omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
-    controlsignal2=0.10f;
-    motor2Power.write(abs(controlsignal2));
-    motor2Direction=0;
-    
-    // State transition guard
-    if ( abs(omega2) >= 1.5f ) {
-        motor_curr_state = motor_cal2;
-        motor_state_changed = true;
-        // More functions
-    }   
-}
-
-void do_motorCalibration2(){
-    // Entry function
-    if ( motor_state_changed == true ) {
-        motor_state_changed = false;
-        // More functions
-    }
-
-    // Do stuff until end condition is met    
-    motor1angle = (counts1 * factorin / gearratio); // Angle of motor shaft in rad
-    omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
-    controlsignal1=0.10f;
-    motor1Power.write(abs(controlsignal1));
-    motor1Direction=0;
-    
-    motor2angle = (counts2 * factorin / gearratio); // Angle of motor shaft in rad
-    omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
-    controlsignal2=0.10f;
-    motor2Power.write(abs(controlsignal2));
-    motor2Direction=0; 
-    
-    // State transition guard
-    if ( abs(omega2) <= 0.5f ) {
-        motor_curr_state = motor_encoderset;
+    // State transition guard  
+if ( button2_pressed ) {   
+        button2_pressed = false;      
+        motor_curr_state = motor_encoderset;           //Beginnen met calibratie
         motor_state_changed = true;
         // More functions
     }    
-}
+   
+}    
 
 void do_motor_Encoder_Set(){
     // Entry function
@@ -258,49 +177,12 @@
     }        
 }
 
-void do_motor_wait(){
-    // Entry function
-    if ( motor_state_changed == true ) {
-        motor_state_changed = false;
-        // More functions
-    }
-
-    // Do nothing until end condition is met 
-    
-    // State transition guard
-if ( button1_pressed ) {   
-        button1_pressed = false;      
-        motor_curr_state = motor_cal1_start;           //Beginnen met calibratie
-        motor_state_changed = true;
-        // More functions
-    }     
-else if ( button2_pressed ) {   
-        button2_pressed = false;      
-        motor_curr_state = motor_encoderset;           //Beginnen met calibratie
-        motor_state_changed = true;
-        // More functions
-    }    
-   
-}    
-
 void motor_state_machine()
 {
     switch(motor_curr_state) {
         case motor_wait:
             do_motor_wait();
             break;
-        case motor_cal1_start:
-            do_motor_cal1_start();
-            break;
-        case motor_cal1:
-            do_motorCalibration1();
-            break;
-        case motor_cal2_start:
-            do_motor_cal2_start();    
-            break;
-        case motor_cal2:
-            do_motorCalibration2();
-            break;
         case motor_encoderset:
             do_motor_Encoder_Set();
             break;