Diff: main.cpp

Revision:

45:829a3992d689

Parent:

44:25eec278c623

Child:

46:9b60a3c1acab

--- a/main.cpp	Thu Nov 01 12:39:24 2018 +0000
+++ b/main.cpp	Thu Nov 01 19:48:22 2018 +0000
@@ -4,28 +4,37 @@
 #include "HIDScope.h"
 #include "BiQuad.h"
 #include "PID_controller.h"
-#include "kinematics.h"
+//#include "kinematics.h"
+#include "Servo.h"
 #include "processing_chain_emg.h"
 #include <vector>
 
+//pc
+MODSERIAL pc(USBTX, USBRX);
 
 // emg inputs
 AnalogIn    emg0( A0 );
 AnalogIn    emg1( A1 );
+AnalogIn    emg2( A2 );
+AnalogIn    emg3( A3 );
+//Servo myservo(A5);
+//InterruptIn button1(SW2);
+//InterruptIn button2(SW3);
+double range;
 
 // motor ouptuts
 PwmOut      motor1_pwm(D5);
 DigitalOut  motor1_dir(D4);
 PwmOut      motor2_pwm(D6);
 DigitalOut  motor2_dir(D7);
+AnalogIn vx_adjustment(A4);
+AnalogIn vy_adjustment(A5);
 
 // defining encoders
 QEI motor_1_encoder(D12,D13,NC,32);
 QEI motor_2_encoder(D10,D11,NC,32);
 
 // other objects
-AnalogIn    potmeter1(A2);
-AnalogIn    potmeter2(A3);
 DigitalIn   button(D0);
 DigitalIn   emgstop(SW2);
 DigitalOut  led_R(LED_RED);
@@ -51,7 +60,7 @@
 Emg_measures_states current_emg_calibration_state = not_in_calib_emg;
 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 des_vx, des_vy, x, y, q1, q2, e1, e2, x_norm, y_norm; //will be set by the motor_controller function
 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;
@@ -67,13 +76,14 @@
 // Variables for calibration
 double calib_q1 = 3.1415926535f;
 double calib_q2 = double(7)/double(6) * 3.1415926535f;
-double off_set_q1 = 0; // This variable is used to determine the off set from our definition from q1 and q2.
-double off_set_q2 = 0;
+double off_set_q1 = 0.5f*3.1415926535f; // This variable is used to determine the off set from our definition from q1 and q2.
+double off_set_q2 = 3.1415926535f;
 
 // Variables defined for the homing state
 double q1_homing = 0.5f*3.1415926535f, q2_homing = 3.1415926535f;
-double beta = 5;
-double k_hom = 2;
+double qref1 = 0.5f*3.1415926535f, qref2 = 3.1415926535f;
+double beta = 0.05;
+double k_hom = 0.05;
 
 // For the state calib_enc
 double q1old;
@@ -82,6 +92,7 @@
 vector<double> currentconfig;
 vector<double> newconfig;
 vector<double> ref;
+vector<double> testconfig;
 
 double currentx;
 double currenty;
@@ -99,11 +110,21 @@
 bool state_changed = false;
 const double T = 0.001;
 
+bool gripperclosed = false;
+
 // Resolution of the encoder at the output axle
 double resolution = (2.0f*3.1415926535f/double(counts_per_rotation))*(1.0/131.0); // In radians
 
 // Functions
 
+//void open_gripper(){
+//    myservo=0;
+//}
+//
+//void close_gripper(){
+//    myservo=1;
+//}
+
 vector<double> forwardkinematics_function(double& q1, double& q2) {
     // input are joint angles, output are x and y position of end effector
     
@@ -118,7 +139,7 @@
    
 }
  
-vector<double> inversekinematics_function(const double& T, double& q1, double& q2, double& des_vx, double& des_vy, double ref1, double ref2) {
+vector<double> inversekinematics_function(const double& T, double ref1, double ref2, double& q1, double& q2, double& des_vx, double& des_vy) {
     // x, y: positions of end effector | T: time to hold current velocity | qref1, qref2: reference thetas | q1, q2: current thetas | vx, vy: desired x, y velocities
     
     // pseudo inverse jacobian to get joint speeds
@@ -127,14 +148,13 @@
     double q1_star_des; // desired joint velocity of q1_star
     double q2_star_des; // same as above but then for q2_star
     double C;
-    double qref1, qref2;
  
     // The calculation below assumes that the end effector position is calculated before this function is executed
     // In our case the determinant will not equal zero, hence no problems with singularies I think.
     
     C = L1*L2*sin(ref1)*cos(ref2) - L1*L2*sin(ref2)*cos(ref1);
-    q1_star_des = double(1/C)*(-L2*cos(ref2)*des_vy - L2*sin(ref2)*des_vx);
-    q2_star_des = double(1/C)*((L2*cos(ref2)-L1*cos(ref1))*des_vy + (L2*sin(ref2)-L1*sin(ref1))*des_vx);
+    q1_star_des = double(1/C)*(-L2*cos(ref2)*des_vx - L2*sin(ref2)*des_vy);
+    q2_star_des = double(1/C)*((L2*cos(ref2)-L1*cos(ref1))*des_vx + (L2*sin(ref2)-L1*sin(ref1))*des_vy);
     
     qref1 = T*q1_star_des;
     qref2 = T*(q2_star_des+q1_star_des);
@@ -143,7 +163,7 @@
     double testq2 = ref2+qref2;
     newconfig = forwardkinematics_function(testq1, testq2);
     
-    if (sqrt(pow(newconfig[0],2)+pow(newconfig[1],2)) > 0.73)
+    if (sqrt(pow(newconfig[0],2)+pow(newconfig[1],2)) > 1)
     {
         qref1 = ref1;
         qref2 = ref2;
@@ -162,19 +182,26 @@
 
 void measure_all() 
 {
-
-    q1 = motor_1_encoder.getPulses()*2.0f*3.1415926535f/counts_per_rotation + off_set_q1; //do this here, and not in the encoder interrupt, to reduce computational load
-    q2 = motor_2_encoder.getPulses()*2.0f*3.1415926535f/counts_per_rotation + off_set_q2;
+    q1 = (motor_1_encoder.getPulses()*2.0f*3.1415926535f/counts_per_rotation)*(1.0/131.0) + off_set_q1; //do this here, and not in the encoder interrupt, to reduce computational load
+    q2 = (motor_2_encoder.getPulses()*2.0f*3.1415926535f/counts_per_rotation)*(1.0/131.0) + off_set_q2;
     currentconfig = forwardkinematics_function(q1,q2); //previously x,y also input 
     raw_emg_0 = emg0.read(); //sample analog voltages (all sampling theory applies, you might get aliasing etc.)
     raw_emg_1 = emg1.read();
+    raw_emg_2 = emg2.read();
+    raw_emg_3 = emg3.read();
     processing_chain_emg(raw_emg_0, raw_emg_1, raw_emg_2, raw_emg_3, process_emg_0, process_emg_1, process_emg_2, process_emg_3);  // processes the emg signals
 }
 
 void output_all() {
     motor1_pwm = fabs(u1);
+    if (motor1_pwm > 1.0f){
+        motor1_pwm = 1.0f;
+    }
     motor1_dir = u1 > 0.0f;
     motor2_pwm = fabs(u2);
+    if (motor2_pwm > 1.0f){
+        motor2_pwm = 1.0f;
+    }
     motor2_dir = u2 > 0.0f;
     static int output_counter = 0;
     output_counter++;
@@ -184,63 +211,13 @@
     button_suppressed = false;    
 }
 
-
-
-void PID_controller(double error1, double error2, float &u1, float &u2, double T)
-{   
-    static double error_prev1 = error1; // initialization with this value only done once!
-    static double error_prev2 = error2; // initialization with this value only done once!
-    double u_pid2, u_pid1;
-    static double error_integral1, error_integral2 = 0;
-  
-    double u_k1 = Kp * error1;
-    double u_k2 = Kp * error2;
-    
-    static BiQuad LowPassFilter1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
-    static BiQuad LowPassFilter2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
-    
-    double error_derivative1 = (error1 - error_prev1)*T;
-    double filtered_error_derivative1 = LowPassFilter1.step(error_derivative1);
-    double u_d1 = Kd * filtered_error_derivative1;
-    
-    double error_derivative2 = (error2 - error_prev2)*T;
-    double filtered_error_derivative2 = LowPassFilter2.step(error_derivative2);
-    double u_d2 = Kd * filtered_error_derivative2;
-    
-    error_integral1 = error_integral1 +( error1 * T);
-    error_integral2 = error_integral2 +( error2 * T);
-    if (error_integral1 > 1){
-        error_integral1 = 1;
-    }
-    else if (error_integral1 < -1){
-        error_integral1 = -1;
-    }
-    if (error_integral2 > 1){
-        error_integral2 = 1;
-    }
-    else if (error_integral2 < -1){
-        error_integral2 = -1;
-    }
-    
-    double u_i1 = Ki*error_integral1;
-    double u_i2 = Ki*error_integral2;
-    
-    u_pid1 = u_k1 + u_d1 + u_i1;
-    u_pid2 = u_k2 + u_d2 + u_i2;
-    
-    u1 = -u_pid1;
-    u2 = -u_pid2;
-    
-    error_prev1 = error1;
-    error_prev2 = error2;       
-}
-
 void state_machine() {
     switch(current_state) { //States can be: failure, wait, calib_enc, calib_emg, operational, demo,
         case waiting:  //Nothing useful here, maybe a blinking LED for fun and communication to the user
             if (button.read()==false) 
             {
-                current_state = calib_enc; //the NEXT loop we will be in calib_enc state
+                current_state = calib_emg; //the NEXT loop we will be in calib_enc state
+                current_emg_calibration_state = calib_right_bicep;
                 calib_enc_state = calib_enc_q2;
                 state_changed = true;
             }
@@ -277,7 +254,6 @@
                     q2old = q2;
                     break;
                 case calib_enc_q1:
-                    if (state_timer.read() > 0.5f){u1=0.55;}
                     if (q1 - q1old == 0.0 && state_timer.read() > 5.0f)
                     {
                         calib_enc_state = not_in_calib_enc;
@@ -415,8 +391,11 @@
             }
             
             // normalization 
-            double x_norm = process_emg_0 * scaling_right_bicep - process_emg_1 * scaling_right_tricep;
-            double y_norm = process_emg_2 * scaling_left_bicep - process_emg_3 * scaling_left_tricep;
+            x_norm = process_emg_0 * scaling_right_bicep - process_emg_1 * scaling_right_tricep;
+            y_norm = process_emg_2 * scaling_left_bicep - process_emg_3 * scaling_left_tricep;
+               
+             //x_norm = -1+2*vx_adjustment.read();
+             //y_norm = -1+2*vy_adjustment.read();  
                             
             // here we have to determine the desired velocity based on the processed emg signals and calibration
             // x velocity
@@ -440,8 +419,8 @@
             else if(y_norm <= -0.4) { des_vy = -0.1; }
             else if(y_norm <= -0.2) { des_vy = -0.05; }
             else { des_vy = 0; }
-            
-            if (button.read() == true && button_suppressed == false ) {
+    
+            if (button.read() == false && button_suppressed == false ) {
                 current_state = demo; 
                 state_changed = true;
                 button_suppressed = true;
@@ -449,7 +428,7 @@
                 make_button_active.attach(&unsuppressing_button,0.5);
 
             } 
-            
+         
             break;
             
         case demo: //moving according to a specified trajectory
@@ -458,19 +437,25 @@
                     state_changed = false;
                     state_timer.reset();
                     state_timer.start();
-                    des_vx = 0.05;
+                    des_vx = 0.1;
+                    des_vy = 0;
+            }
             double old_vx;
             double old_vy;        
             if (state_timer > 2.0f){
                 old_vx = des_vx;
                 old_vy = des_vy;
-                des_vx = des_vy;
-                des_vy = -des_vx;
-                state_timer.reset();   
+                
+                des_vy = -old_vx;
+                des_vx = old_vy;
+                
+                state_timer.reset();  
+                state_timer.start(); 
             }
+            
+            
             //        des_vx = 0.1;           // first we move to the right
             //        des_vy = 0.0;
-            }
             
             //static double new_vx, new_vy;
                         
@@ -490,7 +475,7 @@
             //}
        
                 
-            if (button.read() == true && button_suppressed == false ) {
+            if (button.read() == false && button_suppressed == false ) {
                 current_state = operational;
                 state_changed = true;
                 
@@ -519,7 +504,7 @@
         qref2 = ref[1];
         e1 = qref1 - q1; //tracking error (q_ref - q_meas)
         e2 = qref2 - q2;
-        PID_controller(e1,e2,u1,u2,T); //feedback controller or with possibly fancy controller additions...; pass by reference
+        PID_controller(e1*(180/3.14),e2*(180/3.14),u1,u2,T); //feedback controller or with possibly fancy controller additions...; pass by reference
         } //otherwise we just don’t mess with the value of control variable ‘u’ that is set somewhere in the state-machine.
 }
 
@@ -534,6 +519,7 @@
 
 int main() 
 {
+    pc.baud(115200);
     motor1_pwm.period_us(60);
     motor2_pwm.period_us(60);
     current_state = waiting; //we start in state ‘waiting’ and current_state can be accessed by all functions
@@ -547,6 +533,14 @@
     led_R = 1;
     led_B = 1;
     led_G = 1;
-
-    while (true) { }  //Do nothing here (timing purposes)
+//    myservo = 1;
+//    button1.fall(&open_gripper);
+//    button2.fall(&close_gripper);
+    
+    while (true) {pc.printf("error1: %f, error2: %f, pwm1: %f, pwm2: %f, qref1: %f, qref2: %f, vx: %f, vy: %f, q1: %f, q2: %f \r\n",  e1, e2, u1, u2, qref1, qref2, des_vx, des_vy, x_norm, y_norm);
+    testconfig = forwardkinematics_function(qref1,qref2);
+    //double x = testconfig[0];
+    //double y = testconfig[1];
+    //pc.printf("x: %f, y: %f \r\n", x,y);
+    }  //Do nothing here (timing purposes)
 }