Diff: main.cpp

Revision:

0:550f6e86da32

Child:

1:2219a519e2bf

diff -r 000000000000 -r 550f6e86da32 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Nov 01 16:52:17 2018 +0000
@@ -0,0 +1,269 @@
+#include "mbed.h"
+#include "FastPWM.h"
+#include "MODSERIAL.h"
+#include "Matrix.h"
+#include "QEI.h"
+#include <math.h>
+MODSERIAL pc(USBTX, USBRX);
+DigitalOut motor1DirectionPin(D7);
+DigitalOut motor2DirectionPin(D4);
+DigitalOut Led(LED_GREEN);
+DigitalIn button2(SW3);
+FastPWM motor1MagnitudePin(D6);
+FastPWM motor2MagnitudePin(D5);
+AnalogIn potMeter1(A4);
+AnalogIn potMeter2(A5);
+InterruptIn button2(D3);
+QEI Encoder1 (D12, D13, NC, 64, QEI::X4_ENCODING);
+QEI Encoder2 (D10, D11, NC, 64, QEI::X4_ENCODING);
+
+// Tickers
+Ticker startMotor;
+Ticker printTicker;
+
+//------------------------------------------------------------------------------
+//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]
+double t = 2.0;                     // Time interval [s]                                SHOULD BE REPLACED          SAMPLING TIJD!!
+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
+ 
+// 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!!
+ 
+
+//------------------------------------------------------------------------------
+// Potmeter values TO DETERMINE VX_DES, VY_DES
+double GetPotMeterPosition1()      // Measure the current Potmeter1 value
+{
+    double potMeter1In = potMeter1.read();
+    potMeterPosition1 = 8.0*3.14*potMeter1In - 4.0*3.14 ; // Reference value y, scaled to -4 to 4 revolutions
+    return potMeterPosition1;
+}
+ 
+double GetPotMeterPosition2()      // Measure the current Potmeter2 value
+{
+    double potMeter2In = potMeter2.read();
+    potMeterPosition2 = 8.0*3.14*potMeter2In - 4.0*3.14 ;
+    return potMeterPosition2;
+}
+
+// Drive the motor
+double FeedbackControl1(double Error1)      //                                  Dit moet zo geschreven worden dat het zowel met EMG als met potmeters gebruikt kan worden
+{
+    static double Error_integral1 = 0;
+    static double Error_prev1 = Error1;
+    //static BiQuad LowPassFilter(..., ..., ..., ..., ...)
+    // Proportional part:
+    //van 0 tot 20, waardes rond de 5 zijn het beste (minder overshoot + minder trilling motor beste combinatie hiervan)
+    double u_k1 = Kp * Error1;
+    // Integral part:
+    Error_integral1 = Error_integral1 + Error1 * Ts;
+    double u_i1 = Ki * Error_integral1;
+    // Derivative part
+    double Error_derivative1 = (Error1 - Error_prev1)/Ts;
+    double 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)
+{
+    static double Error_integral2 = 0;
+    static double Error_prev2 = Error2;
+    //static BiQuad LowPassFilter(..., ..., ..., ..., ...)
+    // Proportional part:
+    //van 0 tot 20, waardes rond de 5 zijn het beste (minder overshoot + minder trilling motor beste combinatie hiervan)
+    double u_k2 = Kp * Error2;
+    // Integral part:
+    Error_integral2 = Error_integral2 + Error2 * Ts;
+    double u_i2 = Ki * Error_integral2;
+    // Derivative part
+    double Error_derivative2 = (Error2 - Error_prev2)/Ts;
+    double 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)
+{
+    // Given -1<=motorValue<=1, this sets the PWM and direction
+    // bits for motor 1. Positive value makes motor rotating
+    // clockwise. motorValues outside range are truncated to
+    // within range
+    // 0 = clockwise motor direction
+    // 1 = counterclockwise motor direction
+    if (motorValue1 >=0) {
+        motor1Direction=0;
+    } else {
+        motor1Direction=1;
+    }
+    if (fabs(motorValue1)>1) {
+        motor1PWM = 1;
+    } else {
+        motor1PWM = fabs(motorValue1);
+    }
+}
+ 
+void SetMotor2(double motorValue2)
+{
+    // Given -1<=motorValue<=1, this sets the PWM and direction
+    // bits for motor 1. Positive value makes motor rotating
+    // clockwise. motorValues outside range are truncated to
+    // within range
+    // 0 = counterclockwise motor direction
+    // 1 = clockwise motor direction
+    if (motorValue2 >=0) {
+        motor2Direction=1;
+    } else {
+        motor2Direction=0;
+    }
+    if (fabs(motorValue2)>1) {
+        motor2PWM = 1;
+    } else {
+        motor2PWM = fabs(motorValue2);
+    }
+}
+ 
+void MeasureAndControl1(void)
+{
+    // This function determines the desired velocity, measures the
+    // actual velocity, and controls the motor with
+    // a simple Feedback controller. Call this from a Ticker.
+    potMeterPosition1 = q1;
+    currentPosition1 = MeasureEncoderPosition1();
+    motorValue1 = FeedbackControl1(potMeterPosition1 - currentPosition1);
+    SetMotor1(motorValue1);
+}
+ 
+void MeasureAndControl2(void)
+{
+    // This function determines the desired velocity, measures the
+    // actual velocity, and controls the motor with
+    // a simple Feedback controller. Call this from a Ticker.
+    potMeterPosition2 = q2;
+    currentPosition2 = MeasureEncoderPosition2();
+    motorValue2 = FeedbackControl2(potMeterPosition2 - currentPosition2);
+    SetMotor2(motorValue2);
+    
+//------------------------------------------------------------------------------
+// Kinematic functions
+
+Matrix 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;
+}
+
+Matrix 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;
+}
+
+double IntegrateQ1(){
+    q1  = q1 + (Q_set(1,1))*t;    // new value for q1
+    return q1;
+}
+
+double IntegrateQ2(){
+    q2 = q2 + (Q_set(2,1))*t;    // new value for q2
+    return q2;
+}
+
+Matrix ComputeQ_set(void){
+    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;
+}
+
+int main(){
+    //led = 0;
+    pc.baud(115200);
+    pc.printf("\r\nDoet het script het?\r\n");
+    // Compute Jacobian
+    J = ComputeJ();
+    J.print();
+    pc.printf("\r\n");
+    // Compute velocities
+    V = ComputeV();
+    V.print();
+    pc.printf("\r\n");
+    // Multiplying matrix J and V and storing in array Q_set
+    Q_set = ComputeQ_set();
+
+    // velocity to position
+    q1 = IntegrateQ1();
+    q2 = IntegrateQ2();
+
+    /*if (q1 > ... && < ...)  {
+        TurnMotorsOff;
+        /go to failure mode
+    }
+    if (q2 > ... && < ...)  {
+        TurnMotorsOff/go to failure mode
+    }*/
+
+    pc.printf("New position q1: %f, New Position q2: %f", q1, q2);
+
+}
+
+int main() {
+    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)
+    IntegrateQ1();                                  // Compute required angle to go to desired position of end-effector
+    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);
+                
+    while (true){}
+}
\ No newline at end of file