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@15:6f5b97d006c2, 2018-11-02 (annotated)

Committer:
Wimboo
Date:
Fri Nov 02 09:38:18 2018 +0000
Revision:
15:6f5b97d006c2
Parent:
14:a98bc99ea004 
Heyo C'aspere

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