Diff: main.cpp

Revision:

11:c7e27de26ac0

Parent:

10:91173ed1e841

Child:

12:c59b25d07bb9

--- a/main.cpp	Thu Nov 01 16:50:47 2018 +0000
+++ b/main.cpp	Thu Nov 01 17:59:25 2018 +0000
@@ -57,11 +57,22 @@
     float filtered_d_error1;
     float filtered_d_error2;
     
+    float kp1 = 0.1;
+    float kp2 = 0.1;
+    float ki1 = 0.05;
+    float ki2 = 0.05;
+    float kd1 = 0.005;
+    float kd2 = 0.005;
+    float olderror1;
+    float olderror2;
+    float d_error1;
+    float d_error2;
+    
 enum States {failure, waiting, calib_motor, homing ,calib_emg, operational, demo};
 enum Operations {rest, forward, backward, up, down};
 
 States current_state = calib_motor;
-Operations movement = forward;
+Operations movement = rest;
 
 float max1 = 0; //initial threshold value for emg signals, changes during calibration left arm
 float max2 = 0; // right arm
@@ -155,23 +166,65 @@
     
     int sign1 = 0.5;
     int sign2 = 1;
+    
 void do_forward(){
     
-    
+    sign1 = 0.5;
+    sign2 = 1;
     twistf[0] = 1;
     twistf[1] = 0;
     
-    if (but1 == false){
-        //abs_sig = (filteredsignal2 - (0.3*max2))/(0.7*max2);       
-        abs_sig = 0.4;
-        
+    if (filteredsignal2 > (0.3*max2)){
+        abs_sig = (filteredsignal2 - (0.3*max2))/(0.7*max2)* gain;                    
         }
     else
-    {
-        abs_sig = 0;
+    {    abs_sig = 0;
+        }    
+    twist[0] = twistf[0] * abs_sig ;
+    twist[1] = twistf[1] * abs_sig ;
+             
+    motor1_speed_control = fabs(u1);
+    
+    if(u1 > 0){
+        motor1_direction = 1;}
+    if(u1 < 0){
+        motor1_direction = 0;}
+
+    motor2_speed_control = fabs(u2);
+    
+    if(u2 > 0){
+        motor2_direction = 1;}
+    if(u2 < 0){
+        motor2_direction = 0;} 
+    
+    if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
+        {
+          movement = backward;
+           timer.reset();
+        
+        d_error1 = 0;
+        d_error2 = 0;
+        u1 = 0;
+        u2 = 0;
+        int_u1 = 0;
+        int_u2 = 0;
+        led_green = 0;
+           }
+    }
+
+void do_backward(){
+  
+    sign1 = -1;
+    sign2 = 1;
+    twistf[0] = -1;
+    twistf[1] = 0;
+    
+    if (filteredsignal2 > (0.3*max2)){
+        abs_sig = (filteredsignal2 - (0.3*max2))/(0.7*max2)* gain;                    
         }
-    
-    
+    else
+    {    abs_sig = 0;
+        }    
     twist[0] = twistf[0] * abs_sig ;
     twist[1] = twistf[1] * abs_sig ;
              
@@ -189,69 +242,126 @@
     if(u2 < 0){
         motor2_direction = 0;} 
     
-    //if( timer.read() > thresholdtime && filteredsignal1 > threshold1*100)
-       // {
-       //     movement = backward;
-        //    timer.reset();
-        //    }
-    }
-
-void do_backward(){
-  
-    
-    
      if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
         {
-            movement = up;
-            timer.reset();
+        movement = up;
+        timer.reset();
+       
+        d_error1 = 0;
+        d_error2 = 0;
+        u1 = 0;
+        u2 = 0;
+        int_u1 = 0;
+        int_u2 = 0;
+        led_red = 1;
             }
     }
 
 void do_up(){
     
-    //Code for moving up
+    sign1 = 1;
+    sign2 = 1;
+    twistf[0] = 0;
+    twistf[1] = 1;
+    
+    if (filteredsignal2 > (0.3*max2)){
+        abs_sig = (filteredsignal2 - (0.3*max2))/(0.7*max2)* gain;                    
+        }
+    else
+    {    abs_sig = 0;
+        }    
+    twist[0] = twistf[0] * abs_sig ;
+    twist[1] = twistf[1] * abs_sig ;
+             
+    motor1_speed_control = fabs(u1);
+    
+    if(u1 > 0){
+        motor1_direction = 1;}
+    if(u1 < 0){
+        motor1_direction = 0;}
+
+    motor2_speed_control = fabs(u2);
+    
+    if(u2 > 0){
+        motor2_direction = 1;}
+    if(u2 < 0){
+        motor2_direction = 0;}
     
      if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
         {
-            movement = down;
-            timer.reset();
+        movement = down;
+        timer.reset();
+       
+        d_error1 = 0;
+        d_error2 = 0;
+        u1 = 0;
+        u2 = 0;
+        int_u1 = 0;
+        int_u2 = 0;
+        led_green = 1;
+        led_blue = 0;
             }
     }
 void do_down(){
     
-    //Code for moving down
+    sign1 = 1;
+    sign2 = -1;
+    twistf[0] = 0;
+    twistf[1] = -1;
+    
+    if (filteredsignal2 > (0.3*max2)){
+        abs_sig = (filteredsignal2 - (0.3*max2))/(0.7*max2)* gain;                    
+        }
+    else
+    {    abs_sig = 0;
+        }    
+    twist[0] = twistf[0] * abs_sig ;
+    twist[1] = twistf[1] * abs_sig ;
+             
+    motor1_speed_control = fabs(u1);
+    
+    if(u1 > 0){
+        motor1_direction = 1;}
+    if(u1 < 0){
+        motor1_direction = 0;}
+
+    motor2_speed_control = fabs(u2);
+    
+    if(u2 > 0){
+        motor2_direction = 1;}
+    if(u2 < 0){
+        motor2_direction = 0;}
     
      if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
         {
-            movement = rest;
-            timer.reset();
+        movement = rest;
+        timer.reset();
+       
+        d_error1 = 0;
+        d_error2 = 0;
+        u1 = 0;
+        u2 = 0;
+        int_u1 = 0;
+        int_u2 = 0;
+        led_red = 0;
+        led_green = 0;
         
             }
     }
 void do_wait(){
     
-    if ( filteredsignal2 > threshold2) {// 
-        current_state = waiting;
-        state_changed = true;
-    }              
+             
         if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
         {
             movement = forward;
             timer.reset();
+            led_green = 1;
+            led_blue = 1;
             }
     }    
 ///////////END MOVEMENT STATES/////////////////////////
 ///////////ROBOT ARM STATES ///////////////////////////
-    float kp1 = 0.1;
-    float kp2 = 0.1;
-    float ki1 = 0.05;
-    float ki2 = 0.05;
-    float kd1 = 0.005;
-    float kd2 = 0.005;
-    float olderror1;
-    float olderror2;
-    float d_error1;
-    float d_error2;
+    
 void do_state_failure(){    
      
     }
@@ -317,14 +427,14 @@
         wu2 = kp12*werror2; //+ (u2 + werror2*0.002)*ki2);
         }
     
-    motor1_speed_control = fabs(wu1)/8;
+    motor1_speed_control = fabs(wu1)/16;
     
     if(wu1 > 0){
         motor1_direction = 1;}
     if(wu1< 0){
         motor1_direction = 0;}
 
-    motor2_speed_control = fabs(wu2)/8;
+    motor2_speed_control = fabs(wu2)/16;
     
     if(wu2 > 0){
         motor2_direction = 1;}
@@ -370,12 +480,12 @@
        threshold2 = 0.5 * max2;
        }
        
-    if (timer.read() > 1 && filteredsignal1 > threshold1 && filteredsignal2 > threshold2) {
+    if (timer.read() > 5 && filteredsignal1 > threshold1 && filteredsignal2 > threshold2) {
         current_state = operational;
         timer.reset();
         state_changed = true;
         led_green = 0;
-        led_red =0;
+        led_red = 0;
     }    
 }
 
@@ -403,10 +513,7 @@
         do_down();      
         break;    
     }
-    if (movement == rest && filteredsignal2 > threshold2) {
-        current_state = waiting;        
-        state_changed = true;
-    }       
+       
     
 }
 
@@ -418,6 +525,10 @@
     if (filteredsignal1 > threshold1 && filteredsignal2 > threshold2) {
         current_state = operational;
         state_changed = true;
+        movement = rest;
+        led_green = 0;
+        led_blue = 0;
+        led_red = 0;
     } 
 }
 //////////////// END ROBOT ARM STATES //////////////////////////////
@@ -546,6 +657,6 @@
     
     pc.baud(115200);
     while (true) { 
-            printf("%f %f %f %f %f %f \n\r",ref_q1,ref_q2,error1,error2,deg_m1, deg_m2);
+            printf("%i %i %f %f %f %f \n\r",movement,current_state,error1,error2,deg_m1, deg_m2);
     }
 }
\ No newline at end of file