DemoState with IK and MotorControl and input vx_des and vy_des with potmeter1 and button2

Dependencies:   FastPWM MODSERIAL Matrix MatrixMath mbed QEI

Files at this revision

API Documentation at this revision

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