Martijn Homsma / Mbed OS Mbed_Shield_KinModel

kinmod shivan

Dependencies:   Encoder MODSERIAL

main.cpp@5:1e7dfd3c55ca, 2017-11-01 (annotated)

Committer:
mhomsma
Date:
Wed Nov 01 14:13:11 2017 +0000
Revision:
5:1e7dfd3c55ca
Parent:
4:c2017b3a7adb 
In before publishing;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mhomsma 0:077896c03576 1 #include "mbed.h"
mhomsma 0:077896c03576 2 #include "encoder.h"
mhomsma 0:077896c03576 3 #include "MODSERIAL.h"
mhomsma 0:077896c03576 4 #include "math.h"
mhomsma 0:077896c03576 5
mhomsma 0:077896c03576 6 #define M_Pi 3.141592653589793238462643383279502884L
mhomsma 0:077896c03576 7
mhomsma 0:077896c03576 8 Serial pc(USBTX, USBRX);
mhomsma 0:077896c03576 9
mhomsma 0:077896c03576 10 DigitalOut led_red(LED_RED);
mhomsma 0:077896c03576 11 DigitalOut led_blue(LED_BLUE);
mhomsma 0:077896c03576 12 InterruptIn button1(D2);
mhomsma 0:077896c03576 13 InterruptIn button2(D3);
mhomsma 0:077896c03576 14 AnalogIn potmeter1(A0);
mhomsma 0:077896c03576 15 AnalogIn potmeter2(A1);
mhomsma 0:077896c03576 16
mhomsma 0:077896c03576 17 DigitalOut motor1DirectionPin(D4);
mhomsma 0:077896c03576 18 PwmOut motor1MagnitudePin(D5);
mhomsma 1:dcc0ad8f6477 19 DigitalOut motor2DirectionPin(D7);
mhomsma 0:077896c03576 20 PwmOut motor2MagnitudePin(D6);
mhomsma 0:077896c03576 21
mhomsma 0:077896c03576 22 Ticker measureTick;
mhomsma 0:077896c03576 23
mhomsma 0:077896c03576 24 Encoder motor1(D13,D12); //On the shield actually M2
mhomsma 0:077896c03576 25 Encoder motor2(D11,D10); //On the shield actually M1 (Production mistake?)
mhomsma 0:077896c03576 26
mhomsma 0:077896c03576 27 bool switch1 = 1; // manual switch for when to start calculations (later removed for a state machine
mhomsma 0:077896c03576 28 bool direction1 = 1; // direction positive, 0 is negative
mhomsma 0:077896c03576 29 bool direction2 = 1;
mhomsma 2:e9e3ff715ef7 30 const double RAD_PER_PULSE = (2 * M_Pi)/8400 ; // Value for RAD_PER_PULSE given through the slides (wrong?)
mhomsma 0:077896c03576 31 const double DEG_PER_RAD = 180 / M_Pi; // Basic knowledge of how many degrees are in 1 radian.
mhomsma 0:077896c03576 32
mhomsma 0:077896c03576 33 double q1 = 0; // Angle of arm 1 (upper) in starting position is 0 degrees
mhomsma 4:c2017b3a7adb 34 double q2 = 135/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)
mhomsma 1:dcc0ad8f6477 35 int L1 = 29; // Length of arm 1 (upper) in cm
mhomsma 1:dcc0ad8f6477 36 int L2 = 47; // Length of arm 2 (lower) in cm
mhomsma 2:e9e3ff715ef7 37 //double xdes = L1-L2; // Desired x coordinate, arm is located at x = L1-L2 in starting position
mhomsma 2:e9e3ff715ef7 38 //double ydes = 0; // Disired y coordinate, arm is located at y = 0 in starting position
mhomsma 0:077896c03576 39 double MotorValue1 = 0;
mhomsma 0:077896c03576 40 double MotorValue2 = 0;
mhomsma 0:077896c03576 41
mhomsma 0:077896c03576 42 // Sample time (motor1-timestep)
mhomsma 0:077896c03576 43 const double M1_Ts = 0.01; //100 Hz systems
mhomsma 0:077896c03576 44 const double M2_Ts = 0.01;
mhomsma 0:077896c03576 45
mhomsma 0:077896c03576 46 // Controller gains (motor1-Kp,-Ki,...)
mhomsma 5:1e7dfd3c55ca 47 const double M1_Kp = 0.1, M1_Ki = 0.02, M1_Kd = 0.00125, M1_N = 100; // THESE VALUES ARE ARBITRARY AT THIS POINT Ki = 0.02
mhomsma 5:1e7dfd3c55ca 48 const double M2_Kp = 0.1, M2_Ki = 0.02, M2_Kd = 0.00125, M2_N = 100; // Inspired by the Ziegler-Nichols Method
mhomsma 0:077896c03576 49
mhomsma 0:077896c03576 50 // Filter variables (motor1-filter-v1,-v2)
mhomsma 0:077896c03576 51 double M1_f_v1 = 0.0, M1_f_v2 = 0.0;
mhomsma 0:077896c03576 52 double M2_f_v1 = 0.0, M2_f_v2 = 0.0;
mhomsma 0:077896c03576 53
mhomsma 4:c2017b3a7adb 54 struct Q { double q1; double q2; };
mhomsma 4:c2017b3a7adb 55
mhomsma 0:077896c03576 56 //Kinematic model
mhomsma 4:c2017b3a7adb 57 void Kinematic_referencer(double &q1, double &q2, double a1, double a2, bool dir1, bool dir2)
mhomsma 0:077896c03576 58 {
mhomsma 5:1e7dfd3c55ca 59 double at = a1 + a2, vx = 0, vy = 0; // current total angle
mhomsma 5:1e7dfd3c55ca 60 double xcurrent = L1 * cos (a1) + L2 * cos (at); // current x position
mhomsma 5:1e7dfd3c55ca 61 double ycurrent = L1 * sin (a1) + L2 * sin (at); // current y position
mhomsma 2:e9e3ff715ef7 62
mhomsma 2:e9e3ff715ef7 63 //pc.printf("x = %.1f, y = %.1f \r\n", xcurrent, ycurrent);
mhomsma 4:c2017b3a7adb 64 double velocity = 5;
mhomsma 0:077896c03576 65
mhomsma 0:077896c03576 66 //Initial twist
mhomsma 4:c2017b3a7adb 67 if (dir1 == 1){
mhomsma 4:c2017b3a7adb 68 vx = velocity* !button1.read();}//(xdes - xcurrent)/0.05; // Running on 100 Hz
mhomsma 4:c2017b3a7adb 69 else {
mhomsma 4:c2017b3a7adb 70 vx = -velocity* !button1.read();}
mhomsma 4:c2017b3a7adb 71 if (dir2 == 1){
mhomsma 4:c2017b3a7adb 72 vy = velocity* !button2.read();}//(ydes - ycurrent)/0.05;
mhomsma 4:c2017b3a7adb 73 else {
mhomsma 4:c2017b3a7adb 74 vy = -velocity* !button2.read();}
mhomsma 2:e9e3ff715ef7 75
mhomsma 4:c2017b3a7adb 76 //pc.printf("vx = %.2f, vy = %.2f \r\n", vx,vy);
mhomsma 0:077896c03576 77
mhomsma 0:077896c03576 78 //Jacobians
mhomsma 5:1e7dfd3c55ca 79 double J11 = -L1 * sin (a1) - L2 * sin (at);
mhomsma 5:1e7dfd3c55ca 80 double J12 = -L2 * sin (at);
mhomsma 5:1e7dfd3c55ca 81 double J21 = L1 * cos (a1) + L2 * cos (at);
mhomsma 5:1e7dfd3c55ca 82 double J22 = L2 * cos (at);
mhomsma 0:077896c03576 83 double Determinant = J11 * J22 - J21 * J12; // calculate determinant
mhomsma 0:077896c03576 84
mhomsma 1:dcc0ad8f6477 85 //pc.printf("D = %.3f \r\n", Determinant);
mhomsma 0:077896c03576 86
mhomsma 0:077896c03576 87 //Calculate angular velocities
mhomsma 5:1e7dfd3c55ca 88 double a1der = ((J22 * vx) - (J12 * vy)) / Determinant;
mhomsma 5:1e7dfd3c55ca 89 double a2der = ((-J21 * vx) + (J11 * vy)) / Determinant;
mhomsma 0:077896c03576 90
mhomsma 2:e9e3ff715ef7 91 //pc.printf("q1d = %.2f, q2d = %.2f \r\n", q1der, q2der);
mhomsma 2:e9e3ff715ef7 92
mhomsma 5:1e7dfd3c55ca 93 //Calculate new reference angles
mhomsma 5:1e7dfd3c55ca 94 double q1new = q1 + (a1der * M1_Ts); //nog fixen met die tijdstappen?
mhomsma 5:1e7dfd3c55ca 95 double q2new = q2 + (a2der * M1_Ts); //hier ook
mhomsma 2:e9e3ff715ef7 96 //pc.printf ("q1=%f, q2=%f\n", q1 * c, q2 * c);
mhomsma 4:c2017b3a7adb 97 double qtnew = q1new + q2new;
mhomsma 0:077896c03576 98
mhomsma 0:077896c03576 99 //Calculate new positions
mhomsma 4:c2017b3a7adb 100 double xnew = L1 * cos (q1new) + L2 * cos (qtnew);
mhomsma 4:c2017b3a7adb 101 double ynew = L1 * sin (q1new) + L2 * sin (qtnew);
mhomsma 0:077896c03576 102 //printf ("x=%f, y=%f\n", x, y);
mhomsma 0:077896c03576 103
mhomsma 4:c2017b3a7adb 104 q1 = q1new;
mhomsma 4:c2017b3a7adb 105 q2 = q2new;
mhomsma 2:e9e3ff715ef7 106
mhomsma 2:e9e3ff715ef7 107 /*
mhomsma 0:077896c03576 108 // Now check whether the calculated position is desired, determinants close to zero may cause the robot to move weirdly
mhomsma 0:077896c03576 109 // New y may not be negative, this means the arm is located in on the plate
mhomsma 0:077896c03576 110 // New q1 may not be less than -45 degrees, less means the arm will crash into the base plate
mhomsma 0:077896c03576 111 // New q2 may not be more than 185 degrees, more means the lower arm will crash into the upper arm
mhomsma 2:e9e3ff715ef7 112 if (ynew > -20 && a1new > -45 / DEG_PER_RAD && a2new < 200 / DEG_PER_RAD && (pow(xnew, 2.0) + pow(ynew,2.0)) > pow(17.0,2.0) )//&& Determinant < 0.01)
mhomsma 0:077896c03576 113 {
mhomsma 0:077896c03576 114 // If desired, change the angles
mhomsma 2:e9e3ff715ef7 115 q1 = a1new;
mhomsma 2:e9e3ff715ef7 116 q2 = a2new;
mhomsma 0:077896c03576 117 }
mhomsma 0:077896c03576 118 else
mhomsma 0:077896c03576 119 {
mhomsma 0:077896c03576 120 // If not desired, don't change the angles, but define current position as desired so the robot ignores the input
mhomsma 2:e9e3ff715ef7 121 xnew = xcurrent;
mhomsma 2:e9e3ff715ef7 122 ynew = ycurrent;
mhomsma 2:e9e3ff715ef7 123 }*/
mhomsma 0:077896c03576 124 }
mhomsma 0:077896c03576 125
mhomsma 0:077896c03576 126 // PROGRAM THAT CALCULATES THE PID
mhomsma 0:077896c03576 127 double PID( double err, const double Kp, const double Ki, const double Kd,
mhomsma 0:077896c03576 128 const double Ts, const double N, double &v1, double &v2 ) {
mhomsma 0:077896c03576 129
mhomsma 0:077896c03576 130 const double a1 = -4/(N*Ts+2), a2 = -(N*Ts-2)/(N*Ts+2), // a1 and a2 are the nominator of our transferfunction
mhomsma 0:077896c03576 131 b0 = (4*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4),
mhomsma 0:077896c03576 132 b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2),
mhomsma 0:077896c03576 133 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
mhomsma 0:077896c03576 134
mhomsma 0:077896c03576 135 double v = err - a1*v1 - a2*v2; // Memory value are calculated and later on stored. (v is like an input)
mhomsma 0:077896c03576 136 double u = b0*v + b1*v1 + b2*v2;
mhomsma 0:077896c03576 137 v2 = v1; v1 = v;
mhomsma 0:077896c03576 138 return u; // u functions as our output value gained from the transferfunction.
mhomsma 0:077896c03576 139 }
mhomsma 0:077896c03576 140
mhomsma 0:077896c03576 141 // PROGRAMS THAT CONTROLS THE VALUE OUTPUT
mhomsma 0:077896c03576 142 void M_Controller(double Angle1, double Angle2, double &MotorValue1, double &MotorValue2) {
mhomsma 0:077896c03576 143 if (potmeter1 > 0.5f) {
mhomsma 0:077896c03576 144 direction1 = 1;
mhomsma 0:077896c03576 145 led_red = 0; }
mhomsma 0:077896c03576 146 else {
mhomsma 0:077896c03576 147 direction1 = 0;
mhomsma 0:077896c03576 148 led_red = 1; }
mhomsma 0:077896c03576 149
mhomsma 0:077896c03576 150 if (potmeter2 > 0.5f) {
mhomsma 0:077896c03576 151 direction2 = 1;
mhomsma 0:077896c03576 152 led_blue = 0; }
mhomsma 0:077896c03576 153 else {
mhomsma 0:077896c03576 154 direction2 = 0;
mhomsma 0:077896c03576 155 led_blue = 1; }
mhomsma 0:077896c03576 156
mhomsma 4:c2017b3a7adb 157 Kinematic_referencer(q1, q2, Angle1, Angle2, direction1, direction2);
mhomsma 2:e9e3ff715ef7 158
mhomsma 0:077896c03576 159 double ref_q1 = 2 * q1 * DEG_PER_RAD;
mhomsma 2:e9e3ff715ef7 160 double ref_q2 = q2 * DEG_PER_RAD;
mhomsma 2:e9e3ff715ef7 161 double a1 = DEG_PER_RAD * Angle1;
mhomsma 2:e9e3ff715ef7 162 double a2 = DEG_PER_RAD * Angle2;
mhomsma 0:077896c03576 163
mhomsma 2:e9e3ff715ef7 164 //pc.printf("a1 = %.2f, a2 = %.2f \r\n", a1, a2);
mhomsma 2:e9e3ff715ef7 165 //pc.printf("q1 = %.2f, q2 = %.2f \r\n", ref_q1, ref_q2);
mhomsma 5:1e7dfd3c55ca 166 pc.printf("e1 = %.2f, e2 = %.2f \r\n", ref_q1-a1, ref_q2-a2);
mhomsma 0:077896c03576 167
mhomsma 2:e9e3ff715ef7 168 MotorValue1 = PID( ref_q1 - a1 , M1_Kp, M1_Ki, M1_Kd, M1_Ts, M1_N, M1_f_v1, M1_f_v2); //Find the motorvalue by going through the PID
mhomsma 2:e9e3ff715ef7 169 MotorValue2 = PID( ref_q2 - a2 , M2_Kp, M2_Ki, M2_Kd, M2_Ts, M2_N, M2_f_v1, M2_f_v2);
mhomsma 0:077896c03576 170 }
mhomsma 0:077896c03576 171
mhomsma 0:077896c03576 172
mhomsma 0:077896c03576 173 // PROGRAMS FOR POWERING THE MOTOR ACCORDING TO THE ERROR (P VARIANT)
mhomsma 0:077896c03576 174 void SetMotor1(double motor1Value) // function that actually changes the output for the motor
mhomsma 0:077896c03576 175 {
mhomsma 0:077896c03576 176 if(motor1Value >= 0 ) //Function sets direction and strength
mhomsma 0:077896c03576 177 motor1DirectionPin = 1; //If the reference value is positive, we will turn clockwise
mhomsma 0:077896c03576 178 else
mhomsma 0:077896c03576 179 motor1DirectionPin = 0; // if not, counterclockwise
mhomsma 0:077896c03576 180
mhomsma 1:dcc0ad8f6477 181 if(fabs(motor1Value) > 1.0 ) // Next, check the absolute motor value, which is the magnitude
mhomsma 1:dcc0ad8f6477 182 motor1MagnitudePin = 1.0; // This is a safety. We never want to exceed 1
mhomsma 0:077896c03576 183 else
mhomsma 0:077896c03576 184 motor1MagnitudePin = fabs(motor1Value); // if we fall underneath the safety, take the magnitude
mhomsma 0:077896c03576 185 }
mhomsma 0:077896c03576 186
mhomsma 0:077896c03576 187 void SetMotor2(double motor2Value) // function that actually changes the output for the motor
mhomsma 0:077896c03576 188 {
mhomsma 0:077896c03576 189 if(motor2Value >= 0 ) //Function sets direction and strength
mhomsma 0:077896c03576 190 motor2DirectionPin = 0; //If the reference value is positive, we will turn clockwise
mhomsma 0:077896c03576 191 else
mhomsma 0:077896c03576 192 motor2DirectionPin = 1; // if not, counterclockwise
mhomsma 0:077896c03576 193
mhomsma 1:dcc0ad8f6477 194 if(fabs(motor2Value) > 1.0 ) // Next, check the absolute motor value, which is the magnitude
mhomsma 1:dcc0ad8f6477 195 motor2MagnitudePin = 1.0; // This is a safety. We never want to exceed 1
mhomsma 0:077896c03576 196 else
mhomsma 0:077896c03576 197 motor2MagnitudePin = fabs(motor2Value); // if we fall underneath the safety, take the magnitude
mhomsma 0:077896c03576 198 }
mhomsma 0:077896c03576 199
mhomsma 0:077896c03576 200 // PROGRAM THAT MEASURES AND CONTROLES THE MOTOR OUTPUT
mhomsma 0:077896c03576 201 void MeasureAndControl() // Pure values being calculated and send to the Mbed.
mhomsma 0:077896c03576 202 {
mhomsma 2:e9e3ff715ef7 203 double Angle1 = RAD_PER_PULSE * motor1.getPosition(); // [rad]
mhomsma 4:c2017b3a7adb 204 double Angle2 = RAD_PER_PULSE * motor2.getPosition() + 135 / DEG_PER_RAD; // [rad]
mhomsma 0:077896c03576 205
mhomsma 0:077896c03576 206 M_Controller(Angle1, Angle2, MotorValue1, MotorValue2 ); //Perhaps call the Motorvalues themselves inside this function and edit them that way...
mhomsma 0:077896c03576 207
mhomsma 0:077896c03576 208 SetMotor1( MotorValue1 );
mhomsma 0:077896c03576 209 SetMotor2( MotorValue2 );
mhomsma 0:077896c03576 210 }
mhomsma 0:077896c03576 211
mhomsma 0:077896c03576 212 int main() // Main function
mhomsma 0:077896c03576 213 {
mhomsma 0:077896c03576 214 pc.baud(115200); // For post analysis, seeing if the plug works etc.
mhomsma 0:077896c03576 215 pc.printf("STARTING SEQUENCE \r\n"); //Merely checking if there is a serial connection at all
mhomsma 0:077896c03576 216 measureTick.attach(&MeasureAndControl, M1_Ts); // Tick that changes the motor (currently 1Hz)
mhomsma 0:077896c03576 217 led_red = 1; // Set the LED off in the positive direction, on in the negative direction
mhomsma 0:077896c03576 218 led_blue = 1;
mhomsma 0:077896c03576 219 }
mhomsma 0:077896c03576 220