shivan bissesar / Mbed OS Mbed_Shield_KinModel

Code with changes in kinematic model

Dependencies:   Encoder MODSERIAL

Fork of Mbed_Shield_KinModel by Martijn Homsma

main.cpp@1:d57a502fb30f, 2017-10-31 (annotated)

Committer:
Shivan_1997
Date:
Tue Oct 31 19:00:47 2017 +0000
Revision:
1:d57a502fb30f
Parent:
0:077896c03576 
Code with changes in kinematic model angles

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 0:077896c03576 19 DigitalOut motor2DirectionPin(D7); // Sequence? Lines 181-183
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 0:077896c03576 30 const double RAD_PER_PULSE = 0.002991; // Value for RAD_PER_PULSE given through the slides (wrong?)
Shivan_1997 1:d57a502fb30f 31 const double DEG_PER_RAD = 180 / M_Pi;
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)
Shivan_1997 1:d57a502fb30f 35 int L2 = 47; // Length of arm 1 (upper) in cm
Shivan_1997 1:d57a502fb30f 36 int L1 = 29; // Length of arm 2 (lower) in cm
mhomsma 0:077896c03576 37 double xdes = L1-L2; // Desired x coordinate, arm is located at x = L1-L2 in starting position
mhomsma 0:077896c03576 38 double ydes = 0; // Disired y coordinate, arm is located at y = 0 in starting position
Shivan_1997 1:d57a502fb30f 39 double MotorValue1 = 0;
mhomsma 0:077896c03576 40 double MotorValue2 = 0;
mhomsma 0:077896c03576 41
mhomsma 0:077896c03576 42 // Sample time (motor1-timestep)
Shivan_1997 1:d57a502fb30f 43 const double M1_Ts = 0.01;
mhomsma 0:077896c03576 44 const double M2_Ts = 0.01;
mhomsma 0:077896c03576 45
mhomsma 0:077896c03576 46 // Controller gains (motor1-Kp,-Ki,...)
Shivan_1997 1:d57a502fb30f 47 const double M1_Kp = 0.2, M1_Ki = 40, M1_Kd = 0.000666666, M1_N = 100; // THESE VALUES ARE ARBITRARY AT THIS POINT
Shivan_1997 1:d57a502fb30f 48 const double M1_Kp = 0.2, M1_Ki = 40, M1_Kd = 0.000666666, M1_N = 100; // I was hoger na opnieuw uitrekenen met ZN methode
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 0:077896c03576 55
mhomsma 0:077896c03576 56 //Xdes and Ydes changer
Shivan_1997 1:d57a502fb30f 57 void Counter(double &des, double dir, double sig){//wat doet dit???
Shivan_1997 1:d57a502fb30f 58 if (sig == 0){
mhomsma 0:077896c03576 59 if (dir == 1)
mhomsma 0:077896c03576 60 des = des + 0.1;
mhomsma 0:077896c03576 61 else if (dir == 0)
mhomsma 0:077896c03576 62 des = des - 0.1;
mhomsma 0:077896c03576 63 }
mhomsma 0:077896c03576 64 }
mhomsma 0:077896c03576 65
mhomsma 0:077896c03576 66 //Kinematic model
Shivan_1997 1:d57a502fb30f 67 void Kinematic_referencer( double &xdes, double &ydes, double &q1, double &q2, double &Angle1, double &Angle2 )
mhomsma 0:077896c03576 68 {
Shivan_1997 1:d57a502fb30f 69 double Anglet = Angle1 + Angle2; // current total angle
Shivan_1997 1:d57a502fb30f 70 double xcurrent = L1 * cos (Angle1) + L2 * cos (Anglet); // current x position
Shivan_1997 1:d57a502fb30f 71 double ycurrent = L1 * sin (Angle1) + L2 * sin (Anglet); // current y position
mhomsma 0:077896c03576 72
mhomsma 0:077896c03576 73 //Initial twist
Shivan_1997 1:d57a502fb30f 74 double vx = (xdes - xcurrent)/0.01; // Running on 100 Hz
Shivan_1997 1:d57a502fb30f 75 double vy = (ydes - ycurrent)/0.01;
mhomsma 0:077896c03576 76
mhomsma 0:077896c03576 77 //Jacobians
mhomsma 0:077896c03576 78 double J11 = -ycurrent;
Shivan_1997 1:d57a502fb30f 79 double J12 = -L2 * sin (Anglet);
mhomsma 0:077896c03576 80 double J21 = xcurrent;
Shivan_1997 1:d57a502fb30f 81 double J22 = L2 * cos (Anglet);
mhomsma 0:077896c03576 82 double Determinant = J11 * J22 - J21 * J12; // calculate determinant
mhomsma 0:077896c03576 83
mhomsma 0:077896c03576 84 double q1der = (J22 * vx - J12 * vy) / Determinant;
mhomsma 0:077896c03576 85 double q2der = (-J21 * vx + J11 * vy) / Determinant;
Shivan_1997 1:d57a502fb30f 86
Shivan_1997 1:d57a502fb30f 87 double Angle1new = Angle1 + q1der/100; //nog fixen met die tijdstappen?
Shivan_1997 1:d57a502fb30f 88 double Angle2new = Angle2 + q2der/100;
Shivan_1997 1:d57a502fb30f 89
Shivan_1997 1:d57a502fb30f 90 double Angletnew = Angle1new + Angle2new;
Shivan_1997 1:d57a502fb30f 91
Shivan_1997 1:d57a502fb30f 92 double xnew = L1 * cos (Angle1new) + L2 * cos (Angletnew);
Shivan_1997 1:d57a502fb30f 93 double ynew = L1 * sin (Angle1new) + L2 * sin (Angletnew);
mhomsma 0:077896c03576 94
mhomsma 0:077896c03576 95 // Now check whether the calculated position is desired, determinants close to zero may cause the robot to move weirdly
mhomsma 0:077896c03576 96 // New y may not be negative, this means the arm is located in on the plate
Shivan_1997 1:d57a502fb30f 97 // New q1 may not be less than -55 degrees, less means the arm will crash into the base plate
Shivan_1997 1:d57a502fb30f 98 // New q2 may not be more than 195 degrees, more means the lower arm will crash into the upper arm
Shivan_1997 1:d57a502fb30f 99 if (ynew > -10 && qAngle1new > -55 / DEG_PER_RAD && qAngle2new < 195 / DEG_PER_RAD ))
mhomsma 0:077896c03576 100 {
mhomsma 0:077896c03576 101 // If desired, change the angles
Shivan_1997 1:d57a502fb30f 102 Angle1 = Angle1new;
Shivan_1997 1:d57a502fb30f 103 Angle2 = Angle2new;
mhomsma 0:077896c03576 104 }
mhomsma 0:077896c03576 105 else
mhomsma 0:077896c03576 106 {
mhomsma 0:077896c03576 107 // If not desired, don't change the angles, but define current position as desired so the robot ignores the input
mhomsma 0:077896c03576 108 xdes = xcurrent;
Shivan_1997 1:d57a502fb30f 109 ydes = ycurrent;//loopt het hier niet vast??
mhomsma 0:077896c03576 110 }
mhomsma 0:077896c03576 111 }
mhomsma 0:077896c03576 112
mhomsma 0:077896c03576 113 // PROGRAM THAT CALCULATES THE PID
mhomsma 0:077896c03576 114 double PID( double err, const double Kp, const double Ki, const double Kd,
mhomsma 0:077896c03576 115 const double Ts, const double N, double &v1, double &v2 ) {
mhomsma 0:077896c03576 116
mhomsma 0:077896c03576 117 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 118 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 119 b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2),
mhomsma 0:077896c03576 120 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 121
mhomsma 0:077896c03576 122 double v = err - a1*v1 - a2*v2; // Memory value are calculated and later on stored. (v is like an input)
mhomsma 0:077896c03576 123 double u = b0*v + b1*v1 + b2*v2;
mhomsma 0:077896c03576 124 v2 = v1; v1 = v;
mhomsma 0:077896c03576 125 return u; // u functions as our output value gained from the transferfunction.
mhomsma 0:077896c03576 126 }
mhomsma 0:077896c03576 127
mhomsma 0:077896c03576 128 // PROGRAMS THAT CONTROLS THE VALUE OUTPUT
Shivan_1997 1:d57a502fb30f 129 void M_Controller(double Angle1, double Angle2, double &MotorValue1, double &MotorValue2) {// waarom gebruik je pass by reference
mhomsma 0:077896c03576 130 if (potmeter1 > 0.5f) {
mhomsma 0:077896c03576 131 direction1 = 1;
mhomsma 0:077896c03576 132 led_red = 0; }
mhomsma 0:077896c03576 133 else {
mhomsma 0:077896c03576 134 direction1 = 0;
mhomsma 0:077896c03576 135 led_red = 1; }
mhomsma 0:077896c03576 136
mhomsma 0:077896c03576 137 if (potmeter2 > 0.5f) {
mhomsma 0:077896c03576 138 direction2 = 1;
mhomsma 0:077896c03576 139 led_blue = 0; }
mhomsma 0:077896c03576 140 else {
mhomsma 0:077896c03576 141 direction2 = 0;
mhomsma 0:077896c03576 142 led_blue = 1; }
mhomsma 0:077896c03576 143
mhomsma 0:077896c03576 144 Counter(xdes, direction1, button1.read());
mhomsma 0:077896c03576 145 Counter(ydes, direction2, button2.read());
mhomsma 0:077896c03576 146
Shivan_1997 1:d57a502fb30f 147 Kinematic_referencer(xdes, ydes, q1, q2, Angle1, Angle2);
mhomsma 0:077896c03576 148
Shivan_1997 1:d57a502fb30f 149 double ref_q1 = 2 * Angle1 * DEG_PER_RAD;
Shivan_1997 1:d57a502fb30f 150 double ref_q2 = (Angle2 - M_Pi) * DEG_PER_RAD;// waarom -Pi???
mhomsma 0:077896c03576 151
mhomsma 0:077896c03576 152
mhomsma 0:077896c03576 153 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
mhomsma 0:077896c03576 154 MotorValue2 = PID( ref_q2 - Angle2 , M2_Kp, M2_Ki, M2_Kd, M2_Ts, M2_N, M2_f_v1, M2_f_v2);
mhomsma 0:077896c03576 155 }
mhomsma 0:077896c03576 156
mhomsma 0:077896c03576 157
mhomsma 0:077896c03576 158 // PROGRAMS FOR POWERING THE MOTOR ACCORDING TO THE ERROR (P VARIANT)
mhomsma 0:077896c03576 159 void SetMotor1(double motor1Value) // function that actually changes the output for the motor
mhomsma 0:077896c03576 160 {
mhomsma 0:077896c03576 161 if(motor1Value >= 0 ) //Function sets direction and strength
mhomsma 0:077896c03576 162 motor1DirectionPin = 1; //If the reference value is positive, we will turn clockwise
mhomsma 0:077896c03576 163 else
mhomsma 0:077896c03576 164 motor1DirectionPin = 0; // if not, counterclockwise
mhomsma 0:077896c03576 165
mhomsma 0:077896c03576 166 if(fabs(motor1Value) > 0.5 ) // Next, check the absolute motor value, which is the magnitude
mhomsma 0:077896c03576 167 motor1MagnitudePin = 0.5; // This is a safety. We never want to exceed 1
mhomsma 0:077896c03576 168 else
mhomsma 0:077896c03576 169 motor1MagnitudePin = fabs(motor1Value); // if we fall underneath the safety, take the magnitude
mhomsma 0:077896c03576 170 }
mhomsma 0:077896c03576 171
mhomsma 0:077896c03576 172 void SetMotor2(double motor2Value) // function that actually changes the output for the motor
mhomsma 0:077896c03576 173 {
mhomsma 0:077896c03576 174 if(motor2Value >= 0 ) //Function sets direction and strength
mhomsma 0:077896c03576 175 motor2DirectionPin = 0; //If the reference value is positive, we will turn clockwise
mhomsma 0:077896c03576 176 else
mhomsma 0:077896c03576 177 motor2DirectionPin = 1; // if not, counterclockwise
mhomsma 0:077896c03576 178
mhomsma 0:077896c03576 179 if(fabs(motor2Value) > 0.5 ) // Next, check the absolute motor value, which is the magnitude
mhomsma 0:077896c03576 180 motor2MagnitudePin = 0.5; // This is a safety. We never want to exceed 1
mhomsma 0:077896c03576 181 else
mhomsma 0:077896c03576 182 motor2MagnitudePin = fabs(motor2Value); // if we fall underneath the safety, take the magnitude
mhomsma 0:077896c03576 183 }
mhomsma 0:077896c03576 184
mhomsma 0:077896c03576 185 // PROGRAM THAT MEASURES AND CONTROLES THE MOTOR OUTPUT
mhomsma 0:077896c03576 186 void MeasureAndControl() // Pure values being calculated and send to the Mbed.
mhomsma 0:077896c03576 187 {
mhomsma 0:077896c03576 188 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
Shivan_1997 1:d57a502fb30f 189 double Angle2 = DEG_PER_RAD * RAD_PER_PULSE * motor2.getPosition();
mhomsma 0:077896c03576 190
mhomsma 0:077896c03576 191 M_Controller(Angle1, Angle2, MotorValue1, MotorValue2 ); //Perhaps call the Motorvalues themselves inside this function and edit them that way...
mhomsma 0:077896c03576 192
mhomsma 0:077896c03576 193 SetMotor1( MotorValue1 );
mhomsma 0:077896c03576 194 SetMotor2( MotorValue2 );
mhomsma 0:077896c03576 195 }
mhomsma 0:077896c03576 196
mhomsma 0:077896c03576 197 int main() // Main function
mhomsma 0:077896c03576 198 {
mhomsma 0:077896c03576 199 pc.baud(115200); // For post analysis, seeing if the plug works etc.
mhomsma 0:077896c03576 200 pc.printf("STARTING SEQUENCE \r\n"); //Merely checking if there is a serial connection at all
mhomsma 0:077896c03576 201 measureTick.attach(&MeasureAndControl, M1_Ts); // Tick that changes the motor (currently 1Hz)
mhomsma 0:077896c03576 202 led_red = 1; // Set the LED off in the positive direction, on in the negative direction
mhomsma 0:077896c03576 203 led_blue = 1;
mhomsma 0:077896c03576 204 }
mhomsma 0:077896c03576 205