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@0:077896c03576, 2017-10-31 (annotated)

Committer:
mhomsma
Date:
Tue Oct 31 12:52:29 2017 +0000
Revision:
0:077896c03576
Child:
1:d57a502fb30f
Kinmod shivan

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