![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
DemoState with IK and MotorControl and input vx_des and vy_des with potmeter1 and button2
Dependencies: FastPWM MODSERIAL Matrix MatrixMath mbed QEI
Revision 9:7443c37f0f7b, committed 2018-11-06
- Comitter:
- 1856413
- Date:
- Tue Nov 06 20:44:11 2018 +0000
- Parent:
- 8:3d2228402c71
- Commit message:
- Final final
;
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 3d2228402c71 -r 7443c37f0f7b main.cpp --- a/main.cpp Tue Nov 06 20:25:40 2018 +0000 +++ b/main.cpp Tue Nov 06 20:44:11 2018 +0000 @@ -27,10 +27,10 @@ volatile float currentPosition2 = -(0.5*3.14); // Starting position of motor 2 wrt motor 1 [rad] //Inverse Kinematics -const float L0 = 0.1; // Horizontal length from base to first joint [m] -const float L1 = 0.3; // Length link 1 [m] -const float L2 = 0.3; // Length link 2 [m] -const float L3 = 0.1; // Vertical length from base to first joint [m] +const float L0 = 0.1; // Horizontal length from base to first joint [m] +const float L1 = 0.3; // Length link 1 [m] +const float L2 = 0.3; // Length link 2 [m] +const float L3 = 0.1; // Vertical length from base to first joint [m] volatile float q1 = 0.1; // Starting reference angle of first link [rad] volatile float q2 = -(0.5*3.12); // Starting reference angle of second link wrt first link [rad] @@ -58,7 +58,7 @@ volatile float Kp = 0.34; volatile float Ki = 0.0; volatile float Kd = 0.0; -volatile float Ts = 0.02; +volatile float Ts = 0.02; //------------------------------------------------------------------------------ @@ -219,7 +219,7 @@ } } -void MeasureAndInverseK() +void MeasureAndInverseK() { MeasureEncoderPosition1(); // You want to know the current angle of the motors to get the right Jacobian MeasureEncoderPosition2(); // You want to know the current angle of the motors to get the right Jacobian @@ -228,60 +228,71 @@ ComputeQ_set(); // Compute Q_set (stores Q1 and Q2) q1 = IntegrateQ1(); // Compute required angle to go to desired position of end-effector q2 = IntegrateQ2(); // Compute required angle to go to desired position of end-effector - if (q1 < 1.047) { // q1 can only be smaller than 1.047 rad - q1 = q1; - } else { // If value of q1 is greater than 1.047 rad, then stay at maximum angle - q1 = 1.047; + if (q > 0.0) { // q1 cannot be greater than 0 + q = 0.0; + } else if (q < - 1.047) { // q1 cannot be smaller than 60 degrees + q1 = -1.047; + } else { // q1 can be computed q1 between intervals + q1 = q1; + } + if (q2 > -0.61) { // q2 cannot be smaller than 0.61 rad + q2 = -0.61; // Stay at mimimum angle + } else if (q2 < -1.57) { // q2 cannot be greater than 1.57 rad + q2 = -1.57; // Stay at maximum angle + } else { // If q2 is in the right range, let calculated q2 be q2 + q2 = q2; +// Determine error and add PID control + errorIK1 = ErrorInverseKinematics1(); // Determine difference between current angle motor 1 and desired angle + errorIK2 = ErrorInverseKinematics2(); // Determine difference between current angle motor 2 and desired angle +// Determine motorValues + motorValue1 = FeedbackControl1(errorIK1); // Calculate motorValue1 + motorValue2 = FeedbackControl2(errorIK2); // Calculate motorValue2 + if (errorIK1<0.1) { // When error is small enough, stop turning + if (errorIK2<0.1) { + TurnMotorsOff(); + } else { + motor1PWM = 0.0; + SetMotor2(); } - if (q2 < 0.61) { // q2 cannot be smaller than 0.61 rad - q2 = 0.61; // Stay at mimimum angle - } else if (q2 > 1.57) { // q2 cannot be greater than 1.57 rad - q2 = 1.57; // Stay at maximum angle - } else { // If q2 is in the right range, let calculated q2 be q2 - q2 = q2; + } else if (errorIK2<0.1) { + if (errorIK1<0.1) { + TurnMotorsOff(); + } else { + motor2PWM = 0.0; + SetMotor1(); } + } -// Determine error and add PID control - errorIK1 = ErrorInverseKinematics1(); // Determine difference between current angle motor 1 and desired angle - errorIK2 = ErrorInverseKinematics2(); // Determine difference between current angle motor 2 and desired angle -// Determine motorValues - motorValue1 = FeedbackControl1(errorIK1); // Calculate motorValue1 - motorValue2 = FeedbackControl2(errorIK2); // Calculate motorValue2 - if (errorIK1<0.5); - { - TurnMotorsOff(); - } // Make Motor move - SetMotor1(motorValue1); - SetMotor2(motorValue2); + SetMotor1(motorValue1); + SetMotor2(motorValue2); // Press button to switch velocity direction - if (!button2.read()) { //keep pressing button2 - vy_des=vx_des; - vx_des=0.0; - } else { - vy_des=0.0; + if (!button2.read()) { //keep pressing button2 + vy_des=vx_des; + vx_des=0.0; + } else { + vy_des=0.0; + } } -} -/*void PrintValues() -{ - pc.printf("vx_des = %f \t vy_des = %f \r\n", vx_des, vy_des); - pc.printf("Error IK1 = %f \t Error IK2 = %f \r\n", errorIK1, errorIK2); - pc.printf("MotorValue 1 = %f \t MotorValue 2 = %f \r\n", motorValue1, motorValue2); - pc.printf("q1 Qset = %f \t q2 Qset = %f \r\n", Q1, Q2); -}*/ + /*void PrintValues() + { + pc.printf("vx_des = %f \t vy_des = %f \r\n", vx_des, vy_des); + pc.printf("Error IK1 = %f \t Error IK2 = %f \r\n", errorIK1, errorIK2); + pc.printf("MotorValue 1 = %f \t MotorValue 2 = %f \r\n", motorValue1, motorValue2); + pc.printf("q1 Qset = %f \t q2 Qset = %f \r\n", Q1, Q2); + }*/ //------------------------------------------------------------------------------ // MAIN -int main() -{ - pc.baud(115200); - potmeterTicker.attach(GetPotMeterVelocity1, 0.02); - measurecontrolTicker.attach(MeasureAndInverseK, 0.01); - /*printTicker.attach(printen, 0.5);*/ + int main() { + pc.baud(115200); + potmeterTicker.attach(GetPotMeterVelocity1, 0.02); + measurecontrolTicker.attach(MeasureAndInverseK, 0.01); + /*printTicker.attach(printen, 0.5);*/ - while (true) { - Led = !Led; - wait(0.5); - } -} \ No newline at end of file + while (true) { + Led = !Led; + wait(0.5); + } + } \ No newline at end of file