Diff: main.cpp

Revision:

4:854aa2e7eeb2

Parent:

3:d16182dd3a2a

Child:

5:aca2af310419

--- a/main.cpp	Thu Nov 01 20:15:22 2018 +0000
+++ b/main.cpp	Thu Nov 01 20:50:17 2018 +0000
@@ -18,12 +18,14 @@
 
 // Tickers
 Ticker potmeterTicker;
-
+Ticker allesineenticker;
+Ticker printTicker;
 //------------------------------------------------------------------------------
 //Global Variables
- 
+
 double currentPosition1 = 0.0;          // Starting position of motor 1 [rad]
 double currentPosition2 = -(0.5*3.14);  // Starting position of motor 2 wrt motor 1 [rad]
+// ^ waarom?
 int a = 0;
 
 //Inverse Kinematics
@@ -33,12 +35,15 @@
 const double L3 = 0.1;              // Vertical length from base to first joint [m]     NAMETEN!
 volatile double q1 = 0.0;           // Starting reference angle of first link [rad]
 volatile double q2 = -(0.5*3.14);   // Starting reference angle of second link wrt first link [rad]
+// ^ start buiten bereik, waarom?
 double t = 2.0;                     // Time interval [s]                                SHOULD BE REPLACED          SAMPLING TIJD!!
 volatile double vx_des = 0.0;       // Starting velocity in x-direction [rad/s]
 volatile double vy_des = 0.0;       // Starting velocity in y-direction [rad/s]
 Matrix Q_set(2,1);                  // Setting a matrix for storing the angular velocities [rad/sec]
 Matrix J(2,2);                      // Setting a matrix for the Jacobian
 Matrix V(2,1);                      // Setting a matrix for storing the EMG-measured velocities
+volatile double errorIK1 = 0.0;
+volatile double errorIK2 = 0.0;
 
 // Motor Control
 volatile double potMeterPosition1 = 0.0;
@@ -61,7 +66,8 @@
     //return vx_des;
 }
 
-double SwitchPotmeterVelocity1() {  //Button has been pressed,
+double SwitchPotmeterVelocity1()    //Button has been pressed,
+{
     vy_des = vx_des;
     return vy_des;
 }
@@ -119,7 +125,7 @@
     double errorIK1 = q1 - currentPosition1;
     return errorIK1;
 }
- 
+
 double ErrorInverseKinematics2()
 {
     double errorIK2 = q2 - currentPosition2;
@@ -239,57 +245,66 @@
     motorValue2 = FeedbackControl2(potMeterPosition2 - currentPosition2);
     SetMotor2(motorValue2);
 }*/
+void AllesinEen() //HIER GAAT IETS MIS!!
+{
+    currentPosition1 = MeasureEncoderPosition1();   // You want to know the current angle of the motors to get the right Jacobian
+    currentPosition2 = MeasureEncoderPosition2();   // You want to know the current angle of the motors to get the right Jacobian
+    J = ComputeJ();                                 // Compute matrix J (inverse Jacobian, and Twist is already left out)
+    V = ComputeV();                                 // Compute matrix V (stores the desired velocities obtained from the EMG signal)
+    Q_set = 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 (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
+    // Make Motor move
+    SetMotor1(motorValue1);
+    SetMotor2(motorValue2);
 
+    // Press button to switch velocity direction
+    //if (!button2.read()) {
+    if (!button2.read()) { //button2 blijven indrukken
+        //SwitchPotmeterVelocity1();
+        vy_des=vx_des;
+        vx_des=0;
+    } else {
+        vy_des=0;
+    }
+}
+
+void printen()
+{
+    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", Q_set(1,1), Q_set(2,1));
+}
 //------------------------------------------------------------------------------
 // MAIN
 int main()
 {
     pc.baud(115200);
-    potmeterTicker.attach(GetPotMeterVelocity1, 0.01);
-    
+    potmeterTicker.attach(GetPotMeterVelocity1, 0.01f);
+    allesineenticker.attach(AllesinEen, 0.01f);
+    printTicker.attach(printen, 0.5f);
+
     while (true) {
-        currentPosition1 = MeasureEncoderPosition1();   // You want to know the current angle of the motors to get the right Jacobian
-        currentPosition2 = MeasureEncoderPosition2();   // You want to know the current angle of the motors to get the right Jacobian
-        J = ComputeJ();                                 // Compute matrix J (inverse Jacobian, and Twist is already left out)
-        V = ComputeV();                                 // Compute matrix V (stores the desired velocities obtained from the EMG signal)
-        Q_set = 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 (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
-        double errorIK1 = ErrorInverseKinematics1();       // Determine difference between current angle motor 1 and desired angle
-        double errorIK2 = ErrorInverseKinematics2();        // Determine difference between current angle motor 2 and desired angle
-        // Determine motorValues
-        motorValue1 = FeedbackControl1(errorIK1);                       // Calculate motorValue1
-        motorValue2 = FeedbackControl2(errorIK2);                       // Calculate motorValue2
-        // Make Motor move
-        SetMotor1(motorValue1);
-        SetMotor2(motorValue2);
 
-        // Press button to switch velocity direction
-        //if (!button2.read()) {
-        if (!button2.read()) { //button2 blijven indrukken
-            //SwitchPotmeterVelocity1();
-            vy_des=vx_des;
-            vx_des=0;
-        } else {
-            vy_des=0;
-        }
-        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", Q_set(1,1), Q_set(2,1));
-        wait(0.01);
+        //wait(0.01);
     }
 }
\ No newline at end of file