2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
Diff: main.cpp
- Revision:
- 37:4d7b7ced20ef
- Parent:
- 36:4d4fc200171b
- Child:
- 38:c8ac615d0c8f
--- a/main.cpp Thu Oct 22 23:19:50 2015 +0000 +++ b/main.cpp Thu Oct 22 23:26:37 2015 +0000 @@ -212,16 +212,17 @@ void sample_filter(void); //Sampling and filtering void control(); //Control - reference -> error -> pid -> motor output -void dlscontrol(); -void calibrate_emg(); //Instructions + measurement of MVC of each muscle +void dlscontrol(); //Damped Least Squares method +void calibrate_emg(); //Instructions + measurement of Min and MVC of each muscle void emg_mvc(); //Helper funcion for storing MVC value +void emg_min(); //Helper function for storing Min value void calibrate_arm(void); //Calibration of the arm with limit switches void start_sampling(void); //Attaches the sample_filter function to a 500Hz ticker void stop_sampling(void); //Stops sample_filter void start_control(void); //Attaches the control function to a 100Hz ticker -void start_dlscontrol(void); +void start_dlscontrol(void);//Attaches DLS control function to a 100Hz ticker void stop_control(void); //Stops control function -void emg_min(); + //Keeps the input between min and max value void keep_in_range(double * in, double min, double max); @@ -252,8 +253,8 @@ red=1; green=1; blue=1; //Make sure debug LEDs are off //Set PwmOut frequency to 10k Hz - //pwm_motor1.period(PWM_PERIOD); - //pwm_motor2.period(PWM_PERIOD); + //pwm_motor1.period(0.0001); + //pwm_motor2.period(0.0001); clearTerminal(); //Clear the putty window @@ -262,10 +263,8 @@ mainMenu(); //set initial reference position - x = 0.5; - y = 0; - theta1=0.6980; - theta2=0.5200; + //x = 0.5; + //y = 0; //maybe some stop commands when a button is pressed: detach from timers. //stop_control(); @@ -859,6 +858,7 @@ y_error = y - current_y; + if (control_method=1){ //inverse kinematics (refpos to refangle) costheta2 = (pow(x,2) + pow(y,2) - pow(l1,2) - pow(l2,2)) / (2*l1*l2) ; @@ -882,7 +882,22 @@ //Angle error m1_error = dtheta1-theta1; m2_error = dtheta2-theta2; + } + if(control_method=2){ + //inverse kinematics (error in position to error in angles) + dls1= -(l2*pow(lambda,2)*sin(theta1 + theta2) + l1*pow(lambda,2)*sin(theta1) + l1*pow(l2,2)*pow(cos(theta1 + theta2),2)*sin(theta1) - l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1))/(pow(lambda,4) + 2*pow(l2,2)*pow(lambda,2)*pow(cos(theta1 + theta2),2) + 2*pow(l2,2)*pow(lambda,2)*pow(sin(theta1 + theta2),2) + pow(l1,2)*pow(lambda,2)*pow(cos(theta1),2) + pow(l1,2)*pow(lambda,2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(cos(theta1 + theta2),2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(sin(theta1 + theta2),2)*pow(cos(theta1),2) + 2*l1*l2*pow(lambda,2)*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*pow(lambda,2)*sin(theta1 + theta2)*sin(theta1) - 2*pow(l1,2)*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1)); + dls2= (l2*pow(lambda,2)*cos(theta1 + theta2) + l1*pow(lambda,2)*cos(theta1) + l1*pow(l2,2)*pow(sin(theta1 + theta2),2)*cos(theta1) - l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*sin(theta1))/(pow(lambda,4) + 2*pow(l2,2)*pow(lambda,2)*pow(cos(theta1 + theta2),2) + 2*pow(l2,2)*pow(lambda,2)*pow(sin(theta1 + theta2),2) + pow(l1,2)*pow(lambda,2)*pow(cos(theta1),2) + pow(l1,2)*pow(lambda,2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(cos(theta1 + theta2),2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(sin(theta1 + theta2),2)*pow(cos(theta1),2) + 2*l1*l2*pow(lambda,2)*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*pow(lambda,2)*sin(theta1 + theta2)*sin(theta1) - 2*pow(l1,2)*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1)); + dls3= -(l2*pow(lambda,2)*sin(theta1 + theta2) - l1*pow(l2,2)*pow(cos(theta1 + theta2),2)*sin(theta1) + pow(l1,2)*l2*sin(theta1 + theta2)*pow(cos(theta1),2) - pow(l1,2)*l2*cos(theta1 + theta2)*cos(theta1)*sin(theta1) + l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1))/(pow(lambda,4) + 2*pow(l2,2)*pow(lambda,2)*pow(cos(theta1 + theta2),2) + 2*pow(l2,2)*pow(lambda,2)*pow(sin(theta1 + theta2),2) + pow(l1,2)*pow(lambda,2)*pow(cos(theta1),2) + pow(l1,2)*pow(lambda,2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(cos(theta1 + theta2),2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(sin(theta1 + theta2),2)*pow(cos(theta1),2) + 2*l1*l2*pow(lambda,2)*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*pow(lambda,2)*sin(theta1 + theta2)*sin(theta1) - 2*pow(l1,2)*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1)); + dls4= (l2*pow(lambda,2)*cos(theta1 + theta2) - l1*pow(l2,2)*pow(sin(theta1 + theta2),2)*cos(theta1) + pow(l1,2)*l2*cos(theta1 + theta2)*pow(sin(theta1),2) - pow(l1,2)*l2*sin(theta1 + theta2)*cos(theta1)*sin(theta1) + l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*sin(theta1))/(pow(lambda,4) + 2*pow(l2,2)*pow(lambda,2)*pow(cos(theta1 + theta2),2) + 2*pow(l2,2)*pow(lambda,2)*pow(sin(theta1 + theta2),2) + pow(l1,2)*pow(lambda,2)*pow(cos(theta1),2) + pow(l1,2)*pow(lambda,2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(cos(theta1 + theta2),2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(sin(theta1 + theta2),2)*pow(cos(theta1),2) + 2*l1*l2*pow(lambda,2)*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*pow(lambda,2)*sin(theta1 + theta2)*sin(theta1) - 2*pow(l1,2)*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1)); + + q1_error = dls1 * x_error + dls2 * y_error; + q2_error = dls3 * x_error + dls4 * y_error; + + //Angle error + m1_error = q1_error; + m2_error = q2_error; + } //PID controller