Diff: main.cpp

Revision:

38:0f824c45f717

Parent:

37:5ddfd9e6cdb2

Child:

42:bcd496523c66

--- a/main.cpp	Wed Oct 31 13:00:49 2018 +0000
+++ b/main.cpp	Wed Oct 31 15:33:20 2018 +0000
@@ -14,8 +14,8 @@
 // motor ouptuts
 PwmOut      motor1_pwm(D5);
 DigitalOut  motor1_dir(D4);
-PwmOut      motor2_pwm(D7);
-DigitalOut  motor2_dir(D6);
+PwmOut      motor2_pwm(D6);
+DigitalOut  motor2_dir(D7);
 
 // defining encoders
 QEI motor_1_encoder(D12,D13,NC,32);
@@ -49,7 +49,7 @@
 calib_enc_states calib_enc_state = not_in_calib_enc;
 
 double des_vx, des_vy, x, y, q1, q2, qref1, qref2, e1, e2; //will be set by the motor_controller function
-double u1 = 0, u2= 0;
+double u1, u2;
 double vxmax = 1.0, vymax = 1.0;
 double right_bicep_max = 0.0, right_tricep_max = 0.0, left_bicep_max= 0.0, left_tricep_max = 0.0;
 
@@ -135,8 +135,8 @@
                 led_G = 0;
                 led_B = 1;
                 led_R = 1;
-                u1 = 0;
-                u2 = -0.55f;
+                u1 = 0.2;
+                u2 = -0.65f;
                 q1old = 0; 
                 q2old = 0;
             }
@@ -147,14 +147,15 @@
                     {
                         calib_enc_state = calib_enc_q1;
                         off_set_q2 = calib_q2 - q2;
-                        u2 = 0;
-                        u1 = -0.55f;
+                        u2 = -0.4;
+                        u1 = -0.75;
                         state_timer.reset();
                         state_timer.start();
                     }    
                     q2old = q2;
                     break;
                 case calib_enc_q1:
+                    if (state_timer.read() > 1.0f){u1=0.55;}
                     if (q1 - q1old == 0.0 && state_timer.read() > 5.0f)
                     {
                         calib_enc_state = not_in_calib_enc;
@@ -166,7 +167,7 @@
                         state_timer.reset();
                         state_timer.start();
                     }    
-                    q2old = q2;
+                    q1old = q1;
                     break;
                 default:
                     current_state = failure;