Diff: main.cpp

Revision:

5:aca2af310419

Parent:

4:854aa2e7eeb2

Child:

6:a6f79f31767b

--- a/main.cpp	Thu Nov 01 20:50:17 2018 +0000
+++ b/main.cpp	Sat Nov 03 13:46:07 2018 +0000
@@ -23,50 +23,57 @@
 //------------------------------------------------------------------------------
 //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]
+float currentPosition1 = 0.1;          // Starting position of motor 1 [rad]
+float currentPosition2 = -(0.5*3.14);  // Starting position of motor 2 wrt motor 1 [rad]
 // ^ waarom?
 int a = 0;
 
 //Inverse Kinematics
-const double L0 = 0.1;              // Horizontal length from base to first joint [m]   NAMETEN!!
-const double L1 = 0.3;              // Length link 1 [m]                                NAMETEN!!
-const double L2 = 0.3;              // Length link 2 [m]                                NAMETEN!!
-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]
+const float L0 = 0.1;              // Horizontal length from base to first joint [m]   NAMETEN!!
+const float L1 = 0.3;              // Length link 1 [m]                                NAMETEN!!
+const float L2 = 0.3;              // Length link 2 [m]                                NAMETEN!!
+const float L3 = 0.1;              // Vertical length from base to first joint [m]     NAMETEN!
+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]
 // ^ 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;
+float t = 0.01;                     // Time interval [s]                                SHOULD BE REPLACED          SAMPLING TIJD!!
+volatile float vx_des = 0.0;       // Starting velocity in x-direction [rad/s]
+volatile float vy_des = 0.0;       // Starting velocity in y-direction [rad/s]
+volatile float errorIK1 = 0.0;
+volatile float errorIK2 = 0.0;
+volatile float J1 = 0.0;
+volatile float J2 = 0.0;
+volatile float J3 = 0.0;
+volatile float J4 = 0.0;
+volatile float V1 = 0.0;
+volatile float V2 = 0.0;
+volatile float Q1 = 0.0;
+volatile float Q2 = 0.0;
 
 // Motor Control
-volatile double potMeterPosition1 = 0.0;
-//volatile double potMeterPosition2 = 0.0;
-volatile double motorValue1 = 0.01;
-volatile double motorValue2 = 0.01;
-volatile double Kp = 0.34; //dit maken we variabel, dit zorgt voor een grote of kleine overshoot
-volatile double Ki = 0.0; //dit moeten we bepalen met een plot bijvoorbeeld
-volatile double Kd = 0.0;
-volatile double Ts = 0.01;                      //                                                          SAMPLING TIJD!!
+volatile float potMeterPosition1 = 0.0;
+int counts1i = 0;
+int counts2i = 0;
+//volatile float potMeterPosition2 = 0.0;
+volatile float motorValue1 = 0.01;
+volatile float motorValue2 = 0.01;
+volatile float Kp = 4.0; //dit maken we variabel, dit zorgt voor een grote of kleine overshoot
+volatile float Ki = 0.0; //dit moeten we bepalen met een plot bijvoorbeeld
+volatile float Kd = 0.0;
+volatile float Ts = 0.01;                      //                                                          SAMPLING TIJD!!
 
 
 //------------------------------------------------------------------------------
 // Potmeter values TO DETERMINE VX_DES, VY_DES
 void GetPotMeterVelocity1()  //Potmeter standard to control X-DIRECTION
 {
-    double potMeter1In = potMeter1.read();
-    //vx_des = 0.5*3.14*potMeter1In - 0.25*3.14 ; // Reference value y, scaled to -0.25 to 0.25 revolutions
-    vx_des = 3.7;
+    float potMeter1In = potMeter1.read();
+    vx_des = 4.0*3.14*potMeter1In - 2.0*3.14; // Reference value y, scaled to -0.25 to 0.25 revolutions
+    //vx_des = 0.5;
     //return vx_des;
 }
 
-double SwitchPotmeterVelocity1()    //Button has been pressed,
+float SwitchPotmeterVelocity1()    //Button has been pressed,
 {
     vy_des = vx_des;
     return vy_des;
@@ -75,116 +82,101 @@
 //------------------------------------------------------------------------------
 // KINEMATIC FUNCTIONS
 
-Matrix ComputeJ(void)
+void ComputeJ(void)
 {
-    double a = -sin(q2)/(L1*cos(q1)*sin(q2)-L1*cos(q2)*sin(q1));
-    double b = cos(q2)/(L1*cos(q1)*sin(q2)-L1*cos(q2)*sin(q1));
-    double c = (L1*sin(q1)+L2*sin(q2))/(L1*L2*cos(q1)*sin(q2)-L1*L2*cos(q2)*sin(q1));
-    double d = -(L1*cos(q1)+L2*cos(q2))/(L1*L2*cos(q1)*sin(q2)-L1*L2*cos(q2)*sin(q1));
-    J << a << b
-      << c << d;
-    return J;
+    J1 = -sin(currentPosition1)/(L1*cos(currentPosition1)*sin(currentPosition2)-L1*cos(currentPosition2)*sin(currentPosition1));
+    J2 = cos(currentPosition2)/(L1*cos(currentPosition1)*sin(currentPosition2)-L1*cos(currentPosition2)*sin(currentPosition1));
+    J3 = (L1*sin(currentPosition1)+L2*sin(currentPosition2))/(L1*L2*cos(currentPosition1)*sin(currentPosition2)-L1*L2*cos(currentPosition2)*sin(currentPosition1));
+    J4 = -(L1*cos(currentPosition1)+L2*cos(currentPosition2))/(L1*L2*cos(currentPosition1)*sin(currentPosition2)-L1*L2*cos(currentPosition2)*sin(currentPosition1));
 }
 
-Matrix ComputeV(void)
+void ComputeV(void)
 {
-    V.add(1,1,vx_des); // Add desired x-velocity in V
-    V.add(2,1,vy_des); // Add desired y-velocity in V
-    return V;
+    V1 = vx_des;
+    V2 = vy_des;
 }
 
-double IntegrateQ1()
+float IntegrateQ1()
 {
-    q1  = q1 + (Q_set(1,1))*t;    // new value for q1
+    q1  = q1 + (Q1*t);    // new value for q1
     return q1;
 }
 
-double IntegrateQ2()
+float IntegrateQ2()
 {
-    q2 = q2 + (Q_set(2,1))*t;    // new value for q2
+    q2 = q2 + (Q2*t);    // new value for q2
     return q2;
 }
 
-Matrix ComputeQ_set(void)
+void ComputeQ_set()
 {
-    float a = J(1,1);
-    float b = J(1,2);
-    float c = J(2,1);
-    float d = J(2,2);
-    float e = V(1,1);
-    float f = V(2,1);
-    float first_row = a*e + b*f;
-    float second_row = c*e + d*f;
-    Q_set.add(1,1,first_row);
-    Q_set.add(2,1,second_row);
-    return Q_set;
+    Q1 = (J1*V1) + (J2*V2);
+    Q2 = (J3*V1) + (J4*V2);
 }
 
-double ErrorInverseKinematics1()
+float ErrorInverseKinematics1()
 {
-    double errorIK1 = q1 - currentPosition1;
+    float errorIK1 = q1 - currentPosition1;
     return errorIK1;
 }
 
-double ErrorInverseKinematics2()
+float ErrorInverseKinematics2()
 {
-    double errorIK2 = q2 - currentPosition2;
+    float errorIK2 = q2 - currentPosition2;
     return errorIK2;
 }
 //------------------------------------------------------------------------------
 // MOTOR PART
 //Encoder Posities
-double MeasureEncoderPosition1()        // Read current position of motor 1, returns value in [rad]
+void MeasureEncoderPosition1()        // Read current position of motor 1, returns value in [rad]
 {
-    int counts1i = Encoder1.getPulses();
-    double counts1 = counts1i*1.0f;
-    double measuredPosition1 = (counts1/8400)*6.28; //Rotational position in radians
-    return measuredPosition1;
+    counts1i = Encoder2.getPulses();
+    float counts1 = counts1i*1.0f;
+    currentPosition1 = (counts1/8400)*6.28; //Rotational position in radians
 }
 
-double MeasureEncoderPosition2()        // Read current postion of motor 2, returns value in [rad]
+void MeasureEncoderPosition2()        // Read current postion of motor 2, returns value in [rad]
 {
-    int counts2i = Encoder2.getPulses();
-    double counts2 = counts2i*1.0f;
-    double measuredPosition2 = (counts2/8400)*6.28; //Rotational position in radians
-    return measuredPosition2;
+    counts2i = Encoder2.getPulses();
+    float counts2 = counts2i*1.0f;
+    currentPosition2 = (counts2/8400)*6.28; //Rotational position in radians
 }
 
-double FeedbackControl1(double Error1)
+float FeedbackControl1(float Error1)
 {
-    static double Error_integral1 = 0;
-    static double Error_prev1 = Error1;
+    static float Error_integral1 = 0;
+    static float Error_prev1 = Error1;
     // Proportional part:
-    double u_k1 = Kp * Error1;
+    float u_k1 = Kp * Error1;
     // Integral part:
     Error_integral1 = Error_integral1 + Error1 * Ts;
-    double u_i1 = Ki * Error_integral1;
+    float u_i1 = Ki * Error_integral1;
     // Derivative part:
-    double Error_derivative1 = (Error1 - Error_prev1)/Ts;
-    double u_d1 = Kd * Error_derivative1;
+    float Error_derivative1 = (Error1 - Error_prev1)/Ts;
+    float u_d1 = Kd * Error_derivative1;
     Error_prev1 = Error1;
     // Sum all parts and return it
     return u_k1 + u_i1 + u_d1; //motorValue
 }
 
-double FeedbackControl2(double Error2)
+float FeedbackControl2(float Error2)
 {
-    static double Error_integral2 = 0;
-    static double Error_prev2 = Error2;
+    static float Error_integral2 = 0;
+    static float Error_prev2 = Error2;
     // Proportional part:
-    double u_k2 = Kp * Error2;
+    float u_k2 = Kp * Error2;
     // Integral part:
     Error_integral2 = Error_integral2 + Error2 * Ts;
-    double u_i2 = Ki * Error_integral2;
+    float u_i2 = Ki * Error_integral2;
     // Derivative part:
-    double Error_derivative2 = (Error2 - Error_prev2)/Ts;
-    double u_d2 = Kd * Error_derivative2;
+    float Error_derivative2 = (Error2 - Error_prev2)/Ts;
+    float u_d2 = Kd * Error_derivative2;
     Error_prev2 = Error2;
     // Sum all parts and return it
     return u_k2 + u_i2 + u_d2; //motorValue
 }
 
-void SetMotor1(double motorValue1)
+void SetMotor1(float motorValue1)
 {
     // Given -1<=motorValue<=1, this sets the PWM and direction
     // bits for motor 1. Positive value makes motor rotating
@@ -204,7 +196,7 @@
     }
 }
 
-void SetMotor2(double motorValue2)
+void SetMotor2(float motorValue2)
 {
     // Given -1<=motorValue<=1, this sets the PWM and direction
     // bits for motor 1. Positive value makes motor rotating
@@ -247,43 +239,45 @@
 }*/
 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)
+    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
+    ComputeJ();                                 // Compute matrix J (inverse Jacobian, and Twist is already left out)
+    ComputeV();                                 // Compute matrix V (stores the desired velocities obtained from the EMG signal)
+    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
+    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;
-    } 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
+    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
+// 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
+// Determine motorValues
     motorValue1 = FeedbackControl1(errorIK1);                       // Calculate motorValue1
     motorValue2 = FeedbackControl2(errorIK2);                       // Calculate motorValue2
-    // Make Motor move
+// Make Motor move
     SetMotor1(motorValue1);
     SetMotor2(motorValue2);
 
-    // Press button to switch velocity direction
-    //if (!button2.read()) {
+// Press button to switch velocity direction
+//if (!button2.read()) {
     if (!button2.read()) { //button2 blijven indrukken
         //SwitchPotmeterVelocity1();
         vy_des=vx_des;
-        vx_des=0;
+        vx_des=0.0;
     } else {
-        vy_des=0;
+        vy_des=0.0;
     }
 }
 
@@ -292,19 +286,20 @@
     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));
+    pc.printf("q1  Qset = %f \t q2 Qset = %f \r\n", Q1, Q2);
 }
 //------------------------------------------------------------------------------
 // MAIN
 int main()
 {
     pc.baud(115200);
-    potmeterTicker.attach(GetPotMeterVelocity1, 0.01f);
-    allesineenticker.attach(AllesinEen, 0.01f);
-    printTicker.attach(printen, 0.5f);
+    potmeterTicker.attach(GetPotMeterVelocity1, 0.01);
+    allesineenticker.attach(AllesinEen, 0.01);
+    printTicker.attach(printen, 0.5);
 
     while (true) {
-
+        Led = !Led;
+        wait(0.5);
         //wait(0.01);
     }
 }
\ No newline at end of file