Martijn Homsma / Mbed OS Mbed_Shield_KinModel

kinmod shivan

Dependencies:   Encoder MODSERIAL

main.cpp@3:c34df562d713, 2017-11-01 (annotated)

Committer:
mhomsma
Date:
Wed Nov 01 11:35:20 2017 +0000
Revision:
3:c34df562d713
Parent:
2:e9e3ff715ef7
Child:
4:c2017b3a7adb
BUGFIXING: Kinematic Referencer does something weird; probably the Vx and Vy are translated WRONG causing too small / large changes to q1 and q2

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