Diff: main.cpp

Revision:

7:676a83def149

Parent:

6:e7e39d116ed0

Child:

8:3cfc8be293d3

diff -r e7e39d116ed0 -r 676a83def149 main.cpp
--- a/main.cpp	Tue Oct 29 16:11:21 2019 +0000
+++ b/main.cpp	Tue Oct 29 16:35:45 2019 +0000
@@ -25,6 +25,8 @@
 float Ts = 0.01;                                //Sample time
 float motor1angle;                              //Measured angle motor 1
 float motor2angle;                              //Measured angle motor 2
+float motor1offset;                             //Offset bij calibratie
+float motor2offset;
 float potmeter;
 float omega1;                                   //velocity rad/s motor 1
 float omega2;                                   //Velocity rad/s motor2
@@ -83,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_cal_manual , 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;
@@ -243,15 +245,16 @@
         motor_state_changed = false;
         // More functions
     }
-
-    // Do stuff until end condition is met 
-    
-    
+    motor1Power.write(0.0f);
+    motor2Power.write(0.0f);
+    motor1offset = (counts1 * factorin / gearratio);
+    motor2offset = (counts2 * factorin / gearratio);
+      
     // State transition guard
-    if ( omega2 <= 0.5f ) {         
-        motor_curr_state = motor_encoderset;
+if ( button2_pressed ) {   
+        button2_pressed = false;        
+        motor_curr_state = motor_wait;
         motor_state_changed = true;
-        // More functions
     }        
 }
 
@@ -273,32 +276,13 @@
     }     
 else if ( button2_pressed ) {   
         button2_pressed = false;      
-        motor_curr_state = motor_cal_manual;           //Beginnen met calibratie
+        motor_curr_state = motor_encoderset;           //Beginnen met calibratie
         motor_state_changed = true;
         // More functions
     }    
    
 }    
 
-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) {
@@ -320,9 +304,6 @@
         case motor_encoderset:
             do_motor_Encoder_Set();
             break;
-        case motor_cal_manual:
-            do_motor_cal_manual();
-            break;
     }
 }
 
@@ -353,7 +334,8 @@
     
     while (true) {
             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);
+            pc.printf("Currentstate: %i\r\n",motor_curr_state);
+            pc.printf("motor1offset: %f motor2offset: %f\r\n",motor1offset,motor2offset);
         wait(0.5);
     }
 }