Diff: main.cpp

Revision:

4:34ad002cb646

Parent:

3:055eb9f256fc

Child:

5:892531e4e015

--- a/main.cpp	Fri Oct 26 11:46:25 2018 +0000
+++ b/main.cpp	Mon Oct 29 09:55:04 2018 +0000
@@ -15,7 +15,7 @@
 
 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);
 int counts_m1 = 0;
 int counts_m2 = 0;
 int counts_m1_prev = 0;
@@ -23,15 +23,30 @@
 float deg_m1 = 0;
 float deg_m2 = 0;
 
-//Vector2d twist(0,0);
-//Matrix2f jacobian(0, 0, 0, 0), inv_jacobian(0, 0, 0, 0); 
+
 
 DigitalOut motor1_direction(D4);// draairichting motor 1 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)
 PwmOut motor1_speed_control(D5);//aanstuursnelheid motor 1
 PwmOut motor2_speed_control(D6);//aanstuursnelheid motor 2
 DigitalOut motor2_direction(D7);// draairichting motor 2 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)
 
-enum States {failure, waiting, calib_motor, calib_emg, operational, demo};
+float kp1 = 2;
+    float kp2 = 2;
+    float ki1 = 0.5;
+    float ki2 = 0.5;
+    float u1 = 0;
+    float u2 = 0;
+    
+     float ref_q1 = 0;    
+     float ref_q2 = 0;
+    float L0 = 0.1;
+    float L1 = 0.1;
+    float L2 = 0.4;
+    
+    float ref_v1;
+    float ref_v2;
+    
+enum States {failure, waiting, calib_motor, homing ,calib_emg, operational, demo};
 enum Operations {rest, forward, backward, up, down};
 
 States current_state = calib_motor;
@@ -46,7 +61,7 @@
 Ticker      loop_timer;
 Ticker      sample_timer;
 Ticker      sample_timer2;
-HIDScope    scope(5);
+//HIDScope    scope(5);
 
 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
@@ -106,23 +121,23 @@
 
 void scopedata()
 {
-    scope.set(0,emg1_input); // 
-    scope.set(1,emg1_input); //    
-    scope.set(2,emg1_input); //
-    scope.set(3,emg1_input);//
-    scope.set(4,filteredsignal1);
-    scope.send(); // send info to HIDScope server
+    //scope.set(0,emg1_input); // 
+    //scope.set(1,emg1_input); //    
+    //scope.set(2,emg1_input); //
+    //scope.set(3,emg1_input);//
+    //scope.set(4,filteredsignal1);
+    //scope.send(); // send info to HIDScope server
 }
 
 
  //////////////////// MOVEMENT STATES
 void do_forward(){
     
-    //twist << 1, 0;
+    //twist1, 0;
     //Vector2d twistf(0,0);
     //twistf << 1, 0;
     if (filteredsignal2 > threshold2){
-        //double abs_sig = (filteredsignal2 - (0.5*max2))/(0.5*max2);
+        double abs_sig = (filteredsignal2 - (0.5*max2))/(0.5*max2);
         
         //twist = twistf * abs_sig;
         
@@ -186,30 +201,60 @@
 void do_state_failure(){    
      
     }
-    bool on = true;
+   int count1 = 0;
+   int count2 = 0;  
 void do_state_calib_motor(){
     if (state_changed==true) {
         state_changed = false;        
     }
     
     
-    if(on){
+ 
+    
+    int deriv1 = deg_m1 - count1;
+    int deriv2 = deg_m2 - count2;
+    count1 = deg_m1;
+    count2 = deg_m2;
+   
+    if (  timer.read() > 3 && deriv1 < 0.5 && deriv2 < 0.5) {
+        motor1_speed_control = 0;
+        motor2_speed_control = 0;
+        current_state = homing;
         timer.reset();
-   
-    on = false;
+        state_changed = true;
+        
+        deg_m1 = -45;
+        deg_m2 = -70;
+    }
     }
     
-    int deriv1 = fabs((counts_m1 - counts_m1_prev)/Ts);
-    int deriv2 = fabs((counts_m2 - counts_m2_prev)/Ts);
+void do_state_homing(){
+     if (state_changed==true) {
+        state_changed = false;        
+    }
+         
+    float werror1 = deg_m1 - 0;
+    float werror2 = deg_m2 - 0;
     
+   
+   float  wu1 = kp1*werror1 + (u1 + werror1*0.002)*ki1;
+    float wu2 = kp2*werror1 + (u2 + werror1*0.002)*ki2;
     
-    if ( timer.read() > 5 && deriv1 < 1 && deriv2 < 1) {
-        motor1_speed_control = 0;
-        motor2_speed_control = 0;
-        current_state = calib_emg;
-        timer.reset();
-        state_changed = true;
-    }
+    motor1_speed_control = fabs(wu1/200);
+    if(wu1 > 0){
+        motor1_direction = 0;}
+    if(wu1< 0){
+        motor1_direction = 1;}
+
+    motor2_speed_control = fabs(wu2/200);
+    
+    if(wu2 > 0){
+        motor2_direction = 0;}
+    if(wu2< 0){
+        motor2_direction = 1;}
+        
+
+    
     }
 void do_state_calib_emg(){    
     if (state_changed==true) {
@@ -274,30 +319,49 @@
     } 
 }
 //////////////// END ROBOT ARM STATES //////////////////////////////
-
+ 
+    
 void motor_controller(){
-    float q1;
-    float q2;
-    //q1 defined
-    //q2 defined
+    
+    
+   
+    
+    float jacobian[4];
+    float inv_jacobian[4];
+    
+    jacobian[0] = L1;
+    jacobian[1] = L2*sin(deg_m1)+L1;
+    jacobian[2] = -L0;
+    jacobian[3] = -L0 - L2*cos(deg_m1);
     
-    //float L0 = 0.1;
-    //float L1 = 0.1;
-    //float L2 = 0.4;
+    float det = 1/(jacobian[0]*jacobian[3]-jacobian[1]*jacobian[2]);
     
+    inv_jacobian[0] = det*jacobian[3];
+    inv_jacobian[1] = -det*jacobian[1]; 
+    inv_jacobian[2] = -det*jacobian[2];
+    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];
+       
     //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;
     
-    //float ref_q1 = 0;
-    //ref_q1 = ref_p1 + 0.002*ref_v1;
-    // float ref_q2 = 0;
-    //ref_q2 = ref_p2 + 0.002*ref_v2;
+    //float error1 = deg_m1 - ref_q1;
+    //float error2 = deg_m2 - ref_q2;
     
-    
+   
+    //u1 = kp1*error1 + (u1 + error1*0.002)*ki1;
+    //u2 = kp2*error1 + (u2 + error1*0.002)*ki2;
     
     }
 
@@ -310,6 +374,9 @@
     case calib_motor:
         do_state_calib_motor();  
         break ;
+    case homing:
+        do_state_homing();
+        break;    
     case calib_emg:
         do_state_calib_emg();
         break;
@@ -327,12 +394,14 @@
 
 int main()
 {        
-    motor1_speed_control.period_us(60); //60 microseconds PWM period, 16.7 kHz
+   // motor1_speed_control.period_us(60); //60 microseconds PWM period, 16.7 kHz
     
     motor1_direction = 1; //set motor 1 to run counterclockwise (positive) direction for calibration
-    motor1_speed_control = 0.55;//to make sure the motor will not run at too high velocity
+    motor1_speed_control = 0.25;//to make sure the motor will not run at too high velocity
     motor2_direction = 0; // set motor 2 to run clockwise (negative) direction
-    motor2_speed_control = 0.55;      
+    motor2_speed_control = 0.25; 
+     
+    led_red = 0;    
     timer.start();
     loop_timer.attach(&loop_function, Ts);    
     sample_timer.attach(&scopedata, Ts);