Diff: main.cpp

Revision:

0:077896c03576

Child:

1:dcc0ad8f6477

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 31 12:52:29 2017 +0000
@@ -0,0 +1,213 @@
+#include "mbed.h"
+#include "encoder.h"
+#include "MODSERIAL.h"
+#include "math.h"
+
+#define     M_Pi 3.141592653589793238462643383279502884L
+
+Serial      pc(USBTX, USBRX);
+
+DigitalOut  led_red(LED_RED);
+DigitalOut  led_blue(LED_BLUE);
+InterruptIn button1(D2);
+InterruptIn button2(D3);
+AnalogIn    potmeter1(A0);
+AnalogIn    potmeter2(A1);
+
+DigitalOut  motor1DirectionPin(D4);
+PwmOut      motor1MagnitudePin(D5);
+DigitalOut  motor2DirectionPin(D7); // Sequence? Lines 181-183
+PwmOut      motor2MagnitudePin(D6);
+
+Ticker      measureTick;
+
+Encoder     motor1(D13,D12); //On the shield actually M2
+Encoder     motor2(D11,D10); //On the shield actually M1 (Production mistake?)
+
+bool         switch1 = 1; // manual switch for when to start calculations (later removed for a state machine
+bool         direction1 = 1; // direction positive, 0 is negative
+bool         direction2 = 1;
+const double RAD_PER_PULSE = 0.002991; // Value for RAD_PER_PULSE given through the slides (wrong?)
+const double DEG_PER_RAD = 180 / M_Pi; // Basic knowledge of how many degrees are in 1 radian.
+
+double       q1 = 0;                // Angle of arm 1 (upper) in starting position is 0 degrees
+double       q2 = 179/DEG_PER_RAD;  // Angle of arm 2 (lower) in starting position is 180 degrees (but can't be 0 or 180 because of determinant = 0)
+int          L1 = 47;               // Length of arm 1 (upper) in cm
+int          L2 = 29;               // Length of arm 2 (lower) in cm
+double       xdes = L1-L2;           // Desired x coordinate, arm is located at x = L1-L2 in starting position
+double       ydes = 0;              // Disired y coordinate, arm is located at y = 0 in starting position
+double       MotorValue1 = 0;
+double       MotorValue2 = 0;
+
+// Sample time (motor1-timestep)
+const double M1_Ts = 0.01; //100 Hz systems
+const double M2_Ts = 0.01;
+
+// Controller gains (motor1-Kp,-Ki,...)
+const double M1_Kp = 0.1, M1_Ki = 0.02, M1_Kd = 0.00125,  M1_N = 100; // THESE VALUES ARE ARBITRARY AT THIS POINT
+const double M2_Kp = 0.1, M2_Ki = 0.02, M2_Kd = 0.00125,  M2_N = 100; // Inspired by the Ziegler-Nichols Method
+
+// Filter variables (motor1-filter-v1,-v2)
+double M1_f_v1 = 0.0, M1_f_v2 = 0.0;
+double M2_f_v1 = 0.0, M2_f_v2 = 0.0;
+
+// PROGRAM THAT CALCULATES ANGLE CHANGES
+
+//Xdes and Ydes changer
+void Counter(double &des, double dir, double sig){
+    if (sig == 0){
+        if (dir == 1)
+            des = des + 0.1;
+        else if (dir == 0)
+            des = des - 0.1;
+    }
+}
+
+//Kinematic model
+void Kinematic_referencer( double &xdes, double &ydes, double &q1, double &q2)
+    {
+    double qt = q1 + q2;                               // current total angle
+    double xcurrent = L1 * cos (q1) + L2 * cos (qt);   // current x position
+    double ycurrent = L1 * sin (q1) + L2 * sin (qt);   // current y position
+    
+    //Initial twist
+    double vx = (xdes - xcurrent)/1;  // Running on 100 Hz
+    double vy = (ydes - ycurrent)/1;
+    
+    //Jacobians
+    double J11 = -ycurrent;
+    double J12 = -L2 * sin (qt);
+    double J21 = xcurrent;
+    double J22 = L2 * cos (qt);
+    double Determinant = J11 * J22 - J21 * J12;   // calculate determinant
+    
+    pc.printf("D = %.3f \r\n", Determinant);
+ 
+    //Calculate angular velocities
+    double q1der = (J22 * vx - J12 * vy) / Determinant;
+    double q2der = (-J21 * vx + J11 * vy) / Determinant;
+    
+    //Calculate new angles
+    double q1new = q1 + q1der/100;    //nog fixen met die tijdstappen?
+    double q2new = q2 + q2der/100;      //hier ook
+    //printf ("q1=%f, q2=%f\n", q1 * c, q2 * c);
+    double qtnew = q1new + q2new;
+    
+    //Calculate new positions
+    double xnew = L1 * cos (q1new) + L2 * cos (qtnew);
+    double ynew = L1 * sin (q1new) + L2 * sin (qtnew);
+    //printf ("x=%f, y=%f\n", x, y);
+    
+    // Now check whether the calculated position is desired, determinants close to zero may cause the robot to move weirdly
+    // New y may not be negative, this means the arm is located in on the plate
+    // New q1 may not be less than -45 degrees, less means the arm will crash into the base plate
+    // New q2 may not be more than 185 degrees, more means the lower arm will crash into the upper arm
+    if (ynew > -5 && q1new > -45 / DEG_PER_RAD && q2new < 185 / DEG_PER_RAD )//&& Determinant < 0.01)
+    {
+        // If desired, change the angles
+        q1 = q1new;
+        q2 = q2new;
+    }
+    else
+    {
+        // If not desired, don't change the angles, but define current position as desired so the robot ignores the input
+        xdes = xcurrent;
+        ydes = ycurrent;
+    }
+}
+
+// PROGRAM THAT CALCULATES THE PID
+double PID( double err, const double Kp, const double Ki, const double Kd,
+const double Ts, const double N, double &v1, double &v2 ) {
+
+    const double a1 = -4/(N*Ts+2), a2 = -(N*Ts-2)/(N*Ts+2), // a1 and a2 are the nominator of our transferfunction
+    b0 = (4*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4),
+    b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2), 
+    b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4); // b0, b1 and b2 the denominator
+
+    double v = err - a1*v1 - a2*v2; // Memory value are calculated and later on stored. (v is like an input)
+    double u = b0*v + b1*v1 + b2*v2;
+    v2 = v1; v1 = v;
+    return u; // u functions as our output value gained from the transferfunction.
+}
+
+// PROGRAMS THAT CONTROLS THE VALUE OUTPUT
+void M_Controller(double Angle1, double Angle2, double &MotorValue1, double &MotorValue2) {
+    if (potmeter1 > 0.5f) {
+        direction1 = 1;
+        led_red = 0; }
+    else {
+        direction1 = 0;
+        led_red = 1; }
+        
+    if (potmeter2 > 0.5f) {
+        direction2 = 1;
+        led_blue = 0; }
+    else {
+        direction2 = 0;
+        led_blue = 1; }
+        
+    Counter(xdes, direction1, button1.read());
+    Counter(ydes, direction2, button2.read()); 
+    
+    Kinematic_referencer(xdes, ydes, q1, q2);
+    
+    //pc.printf("%.2f %.2f \r\n", xdes, ydes); 
+    
+    double ref_q1 = 2 * q1 * DEG_PER_RAD;
+    double ref_q2 = (q2 - M_Pi) * DEG_PER_RAD;
+    
+        
+    MotorValue1 = PID( ref_q1 - Angle1 , M1_Kp, M1_Ki, M1_Kd, M1_Ts, M1_N, M1_f_v1, M1_f_v2); //Find the motorvalue by going through the PID
+    MotorValue2 = PID( ref_q2 - Angle2 , M2_Kp, M2_Ki, M2_Kd, M2_Ts, M2_N, M2_f_v1, M2_f_v2);
+}
+
+
+// PROGRAMS FOR POWERING THE MOTOR ACCORDING TO THE ERROR (P VARIANT)
+void SetMotor1(double motor1Value) // function that actually changes the output for the motor
+{
+    if(motor1Value >= 0 )  //Function sets direction and strength
+        motor1DirectionPin = 1; //If the reference value is positive, we will turn clockwise
+    else
+        motor1DirectionPin = 0; // if not, counterclockwise
+        
+    if(fabs(motor1Value) > 0.5 ) // Next, check the absolute motor value, which is the magnitude
+        motor1MagnitudePin = 0.5; // This is a safety. We never want to exceed 1
+    else 
+        motor1MagnitudePin = fabs(motor1Value); // if we fall underneath the safety, take the magnitude
+}
+
+void SetMotor2(double motor2Value) // function that actually changes the output for the motor
+{
+    if(motor2Value >= 0 )  //Function sets direction and strength
+        motor2DirectionPin = 0; //If the reference value is positive, we will turn clockwise
+    else
+        motor2DirectionPin = 1; // if not, counterclockwise
+        
+    if(fabs(motor2Value) > 0.5 ) // Next, check the absolute motor value, which is the magnitude
+        motor2MagnitudePin = 0.5; // This is a safety. We never want to exceed 1
+    else 
+        motor2MagnitudePin = fabs(motor2Value); // if we fall underneath the safety, take the magnitude
+}
+
+// PROGRAM THAT MEASURES AND CONTROLES THE MOTOR OUTPUT
+void MeasureAndControl() // Pure values being calculated and send to the Mbed.
+{     
+    double Angle1 = DEG_PER_RAD * RAD_PER_PULSE * motor1.getPosition(); // Angle is equal to the degrees per pulse measured (NOT TRUE AT THIS MOMENT, '360' != 2 * M_Pi
+    double Angle2 = DEG_PER_RAD * RAD_PER_PULSE * motor2.getPosition(); // Angle is equal to the degrees per pulse measured (NOT TRUE AT THIS MOMENT, '360' != 2 * M_Pi
+    
+    M_Controller(Angle1, Angle2, MotorValue1, MotorValue2 ); //Perhaps call the Motorvalues themselves inside this function and edit them that way...
+    
+    SetMotor1( MotorValue1 );
+    SetMotor2( MotorValue2 );
+}
+
+int main() // Main function
+{
+    pc.baud(115200); // For post analysis, seeing if the plug works etc.
+    pc.printf("STARTING SEQUENCE \r\n"); //Merely checking if there is a serial connection at all
+    measureTick.attach(&MeasureAndControl, M1_Ts); // Tick that changes the motor (currently 1Hz)
+    led_red = 1; // Set the LED off in the positive direction, on in the negative direction
+    led_blue = 1;
+}
+