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@11:325a545a757e, 2018-10-26 (annotated)

Committer:
CasperK
Date:
Fri Oct 26 08:34:50 2018 +0000
Revision:
11:325a545a757e
Parent:
10:6b12d31b0fbf
Child:
12:9a2d3d544426
everything in rad

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