Biorobotics 2018: Project group 16
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 4:a682fb1f37d2
- Parent:
- 3:766e9f13d84e
diff -r 766e9f13d84e -r a682fb1f37d2 main.cpp --- a/main.cpp Thu Nov 01 10:52:03 2018 +0000 +++ b/main.cpp Thu Nov 01 23:50:06 2018 +0000 @@ -88,12 +88,18 @@ volatile float error_2_integral = 0; volatile float error_1_prev; // initialization with this value only done once! volatile float error_2_prev; -volatile const float Kp = 17.5; -volatile const float Ki = 1.02; -volatile const float Kd = 23.2; -volatile const float Ts = 0.002; // Sample time in seconds +volatile const float Kp = 3.2; +volatile const float Ki = 0.000; +volatile const float Kd = 0.0; +volatile const float Ts = 0.002f; // Sample time in seconds volatile float error_1; volatile float error_2; +volatile float u_k_1; +volatile float u_k_2; +volatile float u_i_1; +volatile float u_i_2; +volatile float u_d_1; +volatile float u_d_2; volatile float U_1; volatile float U_2; @@ -109,8 +115,10 @@ volatile int iii = 0; // Start up timer // Variables for computations of joint angle and velocities: -volatile float q_1; -volatile float q_2; +volatile float q_ref1_o = 7.0f*pi/4.0f; +volatile float q_ref1_n; +volatile float q_ref2_o = 1.0f*pi/3.0f; +volatile float q_ref2_n; volatile float r_1; volatile float r_2; volatile float w_1; @@ -152,8 +160,8 @@ counts1 = Encoder1.getPulses(); counts2 = Encoder2.getPulses(); - rad_m1 = rad_count * (float)counts1; - rad_m2 = rad_count * (float)counts2; + rad_m1 = (rad_count * (float)counts1) + (1.0f*pi/3.0f); + rad_m2 = (rad_count * (float)counts2) + (7.0f*pi/4.0f); } void Filter() @@ -271,35 +279,6 @@ scope.send(); } -void Inverse() -{ - // Here we calculate the movement of each joint (Motor 1 and Motor 2) given - // a certain linear velocity-input. - - q_1= rad_m1+(pi/6.0f); // uit Encoder - q_2= rad_m2+(pi/6.0f); // uit Encoder - r_1= -0.2f; - r_2= -0.2f; - - float u = -r_2*sin(q_1)*cos(q_2)-(r_2)*cos(q_1)*sin(q_2); - float z = 2.0f*(r_2*cos(q_1)*cos(q_2))-r_3; - float y = r_2*cos(q_1)*cos(q_2)-r_2*sin(q_1)*sin(q_2)+2.0f*(r_1*cos(q_1))-r_3; - float x = (-2.0f)*r_2*sin(q_1)*cos(q_2); - float D = 1.0f/(u*z-x*y); // Determinant - //printf("Determinant is %f\r\n", D); - - float a = D*z; // Inverse jacobian a,b,c,d vormen 2 bij 2 matrix - float b = -D*x; // Inverse jacobian - float c = -D*y; // Inverse jacobian - float d = D*u; // Inverse jacobian - - vx = pot1.read()/5.0f; // uit emg data - vy = pot2.read()/5.0f; // uit emg data - - w_1 = vx*a+vy*b; - w_2 = vx*c+vy*d; -} - void Calibrating() { // Calibration is done to ensure that threshold values not predetermined and @@ -399,7 +378,7 @@ } // Setting the Threshold: -Threshold_Value = 0.8f; +Threshold_Value = 0.85f; Threshold_Bi_R = Threshold_Value * Flex_Bi_R; Threshold_Bi_L = Threshold_Value * Flex_Bi_L; @@ -413,7 +392,7 @@ else if (ii == 20000) { pc.printf("\r\nAutomatic switch to Homing State for Motor 1\r\n"); - Active_State = Homing_M1; + Active_State = Function; i = 0; } } @@ -470,16 +449,16 @@ if (counts1 == 0) { pc.printf("Switching to Homing State for Motor 2\r\n"); - Active_State = Homing_M2; // Statement here instead of statemachine because of checking speed + Active_State = Function; // Statement here instead of statemachine because of checking speed } else if (counts1 > 0) { - PwmPin1 = 0.8f; + PwmPin1 = 0.5f; DirectionPin1 = false; } else { - PwmPin1 = 0.8f; + PwmPin1 = 0.5f; DirectionPin1 = true; } } @@ -494,12 +473,12 @@ } else if (counts2 > 0) { - PwmPin2 = 0.8f; + PwmPin2 = 0.5f; DirectionPin2 = true; } else { - PwmPin2 = 0.8f; + PwmPin2 = 0.5f; DirectionPin2 = false; } } @@ -532,111 +511,205 @@ if (Checking_Bi_R && Checking_Bi_L) // sloping y = x, y > 0 { - vx = 0.5f; - vy = 0.5f; + vx = 0.01f; + vy = 0.01f; } else if (Checking_Bi_R && Checking_Tri_L) // sloping y = -x, y > 0 { - vx = -0.5f; - vy = 0.5f; + vx = -0.01f; + vy = 0.01f; } else if (Checking_Bi_L && Checking_Tri_R) // sloping y = -x, y < 0 { - vx = 0.5f; - vy = -0.5f; + vx = 0.01f; + vy = -0.01f; } else if (Checking_Tri_R && Checking_Tri_L) // sloping y = x, y < 0 { - vx = -0.5f; - vy = -0.5f; + vx = -0.01f; + vy = -0.01f; } - else if (Checking_Bi_R) // y > 0 + if (Checking_Bi_R) // y > 0 { vx = 0.0f; - vy = 0.5f; + vy = 0.01f; } else if (Checking_Bi_L) // x > 0 { - vx = 0.5f; + vx = 0.01f; vy = 0.0f; } - else if (Checking_Tri_R) // y < 0 + else if (Checking_Tri_R) // x < 0 { vx = 0.0f; - vy = -0.5f; + vy = -0.01f; } else if (Checking_Tri_L) // y < 0 { - vx = -0.5f; + vx = -0.01f; vy = 0.0f; } } -void PID_controller() + +void Inverse() +{ + // Here we calculate the movement of each joint (Motor 1 and Motor 2) given + // a certain linear velocity-input. + q_ref1_n = q_ref1_o+w_1*Ts; + q_ref2_n = q_ref2_o+w_2*Ts; + r_1= -0.2f; + r_2= -0.2f; + + float u = -r_2*sin(q_ref1_n)*cos(q_ref2_n)-(r_2)*cos(q_ref1_n)*sin(q_ref2_n); + float z = 2.0f*(r_2*cos(q_ref1_n)*cos(q_ref2_n))-r_3; + float y = r_2*cos(q_ref1_n)*cos(q_ref2_n)-r_2*sin(q_ref1_n)*sin(q_ref2_n)+2.0f*(r_1*cos(q_ref1_n))-r_3; + float x = (-2.0f)*r_2*sin(q_ref1_n)*cos(q_ref2_n); + float D = 1.0f/(u*z-x*y); // Determinant + //printf("Determinant is %f\r\n", D); + + float a = D*z; // Inverse jacobian a,b,c,d vormen 2 bij 2 matrix + float b = -D*x; // Inverse jacobian + float c = -D*y; // Inverse jacobian + float d = D*u; // Inverse jacobian + + float referenceVelocity1; + if (pot1.read()>0.8f) + { + // Clockwise rotation + referenceVelocity1 = 0.005f; + } + + + + else if (pot1.read() < 0.2f) + { + // Counterclockwise rotation + referenceVelocity1 = -0.005f ; + } + + else + { + referenceVelocity1 = pot1.read() * 0.0f; + } + float referenceVelocity2; + + // This is where Motor 2 is opperated. + + if (pot2.read()>0.8f) + { + // Clockwise rotation + referenceVelocity2 = 0.005f; + } + + else if (pot2.read() < 0.2f) + { + // Counterclockwise rotation + referenceVelocity2 = -0.005f ; + } + + else + { + referenceVelocity2 = pot2.read() * 0.0f; + } + + //vx = 0.0;//referenceVelocity1; // uit emg data + //vy = 0.01;referenceVelocity2; // uit emg data + + w_1 = vx*a+vy*b; + w_2 = vx*c+vy*d; + + + q_ref1_o = q_ref1_n; + q_ref2_o = q_ref2_n; +} + + + void PID_controller() { // The PID-controller reduces the error between the reference signal // and the actual value. - error_1 = (w_1*0.002f) - rad_m1; - error_2 = (w_2*0.002f) - rad_m2; + error_1 = q_ref1_o - rad_m2 + (w_1*Ts); + error_2 = q_ref2_o - rad_m1 + (w_2*Ts); error_1_prev = error_1; error_2_prev = error_2; // Proportional part: - float u_k_1 = Kp * error_1; - float u_k_2 = Kp * error_2; + u_k_1 = Kp * error_1; + u_k_2 = Kp * error_2; // Integral part error_1_integral = error_1_integral + error_1 * Ts; error_2_integral = error_2_integral + error_2 * Ts; - float u_i_1 = Ki * error_1_integral; - float u_i_2 = Ki * error_2_integral; + u_i_1 = Ki * error_1_integral; + u_i_2 = Ki * error_2_integral; // Derivative part float error_1_derivative = (error_1 - error_1_prev)/Ts; float error_2_derivative = (error_2 - error_2_prev)/Ts; - float filtered_error_1_derivative = LowPassFilter.step(error_1_derivative); - float filtered_error_2_derivative = LowPassFilter.step(error_2_derivative); - float u_d_1 = Kd * filtered_error_1_derivative; - float u_d_2 = Kd * filtered_error_2_derivative; + //float filtered_error_1_derivative = LowPassFilter.step(error_1_derivative); + //float filtered_error_2_derivative = LowPassFilter.step(error_2_derivative); + + u_d_1 = Kd * error_1_derivative; + u_d_2 = Kd * error_2_derivative; + error_1_prev = error_1; error_2_prev = error_2; // Sum all parts and return it - U_1 = u_k_1 + u_i_1 + u_d_1; - U_2 = u_k_2 + u_i_2 + u_d_2; + U_1 = u_k_1; //+ u_i_1 + u_d_1; + U_2 = u_k_2;//+ u_i_2 + u_d_2; + } void motor1() -{ - // This is where Motor 1 is opperated. +{ + // This is where Motor 1 is operated. - float u = U_1; - DirectionPin1 = u < 0.0f; + float u = 0.4f + 0.6f* fabs(U_1); PwmPin1 = fabs(u); + if (U_1 >= 0) + { + DirectionPin1 = false; + } + else if (U_1 < 0) + { + DirectionPin1 = true; + } + } void motor2() -{ - // This is where Motor 2 is opperated. - - float u = U_2; - DirectionPin2 = u > 0.0f; +{ + float u = 0.4f+ 0.6f* fabs(U_2); PwmPin2 = fabs(u); + if (U_2 <= 0) + { + DirectionPin2 = false; + } + else if (U_2 > 0) + { + DirectionPin2 = true; + } + //DirectionPin2 = u > 0.0f; + } void Printing() { // This function is merely for printing feedback from the system to our // screen when we wish to see or check something. - - float v1 = PwmPin1 * maxVelocity; - float v2 = PwmPin2 * maxVelocity; - + if (Active_State == Function )//|| Active_State == Homing_M1) { - pc.printf("q1 = %f [rad] \r\nq2 = %f [rad] \r\ncount1= %i\r\ncount2= %i\r\nq1dot = %f [rad/s] \r\nq2dot = %f [rad/s] \r\n\r\n\r\n\r\n\r\n", rad_m1, rad_m2,counts1, counts2, v1, v2); + pc.printf("U_K1 is %f \r\nU_K2 is %f \r\n\r\n rad_m1 is %f \r\n rad_m2 is %f \r\n\r\n Bi_R is %f \r\n Bi_L is %f \r\n\r\n counts1 is %i \r\n counts2 is %i\r\n\r\n", u_k_1, u_k_2, rad_m1, rad_m2, Filtered_Bi_R, Filtered_Bi_L, counts1, counts2); + //u_k_1 + u_i_1 + u_d_1; + //u_k_2 + u_i_2 + u_d_2; + //pc.printf("q1 = %f [rad] \r\nq2 = %f [rad] \r\ncount1= %i\r\ncount2= %i\r\nq1dot = %f [rad/s] \r\nq2dot = %f [rad/s] \r\n\r\n", rad_m1, rad_m2,counts1, counts2, w_1, w_2); + // pc.printf("U_1 = %f [error] \r\nU_2 = %f [error]\n\r\n",U_1,U_2); + //pc.printf("u_k_1 = %f\r\nu_k_2 = %f\r\nu_i_1 = %f \r\nu_i_2 = %f \r\nu_d_1 = %f \r\nu_d_2 = %f\r\n\r\n",u_k_1,u_k_2,u_i_1,u_i_2,u_d_1,u_d_2); } } @@ -677,6 +750,7 @@ // integration and the opperation of the system a lot simpeler. // We have 5 main states: + // // - Starting // - Start_Up // - Sleep_Mode @@ -730,7 +804,7 @@ else if (Knop1==false) { pc.printf("Manual switch to Homing state \r\n"); - Active_State = Homing_M1; + Active_State = Function; } @@ -744,7 +818,7 @@ OFF_m2(); Encoding(); - if (fabs(rad_m1)>(pi/6.0f) || fabs(rad_m2)>(pi/6.0f)) // pi/4 is a safe value, can/will be editted + if (fabs(rad_m1)>(2.0f*pi/6.0f) || fabs(rad_m2)>(pi/6.0f)) // pi/4 is a safe value, can/will be editted { pc.printf("SAFE MODUS ACTIVE!\r\n RESET MANDATORY!\r\n"); Active_State = Safe; @@ -815,7 +889,7 @@ motor1(); motor2(); - if (fabs(rad_m1)>(pi/6.0f) || fabs(rad_m2)>(pi/6.0f)) // pi/4 is a safe value, can/will be editted + if (fabs(rad_m1)>(2.0f*pi/3.0f) || fabs(rad_m2)>(25.0f*pi/12.0f)) // pi/4 is a safe value, can/will be editted { pc.printf("SAFE MODUS ACTIVE!\r\n RESET MANDATORY!\r\n"); Active_State = Safe; @@ -873,7 +947,7 @@ StateTicker.attach(&StateMachine, 0.002); - printTicker.attach(&Printing, 2); + printTicker.attach(&Printing, 0.5); while(true) {