BMT Module 9 Group 4
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