Diff: main.cpp

Revision:

6:e7e39d116ed0

Parent:

5:4d8b85b7cfc4

Child:

7:676a83def149

--- a/main.cpp	Tue Oct 29 15:57:06 2019 +0000
+++ b/main.cpp	Tue Oct 29 16:11:21 2019 +0000
@@ -38,8 +38,6 @@
 DigitalOut motor1Direction(D7);
 FastPWM motor1Power(D6);
 
-volatile int motortoggle = 1;                   //Toggle to stop motors
-
 //Motorcontrol
 bool motordir1;
 bool motordir2;
@@ -85,7 +83,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_cal1_start , motor_cal1 , motor_cal2_start , motor_cal2 , motor_cal_manual , motor_encoderset};
 Motor_States motor_curr_state;
 bool motor_state_changed = true;
 bool motor_cal1_done = false;
@@ -122,6 +120,10 @@
     button1_pressed = true;
 }
 
+void button2Press()
+{
+    button2_pressed = true;
+}
 
 // Ticker Functions
 void readEncoder()
@@ -146,7 +148,7 @@
     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));
+    motor1Power.write(abs(controlsignal1));
     motor1Direction=0;
 
     // State transition guard
@@ -169,7 +171,7 @@
     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*motortoggle));
+    motor1Power.write(abs(controlsignal1));
     motor1Direction=0;
 
     // State transition guard
@@ -190,13 +192,13 @@
     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));
+    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*motortoggle));
+    motor2Power.write(abs(controlsignal2));
     motor2Direction=0;
     
     // State transition guard
@@ -218,13 +220,13 @@
     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));
+    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*motortoggle));
+    motor2Power.write(abs(controlsignal2));
     motor2Direction=0; 
     
     // State transition guard
@@ -263,19 +265,40 @@
     // Do nothing until end condition is met 
     
     // State transition guard
-    if ( button1_pressed ) {   
+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_cal_manual;           //Beginnen met calibratie
+        motor_state_changed = true;
+        // More functions
+    }    
+   
 }    
 
-void toggleMotor()
-{
-    motortoggle = !motortoggle;
+void do_motor_cal_manual(){
+    // Entry function
+    if ( motor_state_changed == true ) {
+        motor_state_changed = false;
+        // More functions
+    }
+    
+    //Op dit moment moet je de robotarm handmatig naar zijn beginpositie brengen,
+    //en op het knopje klikken om door te gaan
+        
+if ( button2_pressed ) {   
+        button2_pressed = false;      
+        motor_curr_state = motor_encoderset;          
+        motor_state_changed = true;
+        // More functions
+    }    
 }
 
+
 void motor_state_machine()
 {
     switch(motor_curr_state) {
@@ -297,6 +320,9 @@
         case motor_encoderset:
             do_motor_Encoder_Set();
             break;
+        case motor_cal_manual:
+            do_motor_cal_manual();
+            break;
     }
 }
 
@@ -323,6 +349,7 @@
     tickGlobal.attach( &tickGlobalFunc, Ts );
     
     button1.fall(&button1Press);
+    button2.fall(&button2Press);
     
     while (true) {
             pc.printf("Omega1: %f Omega 2: %f  controlsignal1: %f \r\n", omega1, omega2, controlsignal1);