Martijn Homsma / Mbed OS Mbed_Shield_KinModel

kinmod shivan

Dependencies:   Encoder MODSERIAL

main.cpp@2:e9e3ff715ef7, 2017-11-01 (annotated)

Committer:
mhomsma
Date:
Wed Nov 01 11:25:27 2017 +0000
Revision:
2:e9e3ff715ef7
Parent:
1:dcc0ad8f6477
Child:
3:c34df562d713
changing the system for now;

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 0:077896c03576 34 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)
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 0:077896c03576 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
mhomsma 0:077896c03576 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 0:077896c03576 54 // PROGRAM THAT CALCULATES ANGLE CHANGES
mhomsma 2:e9e3ff715ef7 55 /*
mhomsma 0:077896c03576 56 //Xdes and Ydes changer
mhomsma 0:077896c03576 57 void Counter(double &des, double dir, double sig){
mhomsma 0:077896c03576 58 if (sig == 0){
mhomsma 0:077896c03576 59 if (dir == 1)
mhomsma 1:dcc0ad8f6477 60 des = des + 0.05;
mhomsma 0:077896c03576 61 else if (dir == 0)
mhomsma 1:dcc0ad8f6477 62 des = des - 0.05;
mhomsma 0:077896c03576 63 }
mhomsma 2:e9e3ff715ef7 64 } */
mhomsma 0:077896c03576 65
mhomsma 0:077896c03576 66 //Kinematic model
mhomsma 2:e9e3ff715ef7 67 void Kinematic_referencer( /*double &xdes, double &ydes,*/ double &q1, double &q2, double a1, double a2)
mhomsma 0:077896c03576 68 {
mhomsma 2:e9e3ff715ef7 69 double at = a1 + a2; // current total angle
mhomsma 2:e9e3ff715ef7 70 double xcurrent = L1 * cos (a1) + L2 * cos (at); // current x position
mhomsma 2:e9e3ff715ef7 71 double ycurrent = L1 * sin (a1) + L2 * sin (at); // current y position
mhomsma 2:e9e3ff715ef7 72
mhomsma 2:e9e3ff715ef7 73 //pc.printf("x = %.1f, y = %.1f \r\n", xcurrent, ycurrent);
mhomsma 0:077896c03576 74
mhomsma 0:077896c03576 75 //Initial twist
mhomsma 2:e9e3ff715ef7 76 double vx = 0.1* !button1.read();//(xdes - xcurrent)/0.05; // Running on 100 Hz
mhomsma 2:e9e3ff715ef7 77 double vy = 0.1* !button2.read();//(ydes - ycurrent)/0.05;
mhomsma 2:e9e3ff715ef7 78
mhomsma 2:e9e3ff715ef7 79 //pc.printf("vx = %.0f, vy = %.0f \r\n", vx,vy);
mhomsma 0:077896c03576 80
mhomsma 0:077896c03576 81 //Jacobians
mhomsma 0:077896c03576 82 double J11 = -ycurrent;
mhomsma 2:e9e3ff715ef7 83 double J12 = -L2 * sin (at);
mhomsma 0:077896c03576 84 double J21 = xcurrent;
mhomsma 2:e9e3ff715ef7 85 double J22 = L2 * cos (at);
mhomsma 0:077896c03576 86 double Determinant = J11 * J22 - J21 * J12; // calculate determinant
mhomsma 0:077896c03576 87
mhomsma 1:dcc0ad8f6477 88 //pc.printf("D = %.3f \r\n", Determinant);
mhomsma 0:077896c03576 89
mhomsma 0:077896c03576 90 //Calculate angular velocities
mhomsma 0:077896c03576 91 double q1der = (J22 * vx - J12 * vy) / Determinant;
mhomsma 0:077896c03576 92 double q2der = (-J21 * vx + J11 * vy) / Determinant;
mhomsma 0:077896c03576 93
mhomsma 2:e9e3ff715ef7 94 //pc.printf("q1d = %.2f, q2d = %.2f \r\n", q1der, q2der);
mhomsma 2:e9e3ff715ef7 95
mhomsma 0:077896c03576 96 //Calculate new angles
mhomsma 2:e9e3ff715ef7 97 double a1new = a1 + q1der/100; //nog fixen met die tijdstappen?
mhomsma 2:e9e3ff715ef7 98 double a2new = a2 + q2der/100; //hier ook
mhomsma 2:e9e3ff715ef7 99 //pc.printf ("q1=%f, q2=%f\n", q1 * c, q2 * c);
mhomsma 2:e9e3ff715ef7 100 double atnew = a1new + a2new;
mhomsma 0:077896c03576 101
mhomsma 0:077896c03576 102 //Calculate new positions
mhomsma 2:e9e3ff715ef7 103 double xnew = L1 * cos (a1new) + L2 * cos (atnew);
mhomsma 2:e9e3ff715ef7 104 double ynew = L1 * sin (a1new) + L2 * sin (atnew);
mhomsma 0:077896c03576 105 //printf ("x=%f, y=%f\n", x, y);
mhomsma 0:077896c03576 106
mhomsma 2:e9e3ff715ef7 107 q1 = a1new;
mhomsma 2:e9e3ff715ef7 108 q2 = a2new;
mhomsma 2:e9e3ff715ef7 109
mhomsma 2:e9e3ff715ef7 110 /*
mhomsma 0:077896c03576 111 // Now check whether the calculated position is desired, determinants close to zero may cause the robot to move weirdly
mhomsma 0:077896c03576 112 // New y may not be negative, this means the arm is located in on the plate
mhomsma 0:077896c03576 113 // New q1 may not be less than -45 degrees, less means the arm will crash into the base plate
mhomsma 0:077896c03576 114 // New q2 may not be more than 185 degrees, more means the lower arm will crash into the upper arm
mhomsma 2:e9e3ff715ef7 115 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 116 {
mhomsma 0:077896c03576 117 // If desired, change the angles
mhomsma 2:e9e3ff715ef7 118 q1 = a1new;
mhomsma 2:e9e3ff715ef7 119 q2 = a2new;
mhomsma 0:077896c03576 120 }
mhomsma 0:077896c03576 121 else
mhomsma 0:077896c03576 122 {
mhomsma 0:077896c03576 123 // If not desired, don't change the angles, but define current position as desired so the robot ignores the input
mhomsma 2:e9e3ff715ef7 124 xnew = xcurrent;
mhomsma 2:e9e3ff715ef7 125 ynew = ycurrent;
mhomsma 2:e9e3ff715ef7 126 }*/
mhomsma 0:077896c03576 127 }
mhomsma 0:077896c03576 128
mhomsma 0:077896c03576 129 // PROGRAM THAT CALCULATES THE PID
mhomsma 0:077896c03576 130 double PID( double err, const double Kp, const double Ki, const double Kd,
mhomsma 0:077896c03576 131 const double Ts, const double N, double &v1, double &v2 ) {
mhomsma 0:077896c03576 132
mhomsma 0:077896c03576 133 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 134 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 135 b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2),
mhomsma 0:077896c03576 136 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 137
mhomsma 0:077896c03576 138 double v = err - a1*v1 - a2*v2; // Memory value are calculated and later on stored. (v is like an input)
mhomsma 0:077896c03576 139 double u = b0*v + b1*v1 + b2*v2;
mhomsma 0:077896c03576 140 v2 = v1; v1 = v;
mhomsma 0:077896c03576 141 return u; // u functions as our output value gained from the transferfunction.
mhomsma 0:077896c03576 142 }
mhomsma 0:077896c03576 143
mhomsma 0:077896c03576 144 // PROGRAMS THAT CONTROLS THE VALUE OUTPUT
mhomsma 0:077896c03576 145 void M_Controller(double Angle1, double Angle2, double &MotorValue1, double &MotorValue2) {
mhomsma 0:077896c03576 146 if (potmeter1 > 0.5f) {
mhomsma 0:077896c03576 147 direction1 = 1;
mhomsma 0:077896c03576 148 led_red = 0; }
mhomsma 0:077896c03576 149 else {
mhomsma 0:077896c03576 150 direction1 = 0;
mhomsma 0:077896c03576 151 led_red = 1; }
mhomsma 0:077896c03576 152
mhomsma 0:077896c03576 153 if (potmeter2 > 0.5f) {
mhomsma 0:077896c03576 154 direction2 = 1;
mhomsma 0:077896c03576 155 led_blue = 0; }
mhomsma 0:077896c03576 156 else {
mhomsma 0:077896c03576 157 direction2 = 0;
mhomsma 0:077896c03576 158 led_blue = 1; }
mhomsma 0:077896c03576 159
mhomsma 2:e9e3ff715ef7 160 //Counter(xdes, direction1, button1.read());
mhomsma 2:e9e3ff715ef7 161 //Counter(ydes, direction2, button2.read());
mhomsma 0:077896c03576 162
mhomsma 2:e9e3ff715ef7 163 Kinematic_referencer(/*xdes, ydes,*/ q1, q2, Angle1, Angle2);
mhomsma 2:e9e3ff715ef7 164
mhomsma 0:077896c03576 165 double ref_q1 = 2 * q1 * DEG_PER_RAD;
mhomsma 2:e9e3ff715ef7 166 double ref_q2 = q2 * DEG_PER_RAD;
mhomsma 2:e9e3ff715ef7 167 double a1 = DEG_PER_RAD * Angle1;
mhomsma 2:e9e3ff715ef7 168 double a2 = DEG_PER_RAD * Angle2;
mhomsma 0:077896c03576 169
mhomsma 2:e9e3ff715ef7 170 //pc.printf("a1 = %.2f, a2 = %.2f \r\n", a1, a2);
mhomsma 2:e9e3ff715ef7 171 //pc.printf("q1 = %.2f, q2 = %.2f \r\n", ref_q1, ref_q2);
mhomsma 2:e9e3ff715ef7 172 //pc.printf("e1 = %.2f, e2 = %.2f \r\n", ref_q1-a1, ref_q2-a2);
mhomsma 0:077896c03576 173
mhomsma 2:e9e3ff715ef7 174 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 175 MotorValue2 = PID( ref_q2 - a2 , M2_Kp, M2_Ki, M2_Kd, M2_Ts, M2_N, M2_f_v1, M2_f_v2);
mhomsma 0:077896c03576 176 }
mhomsma 0:077896c03576 177
mhomsma 0:077896c03576 178
mhomsma 0:077896c03576 179 // PROGRAMS FOR POWERING THE MOTOR ACCORDING TO THE ERROR (P VARIANT)
mhomsma 0:077896c03576 180 void SetMotor1(double motor1Value) // function that actually changes the output for the motor
mhomsma 0:077896c03576 181 {
mhomsma 0:077896c03576 182 if(motor1Value >= 0 ) //Function sets direction and strength
mhomsma 0:077896c03576 183 motor1DirectionPin = 1; //If the reference value is positive, we will turn clockwise
mhomsma 0:077896c03576 184 else
mhomsma 0:077896c03576 185 motor1DirectionPin = 0; // if not, counterclockwise
mhomsma 0:077896c03576 186
mhomsma 1:dcc0ad8f6477 187 if(fabs(motor1Value) > 1.0 ) // Next, check the absolute motor value, which is the magnitude
mhomsma 1:dcc0ad8f6477 188 motor1MagnitudePin = 1.0; // This is a safety. We never want to exceed 1
mhomsma 0:077896c03576 189 else
mhomsma 0:077896c03576 190 motor1MagnitudePin = fabs(motor1Value); // if we fall underneath the safety, take the magnitude
mhomsma 0:077896c03576 191 }
mhomsma 0:077896c03576 192
mhomsma 0:077896c03576 193 void SetMotor2(double motor2Value) // function that actually changes the output for the motor
mhomsma 0:077896c03576 194 {
mhomsma 0:077896c03576 195 if(motor2Value >= 0 ) //Function sets direction and strength
mhomsma 0:077896c03576 196 motor2DirectionPin = 0; //If the reference value is positive, we will turn clockwise
mhomsma 0:077896c03576 197 else
mhomsma 0:077896c03576 198 motor2DirectionPin = 1; // if not, counterclockwise
mhomsma 0:077896c03576 199
mhomsma 1:dcc0ad8f6477 200 if(fabs(motor2Value) > 1.0 ) // Next, check the absolute motor value, which is the magnitude
mhomsma 1:dcc0ad8f6477 201 motor2MagnitudePin = 1.0; // This is a safety. We never want to exceed 1
mhomsma 0:077896c03576 202 else
mhomsma 0:077896c03576 203 motor2MagnitudePin = fabs(motor2Value); // if we fall underneath the safety, take the magnitude
mhomsma 0:077896c03576 204 }
mhomsma 0:077896c03576 205
mhomsma 0:077896c03576 206 // PROGRAM THAT MEASURES AND CONTROLES THE MOTOR OUTPUT
mhomsma 0:077896c03576 207 void MeasureAndControl() // Pure values being calculated and send to the Mbed.
mhomsma 0:077896c03576 208 {
mhomsma 2:e9e3ff715ef7 209 double Angle1 = RAD_PER_PULSE * motor1.getPosition(); // [rad]
mhomsma 2:e9e3ff715ef7 210 double Angle2 = RAD_PER_PULSE * motor2.getPosition() + 179 / DEG_PER_RAD; // [rad]
mhomsma 0:077896c03576 211
mhomsma 0:077896c03576 212 M_Controller(Angle1, Angle2, MotorValue1, MotorValue2 ); //Perhaps call the Motorvalues themselves inside this function and edit them that way...
mhomsma 0:077896c03576 213
mhomsma 0:077896c03576 214 SetMotor1( MotorValue1 );
mhomsma 0:077896c03576 215 SetMotor2( MotorValue2 );
mhomsma 0:077896c03576 216 }
mhomsma 0:077896c03576 217
mhomsma 0:077896c03576 218 int main() // Main function
mhomsma 0:077896c03576 219 {
mhomsma 0:077896c03576 220 pc.baud(115200); // For post analysis, seeing if the plug works etc.
mhomsma 0:077896c03576 221 pc.printf("STARTING SEQUENCE \r\n"); //Merely checking if there is a serial connection at all
mhomsma 0:077896c03576 222 measureTick.attach(&MeasureAndControl, M1_Ts); // Tick that changes the motor (currently 1Hz)
mhomsma 0:077896c03576 223 led_red = 1; // Set the LED off in the positive direction, on in the negative direction
mhomsma 0:077896c03576 224 led_blue = 1;
mhomsma 0:077896c03576 225 }
mhomsma 0:077896c03576 226