Diff: main.cpp

Revision:

22:335a30b0825d

Parent:

21:a316452da8cd

Child:

23:b0222fa7c131

--- a/main.cpp	Mon Oct 21 13:23:11 2019 +0000
+++ b/main.cpp	Mon Oct 21 14:13:40 2019 +0000
@@ -17,8 +17,6 @@
 PwmOut motor1_pwm(D6);
 AnalogIn    emg0( A0 );
 AnalogIn    emg1( A1 );
-QEI motor1 (D8, D9, NC, 32);
-QEI motor2 (D12, D13, NC, 32);
 
 Ticker ticktick;
 Timer state_time;
@@ -71,7 +69,7 @@
 float u_1;
 float u_2;
 const float angle2_offset=asin(0.2);
-const float angle1_offset=asin(3.8/35.0);
+const float angle1_offset=atan(2.2/32.8);
 const double pi=3.1415926535897932384626;
 volatile float theta1;
 volatile float theta2;
@@ -143,15 +141,13 @@
     if(!motor1_calibrated&&t>1.0f)
     {
         dir1=1; //???
-        motor1_pwm.write(0.8f);
+        motor1_pwm.write(0.75f);
         pulses=motor1_pos.getPulses();
         if(pos_time_counter%500==0&&fabs(pulses-last_ticks)<1)
         {
             ledblue=!ledblue;
             motor1_pos.reset();
             last_ticks=10000;
-            state_time.reset();
-            dir1=!dir1;
         }
         else if(pos_time_counter%500==0)
         {
@@ -159,11 +155,16 @@
         }
         
     }
-    else if(t>1.0f)
+    else if(!motor1_calibrated)
+    {
+        motor2_pwm.write(0.75f);
+        dir2=1;
+    }
+    else if(t>0.5f)
     {
         motor1_pwm.write(0.0f);
         dir2=1; //???
-        motor2_pwm.write(0.8f);
+        motor2_pwm.write(0.75f);
         pulses=motor2_pos.getPulses();
         if(pos_time_counter%500==0&&fabs(pulses-last_ticks)<1)
         {
@@ -341,16 +342,16 @@
     u_2 = PID2(error_2);
     
     if(u_1 < 0.0f){
-        dir1 = 0;
+        dir1 = 1;
     }else{
-        dir1 = 1;
+        dir1 = 0;
         }
     motor1_pwm.write(fabs(u_1));
     
     if(u_2 < 0.0f){
-        dir2 = 0;
+        dir2 = 1;
     }else{
-        dir2 = 1;
+        dir2 = 0;
         }
     motor2_pwm.write(fabs(u_1));
     
@@ -440,6 +441,8 @@
             if(!motor1_calibrated)
             {
                 motor1_calibrated=true;
+                state_time.reset();
+                dir1=!dir1;
                 wait(1);
             }
             else