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@10:6b12d31b0fbf, 2018-10-26 (annotated)

Committer:
CasperK
Date:
Fri Oct 26 08:16:39 2018 +0000
Revision:
10:6b12d31b0fbf
Parent:
9:e144fd53f606
Child:
11:325a545a757e
Added new kinematics

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