Diff: main_demo.cpp

Revision:

0:755bc7c0f555

Child:

1:7afdfab34bf7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main_demo.cpp	Wed Oct 31 10:44:10 2018 +0000
@@ -0,0 +1,266 @@
+#include "mbed.h"
+#include "FastPWM.h"
+#include "MODSERIAL.h"
+#include "QEI.h"
+#include "math.h"
+
+// --------------------------------------------------
+// ----------------- SET UP -------------------------
+QEI Encoder1(D11, D10, NC, 4200) ;  // Encoder motor 1, (pin 1A, pin 1B, counts/rev)
+QEI Encoder2(D9, D8, NC, 4200) ;    // Encoder motor 2, (pin 2A, pin 2B, counts/rev)
+DigitalOut directionM1(D4);
+DigitalOut directionM2(D7);
+FastPWM motor1_pwm(D5);
+FastPWM motor2_pwm(D6);
+MODSERIAL  pc(USBTX, USBRX);
+
+// Tickers
+Ticker Demo;
+Ticker Write;
+
+// ---------------------------------------------------
+// ----------------- GLOBAL VARIABLES ----------------
+volatile int counts1; 
+volatile int counts2;
+volatile double theta1;
+volatile double theta2;
+const double  pi = 3.14159265359;
+volatile double  error1;
+volatile double  error2;
+double  point1x = 200.0;
+double  point1y = 200.0;
+double  point2x = 350.0;
+double  point2y = 200.0;
+double  point3x = 350.0;
+double  point3y = 0.0;
+double  point4x = 200.0;
+double  point4y = 0.0;
+int track;
+const double x0 = 80.0; //zero x position after homing
+const double y0 = 141.0; //zero y position after homing
+volatile double  setpointx = x0;
+volatile double  setpointy = y0;
+volatile double  U1;
+volatile double  U2;
+
+// Inverse Kinematics
+volatile double q1_diff;
+volatile double q2_diff;
+double sq = 2.0; //to square numbers
+const double L1 = 250.0; //length of the first link
+const double L3 = 350.0; //length of the second link
+
+// Reference angles of the starting position
+double q2_0 = pi + acos((pow(x0,sq)+pow(y0,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3));
+double q1_0 = atan(y0/x0)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x0,sq)+pow(y0,sq))/(2.0*L1*sqrt(pow(x0,sq)+pow(y0,sq))));
+double q2_0_enc = q2_0 + q1_0;
+
+// --------------------------------------------------------------------
+// ---------------Read out encoders------------------------------------
+// --------------------------------------------------------------------    
+double  counts2angle1()
+{
+    counts1 = Encoder1.getPulses();         // Counts of outputshaft of motor 1
+    theta1 = -(double(counts1)/4200) * 2*pi;       // Angle of outputshaft of motor 1
+    return theta1;
+}
+
+double counts2angle2()
+{
+    counts2 = Encoder2.getPulses();         // Counts of outputshaft of motor 2
+    theta2 = (double(counts2)/4200) * 2*pi;       // Angle of outputshaft of motor 2
+    return theta2;
+}
+
+// -------------------------------------------------------------------------
+// -------------- Determine Setpoints --------------------------------------
+// -------------------------------------------------------------------------
+double determinedemosetx(double setpointx, double setpointy)
+{
+
+
+    
+    
+ 
+        /*
+    // Van punt 3 naar punt 4. 
+    if (setpointy >= point3y - 0.3 && setpointx >= point3x - 0.3 && setpointy <= point3y + 0.3 && setpointx <= point3x + 0.3)
+    {
+        setpointx = setpointx - 0.1;          // Van punt 1 naar punt 2 op dezelfde y blijven. 
+        track = 34;
+    }
+    if (setpointy > point3y && track == 34)
+    {
+        setpointx = setpointx - 0.1;
+    }
+        
+    if (setpointy >= point4y - 0.3 && setpointx >= point4x - 0.3 && setpointy <= point4y + 0.3 && setpointx <= point4x + 0.3)
+    {
+        setpointx = 80.0;
+    }
+    */
+      return setpointx;
+}  
+
+double determinedemosety(double &setpointx, double &setpointy)
+{
+    // Van reference positie naar punt 1.
+    if(setpointy < point1y){
+        setpointy = setpointy + 0.2;
+    } 
+
+    if (setpointx < point1x){ 
+        setpointx = setpointx + 0.1;    
+    }
+    
+    // Van punt 1 naar punt 2. 
+    if (setpointy >= point1y - 0.3 && setpointx >= point1x - 0.3 && setpointy <= point1y + 0.3 && setpointx <= point1x + 0.3){
+        setpointx = setpointx + 0.1;
+        setpointy = point2y;          // Van punt 1 naar punt 2 op dezelfde y blijven. 
+        track = 12;
+    }
+    if (setpointx < point2x && track == 12){
+        setpointx = setpointx + 0.2;
+        setpointy = point2y;
+    }
+    
+    // Van punt 2 naar punt 3. 
+    if (setpointx >= (point2x-0.2))
+    {
+        setpointx = point3x; 
+        setpointy = setpointy - 0.2;
+        track = 23;
+    }
+    if (setpointy > point3y && track == 23)
+    {
+        setpointx = point3x;          // Van punt 1 naar punt 2 op dezelfde y blijven. 
+        setpointy = setpointy - 0.2;
+        track = 23;
+    }    
+    
+
+ /* 
+    // Van punt 3 naar punt 4. 
+    if (setpointy >= point3y - 0.3 && setpointx >= point3x - 0.3 && setpointy <= point3y + 0.3 && setpointx <= point3x + 0.3)
+    {
+        setpointy = setpointy;
+        track = 34;
+    }
+    if (setpointy > point3y && track == 34)
+    {
+        setpointy = setpointy;
+    }     
+    
+    
+    if (setpointy >= point4y - 0.3 && setpointx >= point4x - 0.3 && setpointy <= point4y + 0.3 && setpointx <= point4x + 0.3)
+    {
+        setpointy = 141.0;
+    }
+    */ 
+    return setpointy;
+    
+}
+
+// -----------------------------------------------------------------    
+// --------------------------- PI controllers ----------------------
+// -----------------------------------------------------------------
+double PI_controller1(double error1)
+{
+    static double error_integral1 = 0;
+    
+    // Proportional part
+    double Kp1 = 3.95;              // Kp (proportionele controller, nu nog een random waarde)
+    double u_p1 = Kp1*error1;        // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
+    
+    // Integral part
+    double Ki1 = 6.0;             // Ki (Integrale deel vd controller, nu nog een random waarde)
+    double Ts1 = 0.005;           // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)   
+    error_integral1 = error_integral1 + error1 * Ts1;  
+    double u_i1 = Ki1 * error_integral1;
+    
+    // Sum 
+    U1 = u_p1 + u_i1;
+    
+    // Return
+    return U1;      
+}
+double PI_controller2(double error2)
+{
+    static double error_integral2 = 0;
+    
+    // Proportional part
+    double Kp2 = 3.95;              // Kp (proportionele controller, nu nog een random waarde)
+    double u_p2 = Kp2*error2;        // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
+    
+    // Integral part
+    double Ki2 = 6.0;             // Ki (Integrale deel vd controller, nu nog een random waarde)
+    double Ts2 = 0.005;           // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)   
+    error_integral2 = error_integral2 + error2 * Ts2;  
+    double u_i2 = Ki2 * error_integral2;
+    
+    // Sum +
+    U2 = u_p2 + u_i2;
+    
+    // Return
+    return U2;      
+} 
+// ------------------------------------------------------------
+// ------------ Inverse Kinematics ----------------------------
+// ------------------------------------------------------------
+double makeAngleq1(double x, double y){
+    double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration
+    q1_diff = -2.0*(q1-q1_0);                                                                                    //the actual amount of radians that the motor has to turn in total to reach the setpoint
+    return q1_diff;
+}
+
+double makeAngleq2(double x, double y){
+    double q2 = -acos((pow(x,sq)+pow(y,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3)); //angle of the second joint in setpoint configuration
+    double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration
+    double q2_motor = (pi - q2)+q1; //because q2 represents the angle at joint two and not at the motor a calculation has to be done
+    q2_diff = (2.0*(q2_motor - q2_0_enc))/(2.0*pi); //the actual amount of radians that the motor has to turn in total to reach the setpoint
+    return -q2_diff;
+}
+
+
+// -----------------------------------------------
+// ------------ RUN MOTORS -----------------------
+// -----------------------------------------------
+void motoraansturing()
+{
+    setpointx = determinedemosetx(setpointx, setpointy);
+    setpointy = determinedemosety(setpointx, setpointy);
+    q1_diff = makeAngleq1(setpointx, setpointy);
+    q2_diff = makeAngleq2(setpointx, setpointy);
+
+    theta2 = counts2angle2();           
+    error2 = q2_diff - theta2;
+    theta1 = counts2angle1();  
+    error1 = q1_diff - theta1;          // Setpoint error, te behalen setpoint minus de huidige positie van de as.
+
+    U1 = PI_controller1(error1);       // Voltage dat naar de motor gestuurd wordt. 
+    U2 = PI_controller2(error2);   
+    
+    motor1_pwm.write(fabs(U1));         // Motor aansturen
+    directionM1 = U1 > 0.0f;            // Richting van de motor bepalen
+    motor2_pwm.write(fabs(U2));
+    directionM2 = U2 > 0.0f; 
+}
+
+
+void rundemo()
+{
+    motoraansturing();
+}
+
+
+int main()
+{
+    pc.baud(115200);
+    motor1_pwm.period_us(60);                // Period is 60 microseconde
+    motor2_pwm.period_us(60);
+    Demo.attach(&rundemo, 0.005f); 
+    
+    while (true) {
+
+    }
+}    
\ No newline at end of file