Diff: main.cpp

Revision:

7:db050a878cff

Parent:

6:b526cf83fd4f

Child:

8:c7d21f5f87d8

--- a/main.cpp	Tue Oct 30 08:26:42 2018 +0000
+++ b/main.cpp	Tue Oct 30 14:15:57 2018 +0000
@@ -13,9 +13,15 @@
 int gear_ratio = 131;
 float full_ratio  = gear_ratio*sensor_sensitivity*4;
 
+DigitalIn but1(D2);
+
 QEI Encoder1(D10,D11,NC,sensor_sensitivity); //First one is B, Second one is A
 QEI Encoder2(D12,D13,NC,sensor_sensitivity); //
+
 DigitalOut led_red(LED_RED);
+DigitalOut led_green(LED_GREEN);
+DigitalOut led_blue(LED_BLUE);
+
 int counts_m1 = 0;
 int counts_m2 = 0;
 int counts_m1_prev = 0;
@@ -30,10 +36,12 @@
 PwmOut motor2_speed_control(D6);//aanstuursnelheid motor 2
 DigitalOut motor2_direction(D7);// draairichting motor 2 (0 is CCW )
 
-    float kp1 = 0.2;
-    float kp2 = 0.2;
-    float ki1 = 0;
-    float ki2 = 0;
+    float kp1 = 0.1;
+    float kp2 = 0.1;
+    float ki1 = 0.2;
+    float ki2 = 0.2;
+    float int_u1 = 0;
+    float int_u2 = 0;
     float u1 = 0;
     float u2 = 0;
     
@@ -50,7 +58,7 @@
 enum Operations {rest, forward, backward, up, down};
 
 States current_state = calib_motor;
-Operations movement = rest;
+Operations movement = forward;
 
 float max1 = 0; //initial threshold value for emg signals, changes during calibration left arm
 float max2 = 0; // right arm
@@ -61,7 +69,7 @@
 Ticker      loop_timer;
 Ticker      sample_timer;
 Ticker      sample_timer2;
-//HIDScope    scope(2);
+//HIDScope    scope(3);
 
 AnalogIn raw_emg1_input(A0);//input for first emg signal 1, for the modes
 AnalogIn raw_emg2_input(A1);//input for first emg signal 2, for the strength
@@ -76,22 +84,27 @@
 volatile float filteredsignal2;//the first filtered emg signal 2
 
 bool state_changed = false;
+static BiQuad Notch1(0.9714498065192796,-1.5718388053127037,0.9714498065192796,-1.5718388053127037,0.9428996130385592);
+static BiQuad Notch2(0.9714498065192796,-1.5718388053127037,0.9714498065192796,-1.5718388053127037,0.9428996130385592);
+static BiQuad HighPass1(0.95653708,  -1.91307417, 0.95653708, -1.91118480, 0.91496354);
+static BiQuad HighPass2(0.95653708,  -1.91307417, 0.95653708, -1.91118480, 0.91496354);
+static BiQuad LowPass1(0.00362164,  0.00724327, 0.00362164, -1.82267251, 0.83715906);
+static BiQuad LowPass2(0.00362164,  0.00724327, 0.00362164, -1.82267251, 0.83715906);
 
 void filterall()
 {
+    //Notch 50 Hz BW 6 Hz
+    float notch1 = Notch1.step(emg1_input);
+    float notch2 = Notch2.step(emg2_input);
     //Highpass Biquad 5 Hz 
-static BiQuad HighPass1(0.95653708,  -1.91307417, 0.95653708, -1.91118480, 0.91496354);
-float high1 = HighPass1.step(emg1_input);
-static BiQuad HighPass2(0.95653708,  -1.91307417, 0.95653708, -1.91118480, 0.91496354);
-float high2 = HighPass2.step(emg2_input);
+    float high1 = HighPass1.step(notch1);
+    float high2 = HighPass2.step(notch2);
     // Rectify the signal(absolute value)
-float abs1 = fabs(high1);
-float abs2 = fabs(high2);
+    float abs1 = fabs(high1);
+    float abs2 = fabs(high2);
     //Lowpass Biquad 10 Hz
-static BiQuad LowPass1(0.00362164,  0.00724327, 0.00362164, -1.82267251, 0.83715906);
-float low1 = LowPass1.step(abs1);
-static BiQuad LowPass2(0.00362164,  0.00724327, 0.00362164, -1.82267251, 0.83715906);
-float low2 = LowPass2.step(abs2);
+    float low1 = LowPass1.step(abs1);
+    float low2 = LowPass2.step(abs2);
 
 raw_filteredsignal1 = low1;
 raw_filteredsignal2 = low2;
@@ -120,34 +133,65 @@
 
 void scopedata()
 {
-    //scope.set(0,deg_m1); // 
-    //scope.set(1,deg_m2); //    
-    //scope.set(2,emg1_input); //
-    //scope.set(3,emg1_input);//
-    //scope.set(4,filteredsignal1);
+    //scope.set(0,filteredsignal1); // 
+    //scope.set(1,filteredsignal2); //    
+    //scope.set(2,ref_q1); //
+   // scope.set(3,movement);//
+   // scope.set(4,ref_q1);
     //scope.send(); // send info to HIDScope server
 }
 
 
  //////////////////// MOVEMENT STATES
+ float twist[2] = {0,0};
+ float twistf[2] = {0,0};
+ float abs_sig;  
+ 
+    int gain = 1;
+    
 void do_forward(){
     
-    //twist1, 0;
-    //Vector2d twistf(0,0);
-    //twistf << 1, 0;
-    if (filteredsignal2 > threshold2){
-        double abs_sig = (filteredsignal2 - (0.5*max2))/(0.5*max2);
+    
+    twistf[0] = 0;
+    twistf[1] = -1;
+    
+    if (filteredsignal2 > 0.15*max2){
+        abs_sig = (filteredsignal2 - (0.15*max2))/(0.85*max2);       
         
-        //twist = twistf * abs_sig;
         
         }
+    else
+    {
+        abs_sig = 0;
+        }
+    //if (but1 == false){
+     //   abs_sig = 0.75;     
+        
+        
+     //   }    
     
+    twist[0] = twistf[0] * abs_sig* gain;
+    twist[1] = twistf[1] * abs_sig* gain;
+             
+    motor1_speed_control = fabs(u1);
     
-    if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
-        {
-            movement = backward;
-            timer.reset();
-            }
+    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*100)
+       // {
+       //     movement = backward;
+        //    timer.reset();
+        //    }
     }
 
 void do_backward(){
@@ -206,6 +250,7 @@
     if (state_changed==true) {
         state_changed = false;        
     }
+    led_red = 0;
     
     
     
@@ -219,18 +264,22 @@
         motor1_speed_control = 0;
         motor2_speed_control = 0;
         current_state = homing;
-        timer.reset();
+        
         state_changed = true;
-        wait(3);
-        deg_m1 = -2;
-        deg_m2 = -2;
+        wait(1);
+        deg_m1 = 57.4;
+        deg_m2 = 72.93;
+        led_red = 1;
+        led_green = 0;
     }
     }
     
 float wu1;
 float wu2;  
+
 void do_state_homing(){
      if (state_changed==true) {
+        timer.reset();
         state_changed = false;        
     }
          
@@ -242,7 +291,7 @@
     if(werror1 < -5){
         wu1 = -1;        }
     else{
-        wu1 = kp1*werror1 + (u1 + werror1*0.002)*ki1;   
+        wu1 = kp1*werror1; //+ (u1 + werror1*0.002)*ki1;   
     }
     
     if(werror2 > 5){
@@ -250,23 +299,42 @@
     if(werror2 < -5){
         wu2 = -1;}
     else{
-        wu2 = (kp2*werror2 + (u2 + werror2*0.002)*ki2);
+        wu2 = kp2*werror2; //+ (u2 + werror2*0.002)*ki2);
         }
     
-    motor1_speed_control = fabs(wu1)/5 + 0.2;
+    motor1_speed_control = fabs(wu1)/8;
     
     if(wu1 > 0){
-        motor1_direction = 0;}
+        motor1_direction = 1;}
     if(wu1< 0){
-        motor1_direction = 1;}
+        motor1_direction = 0;}
 
-    motor2_speed_control = fabs(wu2)/5 + 0.2;
+    motor2_speed_control = fabs(wu2)/8;
     
     if(wu2 > 0){
-        motor2_direction = 0;}
+        motor2_direction = 1;}
     if(wu2 < 0){
-        motor2_direction = 1;}
+        motor2_direction = 0;}
+        
+    if (  timer.read() > 4) {
+         motor1_speed_control = 0;
+         motor2_speed_control = 0;   
+       
         
+        deg_m1 = 0;
+        deg_m2 = 0;
+        u1 = 0;
+        u2 = 0;
+        int_u1 = 0;
+        int_u2 = 0;
+        wait(1);
+        current_state = calib_emg;
+        timer.reset();
+        state_changed = true;
+        led_green = 1;
+        led_blue = 0;
+         
+    }   
 
     
     }
@@ -277,17 +345,19 @@
     
     if(filteredsignal1 > max1){//calibrate to a new maximum
        max1 = filteredsignal1;
-       threshold1 = 0.5*max1;
+       threshold1 = 0.5 * max1;
        }
     if(filteredsignal2 > max2){//calibrate to a new maximum
        max2 = filteredsignal2;
        threshold2 = 0.5 * max2;
        }
        
-    if (timer.read() > 10 && 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;
     }    
 }
 
@@ -335,10 +405,8 @@
 //////////////// END ROBOT ARM STATES //////////////////////////////
  
     
-void motor_controller(){
-    
-  
-    
+void motor_controller(){   
+     
     float jacobian[4];
     float inv_jacobian[4];
     
@@ -355,27 +423,36 @@
     inv_jacobian[3] = det*jacobian[0];
     
     
-    //ref_v1 = jacobian[0]*twist[0]+jacobian[1]*twist[1];
-    //ref_v2 = jacobian[2]*twist[0]+jacobian[3]*twist[1];
+    ref_v1 = inv_jacobian[0]*twist[0]+inv_jacobian[1]*twist[1];
+    ref_v2 = inv_jacobian[2]*twist[0]+inv_jacobian[3]*twist[1];
        
-    //jacobian << L1, L2*sin(q1)+L1,-L0,-L0 - L2*cos(q1);
-        
-    //inv_jacobian = jacobian.inverse();
-    //Vector2d reference_vector = inv_jacobian*twist;
-    //float ref_v1 = reference_vector(0);
-    //float ref_v2 = reference_vector(1);
-      
+       
      
-    //ref_q1 = ref_q1 + 0.002*ref_v1;
-    //ref_q2 = ref_q2 + 0.002*ref_v2;
+    ref_q1 = ref_q1 + 0.002*ref_v1;
+    ref_q2 = ref_q2 + 0.002*ref_v2;
+    
+    float error1 = deg_m1 - ref_q1;
+    float error2 = deg_m2 - ref_q2;
     
-    //float error1 = deg_m1 - ref_q1;
-    //float error2 = deg_m2 - ref_q2;
+   if(error1 > 5){
+        u1 = 1;        }
+    if(error1 < -5){
+        u1 = -1;        }
+    else{
+        int_u1 = int_u1 + error1 * Ts;
+        u1 = kp1*error1 + int_u1*ki1;   
+    }
     
-   
-    //u1 = kp1*error1 + (u1 + error1*0.002)*ki1;
-    //u2 = kp2*error1 + (u2 + error1*0.002)*ki2;
-    
+    if(error2 > 5){
+        u2 = 1;}
+    if(error2 < -5){
+        u2 = -1;}
+    else{
+        int_u2 = int_u2 + error2 * Ts;
+        u2 = (kp2*error2 + int_u2*ki2);
+        }
+  
+       
     }
 
 void loop_function() { //Main loop function
@@ -402,6 +479,7 @@
 }
 motor_controller();
 // Outputs data to the computer
+
 }
 
 
@@ -410,17 +488,19 @@
    // motor1_speed_control.period_us(60); //60 microseconds PWM period, 16.7 kHz
     
     motor1_direction = 0; //set motor 1 to run clockwisedirection for calibration
-    motor1_speed_control = 0.25;//to make sure the motor will not run at too high velocity
+    motor1_speed_control = 0.15;//to make sure the motor will not run at too high velocity
     motor2_direction = 0; // set motor 2 to run clockwise direction
-    motor2_speed_control = 0.25; 
+    motor2_speed_control = 0.15; 
      
-    led_red = 0;    
+    led_red = 1;  
+    led_green = 1;
+    led_blue = 1;   
     timer.start();
     loop_timer.attach(&loop_function, Ts);    
     sample_timer.attach(&scopedata, Ts);    
     sample_timer2.attach(&filterall, Ts);
 
     while (true) { 
-               
+            printf("%f %f \n\r",ref_q1,ref_q2);
     }
 }
\ No newline at end of file