Wessel Klumperman / Mbed 2 deprecated Inverse_kinematics_PIDController

Werkt

Dependencies:   Encoder HIDScope MODSERIAL QEI mbed

Fork of Inverse_kinematics_PIDController by Casper Kroon

main.cpp@12:9a2d3d544426, 2018-10-30 (annotated)

Committer:
CasperK
Date:
Tue Oct 30 09:22:44 2018 +0000
Revision:
12:9a2d3d544426
Parent:
11:325a545a757e
Child:
13:2d2763be031c
Motor 2 always rotates; PID controller still oscillates much

Who changed what in which revision?

UserRevisionLine numberNew contents of line
CasperK 0:dc2c63f663f8 1 #include "mbed.h"
CasperK 1:fc216448bb57 2 #include "math.h"
CasperK 0:dc2c63f663f8 3 #include "MODSERIAL.h"
CasperK 7:9e0ded88fe60 4 #include "HIDScope.h"
CasperK 12:9a2d3d544426 5 #include "QEI.h"
CasperK 11:325a545a757e 6 #define PI 3.141592f //65358979323846 // pi
CasperK 0:dc2c63f663f8 7
CasperK 8:fd00916552e0 8 PwmOut pwmpin1(D6);
CasperK 8:fd00916552e0 9 PwmOut pwmpin2(D5);
CasperK 12:9a2d3d544426 10 DigitalOut directionpin2(D4);
CasperK 12:9a2d3d544426 11 DigitalOut directionpin1(D7);
CasperK 12:9a2d3d544426 12 QEI motor2(D13,D12,NC, 32);
CasperK 12:9a2d3d544426 13 QEI motor1(D11,D10,NC, 32);
CasperK 8:fd00916552e0 14 DigitalOut ledred(LED_RED);
CasperK 8:fd00916552e0 15
CasperK 8:fd00916552e0 16 DigitalIn KillSwitch(SW2);
CasperK 8:fd00916552e0 17 DigitalIn button(SW3);
CasperK 0:dc2c63f663f8 18 MODSERIAL pc(USBTX, USBRX);
CasperK 12:9a2d3d544426 19 HIDScope scope(6);
CasperK 0:dc2c63f663f8 20
CasperK 7:9e0ded88fe60 21 //values of inverse kinematics
CasperK 3:56cbed6caacc 22 volatile bool emg0Bool = false;
CasperK 3:56cbed6caacc 23 volatile bool emg1Bool = false;
CasperK 3:56cbed6caacc 24 volatile bool emg2Bool = false;
CasperK 12:9a2d3d544426 25 volatile bool x_direction = true;
CasperK 4:f36406c9e42f 26 volatile bool a;
CasperK 1:fc216448bb57 27
CasperK 10:6b12d31b0fbf 28 const float C1 = 3.0; //motor 1 gear ratio
CasperK 10:6b12d31b0fbf 29 const float C2 = 0.013; //motor 2 gear ratio/radius of the circular gear in m
CasperK 10:6b12d31b0fbf 30 const float length = 0.300; //length in m (placeholder)
CasperK 12:9a2d3d544426 31 const float Timestep = 0.1;
CasperK 10:6b12d31b0fbf 32
CasperK 10:6b12d31b0fbf 33 volatile float x_position = length;
CasperK 4:f36406c9e42f 34 volatile float y_position = 0.0;
CasperK 10:6b12d31b0fbf 35 volatile float old_y_position;
CasperK 10:6b12d31b0fbf 36 volatile float old_x_position;
CasperK 10:6b12d31b0fbf 37 volatile float old_motor1_angle;
CasperK 10:6b12d31b0fbf 38 volatile float old_motor2_angle;
CasperK 10:6b12d31b0fbf 39 volatile float motor1_angle = 0.0; //sawtooth gear motor
CasperK 10:6b12d31b0fbf 40 volatile float motor2_angle = 0.0; //rotational gear motor
CasperK 5:14a68d0ee71a 41 volatile float direction;
CasperK 4:f36406c9e42f 42 volatile char c;
CasperK 1:fc216448bb57 43
CasperK 7:9e0ded88fe60 44 //values of PID controller
CasperK 12:9a2d3d544426 45 const float Kp = 1;
CasperK 7:9e0ded88fe60 46 const float Ki = 0.2;
CasperK 7:9e0ded88fe60 47 const float Kd = 0;
CasperK 7:9e0ded88fe60 48 float Output1 = 0 ; //Starting value
CasperK 7:9e0ded88fe60 49 float Output2 = 0 ; //Starting value
CasperK 8:fd00916552e0 50 float P1 = 0; //encoder value
CasperK 8:fd00916552e0 51 float P2 = 0;
CasperK 7:9e0ded88fe60 52 float e1 = 0 ; //Starting value
CasperK 7:9e0ded88fe60 53 float e2 = 0 ; //Starting value
CasperK 7:9e0ded88fe60 54 float e3;
CasperK 7:9e0ded88fe60 55 float f1 = 0 ; //Starting value
CasperK 7:9e0ded88fe60 56 float f2 = 0 ; //Starting value
CasperK 7:9e0ded88fe60 57 float f3;
CasperK 7:9e0ded88fe60 58 float Output_Last1; // Remember previous position
CasperK 7:9e0ded88fe60 59 float Output_Last2; // Remember previous position
CasperK 8:fd00916552e0 60 float Y1; // Value that is outputted to motor control
CasperK 8:fd00916552e0 61 float Y2; // Value that is outputted to motor control
CasperK 8:fd00916552e0 62 float pwm1;
CasperK 8:fd00916552e0 63 float pwm2;
CasperK 7:9e0ded88fe60 64 float P_Last = 0; // Starting position
CasperK 4:f36406c9e42f 65
CasperK 10:6b12d31b0fbf 66 void yDirection() {
CasperK 3:56cbed6caacc 67 //direction of the motion
CasperK 4:f36406c9e42f 68 if (emg0Bool && !emg1Bool) { //if a is pressed and not d, move to the left
CasperK 4:f36406c9e42f 69 direction = -1;
CasperK 3:56cbed6caacc 70 }
CasperK 4:f36406c9e42f 71 else if (!emg0Bool && emg1Bool) { //if d is pressed and not a, move to the right
CasperK 4:f36406c9e42f 72 direction = 1;
CasperK 3:56cbed6caacc 73 }
CasperK 1:fc216448bb57 74
CasperK 12:9a2d3d544426 75 if (emg0Bool || emg1Bool){
CasperK 12:9a2d3d544426 76 //correction from motor 1 to keep x position the same
CasperK 12:9a2d3d544426 77
CasperK 12:9a2d3d544426 78
CasperK 1:fc216448bb57 79 //calculating the motion
CasperK 10:6b12d31b0fbf 80 old_y_position = y_position;
CasperK 10:6b12d31b0fbf 81 y_position = old_y_position + (0.1f * direction);
CasperK 10:6b12d31b0fbf 82 old_motor2_angle = motor2_angle;
CasperK 12:9a2d3d544426 83 motor2_angle = asin( y_position / length )* C1; //saw tooth motor angle in rad
CasperK 12:9a2d3d544426 84 //correction on x- axis
CasperK 10:6b12d31b0fbf 85 old_x_position = x_position;
CasperK 12:9a2d3d544426 86 x_position = old_x_position + (cos(motor2_angle/ C1)-cos(old_motor2_angle/ C1)); // old x + correction
CasperK 10:6b12d31b0fbf 87 old_motor1_angle = motor1_angle;
CasperK 12:9a2d3d544426 88 motor1_angle = old_motor1_angle + ( x_position - length ) / C2;
CasperK 12:9a2d3d544426 89
CasperK 1:fc216448bb57 90 }
CasperK 4:f36406c9e42f 91
CasperK 8:fd00916552e0 92 //reset the booleans, only for demo purposes
CasperK 4:f36406c9e42f 93 emg0Bool = false;
CasperK 4:f36406c9e42f 94 emg1Bool = false;
CasperK 1:fc216448bb57 95 }
CasperK 1:fc216448bb57 96
CasperK 10:6b12d31b0fbf 97 void xDirection () {
CasperK 10:6b12d31b0fbf 98 //if the button is pressed, reverse the y direction
CasperK 10:6b12d31b0fbf 99 if (!button) {
CasperK 10:6b12d31b0fbf 100 x_direction = !x_direction;
CasperK 10:6b12d31b0fbf 101 wait(0.5f);
CasperK 10:6b12d31b0fbf 102 }
CasperK 10:6b12d31b0fbf 103
CasperK 4:f36406c9e42f 104 if (emg2Bool) { //if w is pressed, move up/down
CasperK 1:fc216448bb57 105 //direction of the motion
CasperK 10:6b12d31b0fbf 106 if (x_direction) {
CasperK 10:6b12d31b0fbf 107 direction = 1.0;
CasperK 1:fc216448bb57 108 }
CasperK 10:6b12d31b0fbf 109 else if (!x_direction) {
CasperK 10:6b12d31b0fbf 110 direction = -1.0;
CasperK 1:fc216448bb57 111 }
CasperK 2:ffd0553701d3 112
CasperK 2:ffd0553701d3 113 //calculating the motion
CasperK 10:6b12d31b0fbf 114 old_x_position = x_position;
CasperK 12:9a2d3d544426 115 x_position = old_x_position + (0.01f * direction);
CasperK 10:6b12d31b0fbf 116 old_motor1_angle = motor1_angle;
CasperK 11:325a545a757e 117 motor1_angle = old_motor1_angle + ( x_position - length ) / C2; // sawtooth-gear motor angle in rad
CasperK 4:f36406c9e42f 118
CasperK 10:6b12d31b0fbf 119 //reset the boolean, for demo purposes
CasperK 4:f36406c9e42f 120 emg2Bool = false;
CasperK 1:fc216448bb57 121 }
CasperK 1:fc216448bb57 122 }
CasperK 1:fc216448bb57 123
CasperK 12:9a2d3d544426 124 volatile float Plek1;
CasperK 12:9a2d3d544426 125 void PIDController1() {
CasperK 12:9a2d3d544426 126 Plek1 = motor1.getPulses();
CasperK 12:9a2d3d544426 127 P1 = motor1.getPulses() / 8400 * 2*PI; //actual motor angle in rad
CasperK 7:9e0ded88fe60 128 e1 = e2;
CasperK 7:9e0ded88fe60 129 e2 = e3;
CasperK 8:fd00916552e0 130 e3 = motor1_angle - P1;
CasperK 12:9a2d3d544426 131 float de3 = (e3-e2)/Timestep;
CasperK 12:9a2d3d544426 132 float ie3 = ie3 + e3*Timestep;
CasperK 12:9a2d3d544426 133 Output2 = Kp * e3 + Ki * ie3 + Kd * de3;
CasperK 12:9a2d3d544426 134
CasperK 12:9a2d3d544426 135 // Output_Last1 = Output1;
CasperK 12:9a2d3d544426 136 // Output1 = Kp * (e3 - e2) + Output_Last1 +Ki * e3 + Kd * (e3 - 2*e2 + e1);
CasperK 12:9a2d3d544426 137 Y1 = 0.5f * Output1;
CasperK 12:9a2d3d544426 138
CasperK 12:9a2d3d544426 139 if (Y1 >= 1){
CasperK 8:fd00916552e0 140 Y1 = 1;
CasperK 7:9e0ded88fe60 141 }
CasperK 12:9a2d3d544426 142 else if (Y1 <= -1){
CasperK 8:fd00916552e0 143 Y1 = -1;
CasperK 7:9e0ded88fe60 144 }
CasperK 7:9e0ded88fe60 145 }
CasperK 12:9a2d3d544426 146 volatile float Plek2;
CasperK 7:9e0ded88fe60 147 void PIDController2() {
CasperK 12:9a2d3d544426 148 Plek2 = motor2.getPulses();
CasperK 12:9a2d3d544426 149 P2 = motor2.getPulses() / 8400.0f * 2.0f*PI; // actual motor angle in rad
CasperK 7:9e0ded88fe60 150 f2 = f3;
CasperK 8:fd00916552e0 151 f3 = motor2_angle - P2;
CasperK 12:9a2d3d544426 152 float df3 = (f3-f2)/Timestep;
CasperK 12:9a2d3d544426 153 float if3 = if3 + f3*Timestep;
CasperK 12:9a2d3d544426 154 Output2 = Kp * f3 + Ki * if3 + Kd * df3;
CasperK 12:9a2d3d544426 155 // Output_Last2 = Output2;
CasperK 12:9a2d3d544426 156 // Output2 = Kp * (f3 - f2) + Output_Last2 +Ki * f3 + Kd * (f3 - 2*f2 + f1);
CasperK 12:9a2d3d544426 157 Y2 = 0.5f * Output2;
CasperK 7:9e0ded88fe60 158
CasperK 12:9a2d3d544426 159 if (Y2 >= 1){
CasperK 8:fd00916552e0 160 Y2 = 1;
CasperK 7:9e0ded88fe60 161 }
CasperK 12:9a2d3d544426 162 else if (Y2 <= -1){
CasperK 8:fd00916552e0 163 Y2 = -1;
CasperK 12:9a2d3d544426 164 }
CasperK 8:fd00916552e0 165 }
CasperK 8:fd00916552e0 166
CasperK 8:fd00916552e0 167 void ControlMotor1() {
CasperK 9:e144fd53f606 168 if (Y1 > 0) {
CasperK 8:fd00916552e0 169 Y1 = 0.6f * Y1 + 0.4f;
CasperK 8:fd00916552e0 170 directionpin1 = true;
CasperK 8:fd00916552e0 171 }
CasperK 8:fd00916552e0 172 else if(Y1 < 0){
CasperK 8:fd00916552e0 173 Y1 = 0.6f - 0.4f * Y1;
CasperK 8:fd00916552e0 174 directionpin1 = false;
CasperK 9:e144fd53f606 175 }
CasperK 12:9a2d3d544426 176 pwmpin2 = abs(Y1);
CasperK 8:fd00916552e0 177 }
CasperK 8:fd00916552e0 178
CasperK 8:fd00916552e0 179 void ControlMotor2() {
CasperK 12:9a2d3d544426 180 if (Y2 > 0) {
CasperK 12:9a2d3d544426 181 Y2 = 0.5f * Y2 + 0.5f;
CasperK 8:fd00916552e0 182 directionpin2 = true;
CasperK 8:fd00916552e0 183 }
CasperK 12:9a2d3d544426 184 else if(Y2 < 0){
CasperK 12:9a2d3d544426 185 Y2 = 0.5f - 0.5f * Y2;
CasperK 8:fd00916552e0 186 directionpin2 = false;
CasperK 8:fd00916552e0 187 }
CasperK 12:9a2d3d544426 188 pwmpin1 = abs(Y2);
CasperK 7:9e0ded88fe60 189 }
CasperK 7:9e0ded88fe60 190
CasperK 0:dc2c63f663f8 191 int main() {
CasperK 4:f36406c9e42f 192 pc.baud(115200);
CasperK 0:dc2c63f663f8 193 pc.printf(" ** program reset **\n\r");
CasperK 8:fd00916552e0 194 ledred = true;
CasperK 4:f36406c9e42f 195
CasperK 0:dc2c63f663f8 196 while (true) {
CasperK 3:56cbed6caacc 197
CasperK 4:f36406c9e42f 198 //testing the code from keyboard imputs: a-d: left to right, w: forward/backwards
CasperK 4:f36406c9e42f 199 a = pc.readable();
CasperK 4:f36406c9e42f 200 if (a) {
CasperK 4:f36406c9e42f 201 c = pc.getc();
CasperK 4:f36406c9e42f 202 switch (c){
CasperK 4:f36406c9e42f 203 case 'a': //move to the left
CasperK 4:f36406c9e42f 204 emg0Bool = true;
CasperK 4:f36406c9e42f 205 break;
CasperK 4:f36406c9e42f 206 case 'd': //move to the right
CasperK 4:f36406c9e42f 207 emg1Bool = true;
CasperK 4:f36406c9e42f 208 break;
CasperK 4:f36406c9e42f 209 case 'w': //move up/down
CasperK 4:f36406c9e42f 210 emg2Bool = true;
CasperK 4:f36406c9e42f 211 break;
CasperK 4:f36406c9e42f 212 }
CasperK 4:f36406c9e42f 213 }
CasperK 10:6b12d31b0fbf 214 xDirection(); //call the function to move in the y direction
CasperK 10:6b12d31b0fbf 215 yDirection(); //call the function to move in the x direction
CasperK 7:9e0ded88fe60 216 PIDController1();
CasperK 7:9e0ded88fe60 217 PIDController2();
CasperK 8:fd00916552e0 218 ControlMotor1();
CasperK 8:fd00916552e0 219 ControlMotor2();
CasperK 0:dc2c63f663f8 220
CasperK 9:e144fd53f606 221 if (!KillSwitch) { //even in mekaar gebeund voor het hebben van een stop knop
CasperK 8:fd00916552e0 222 ledred = false;
CasperK 8:fd00916552e0 223 pwmpin1 = 0;
CasperK 8:fd00916552e0 224 pwmpin2 = 0;
CasperK 8:fd00916552e0 225 //for fun blink sos maybe???
CasperK 8:fd00916552e0 226 wait(2.0f);
CasperK 8:fd00916552e0 227 bool u = true;
CasperK 8:fd00916552e0 228
CasperK 8:fd00916552e0 229 while(u) {
CasperK 8:fd00916552e0 230 if (!KillSwitch) {
CasperK 8:fd00916552e0 231 u = false;
CasperK 8:fd00916552e0 232 ledred = true;
CasperK 8:fd00916552e0 233 wait(1.0f);
CasperK 8:fd00916552e0 234 }
CasperK 8:fd00916552e0 235 }
CasperK 8:fd00916552e0 236 }
CasperK 12:9a2d3d544426 237 scope.set(0, Y1);
CasperK 12:9a2d3d544426 238 scope.set(1, Y2);
CasperK 12:9a2d3d544426 239 scope.set(2, motor1_angle);
CasperK 12:9a2d3d544426 240 scope.set(3, motor2_angle);
CasperK 12:9a2d3d544426 241 scope.set(4, x_position);
CasperK 12:9a2d3d544426 242 scope.set(5, y_position);
CasperK 12:9a2d3d544426 243 scope.send();
CasperK 12:9a2d3d544426 244
CasperK 5:14a68d0ee71a 245 // print the motor angles and coordinates
CasperK 5:14a68d0ee71a 246 pc.printf("position: (%f, %f)\n\r", x_position, y_position);
CasperK 5:14a68d0ee71a 247 pc.printf("motor1 angle: %f\n\r", motor1_angle);
CasperK 12:9a2d3d544426 248 pc.printf("motor2 angle: %f\n\r", motor2_angle);
CasperK 12:9a2d3d544426 249 pc.printf("output1: %f\r\n", Output1);
CasperK 12:9a2d3d544426 250 pc.printf("output2: %f\r\n", Output2);
CasperK 12:9a2d3d544426 251 pc.printf("P1: %f\r\n", P1);
CasperK 12:9a2d3d544426 252 pc.printf("P2: %f\r\n", P2);
CasperK 12:9a2d3d544426 253
CasperK 12:9a2d3d544426 254 pc.printf("dirctionpin: %f\r\n\n", (float)directionpin1);
CasperK 12:9a2d3d544426 255 pc.printf("plek1: %f\r\n", Plek1);
CasperK 12:9a2d3d544426 256 pc.printf("plek2: %f\r\n", Plek2);
CasperK 4:f36406c9e42f 257
CasperK 12:9a2d3d544426 258 wait(Timestep); //can also be done with ticker, to be sure that it happens exactly every 0.5 seconds
CasperK 9:e144fd53f606 259 pwmpin1 = 0;
CasperK 9:e144fd53f606 260 pwmpin2 = 0;
CasperK 0:dc2c63f663f8 261 }
CasperK 0:dc2c63f663f8 262 }