Diff: main.cpp

Revision:

5:4d8b85b7cfc4

Parent:

4:1e8da6b5f147

Child:

6:e7e39d116ed0

--- a/main.cpp	Tue Oct 29 15:32:04 2019 +0000
+++ b/main.cpp	Tue Oct 29 15:57:06 2019 +0000
@@ -168,7 +168,6 @@
     // 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
-    float potmeter=potmeter1.read();
     controlsignal1=0.0980f;
     motor1Power.write(abs(controlsignal1*motortoggle));
     motor1Direction=0;
@@ -181,7 +180,32 @@
     }
 }
 
-void do_motor_cal2_start()
+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*motortoggle));
+    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*motortoggle));
+    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
@@ -191,22 +215,20 @@
     }
 
     // Do stuff until end condition is met    
-    potmeter=potmeter1.read();
     motor1angle = (counts1 * factorin / gearratio); // Angle of motor shaft in rad
     omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
-    controlsignal1=potmeter;
+    controlsignal1=0.10f;
     motor1Power.write(abs(controlsignal1*motortoggle));
-    motor1Direction=0;    
+    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=potmeter;
+    omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
+    controlsignal2=0.10f;
     motor2Power.write(abs(controlsignal2*motortoggle));
-    motor2Direction=1;  
-    //Dit moet je doen zolang omega van motor 2 > 0.iets  
+    motor2Direction=0; 
     
     // State transition guard
-    if ( omega2 <= 0.5f ) {
+    if ( abs(omega2) <= 0.5f ) {
         motor_curr_state = motor_encoderset;
         motor_state_changed = true;
         // More functions
@@ -303,7 +325,6 @@
     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);