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@13:2d2763be031c, 2018-10-31 (annotated)

Committer:
CasperK
Date:
Wed Oct 31 16:15:56 2018 +0000
Revision:
13:2d2763be031c
Parent:
12:9a2d3d544426
Child:
14:a98bc99ea004
P controller now works

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