Diff: main.cpp

Revision:

4:1e8da6b5f147

Parent:

3:6e28b992b99e

Child:

5:4d8b85b7cfc4

--- a/main.cpp	Tue Oct 29 14:04:26 2019 +0000
+++ b/main.cpp	Tue Oct 29 15:32:04 2019 +0000
@@ -26,7 +26,7 @@
 float motor1angle;                              //Measured angle motor 1
 float motor2angle;                              //Measured angle motor 2
 float potmeter;
-float omega1=1;                                   //velocity rad/s motor 1
+float omega1;                                   //velocity rad/s motor 1
 float omega2;                                   //Velocity rad/s motor2
 float deg2rad=0.0174532;                        //Conversion factor degree to rad
 float rad2deg=57.29578;                         //Conversion factor rad to degree
@@ -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 , motor_cal2 , motor_encoderset};
+enum Motor_States{motor_wait , motor_cal1_start , motor_cal1 , motor_cal2_start , motor_cal2 , motor_encoderset};
 Motor_States motor_curr_state;
 bool motor_state_changed = true;
 bool motor_cal1_done = false;
@@ -135,6 +135,29 @@
     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*motortoggle));
+    motor1Direction=0;
+
+    // State transition guard
+    if ( abs(omega1) >= 1.5f ){
+        motor_curr_state = motor_cal1;
+        motor_state_changed = true;
+        // More functions
+    }
+    
+}
+
 void do_motorCalibration1() {
     // Entry function
     if ( motor_state_changed == true ) {
@@ -151,13 +174,15 @@
     motor1Direction=0;
 
     // State transition guard
-    if ( omega1 <= 0.5f ) {
-        motor_curr_state = motor_cal2;
+    if ( abs(omega1) <= 0.5f ) {
+        motor_curr_state = motor_cal2_start;
         motor_state_changed = true;
         // More functions
     }
 }
 
+void do_motor_cal2_start()
+
 void do_motorCalibration2(){
     // Entry function
     if ( motor_state_changed == true ) {
@@ -218,7 +243,7 @@
     // State transition guard
     if ( button1_pressed ) {   
         button1_pressed = false;      
-        motor_curr_state = motor_cal1;           //Beginnen met calibratie
+        motor_curr_state = motor_cal1_start;           //Beginnen met calibratie
         motor_state_changed = true;
         // More functions
     }        
@@ -235,9 +260,15 @@
         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;
@@ -253,6 +284,7 @@
     //sampleSignal();
     //emg_state_machine();
     motor_state_machine();
+    readEncoder();
     // controller();
     // outputToMotors();
 }
@@ -268,11 +300,12 @@
     motor_curr_state = motor_wait;                          // Start off in EMG Wait state
     tickGlobal.attach( &tickGlobalFunc, Ts );
     
-    button2.fall(&toggleMotor);
+    button1.fall(&button1Press);
     
     while (true) {
             potmeter=potmeter1.read();
             pc.printf("Omega1: %f Omega 2: %f  controlsignal1: %f \r\n", omega1, omega2, controlsignal1);
+            pc.printf("Currentstate: %i motor_cal1: %i\r\n",motor_curr_state, motor_cal1);
         wait(0.5);
     }
 }