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@9:e144fd53f606, 2018-10-25 (annotated)

Committer:
CasperK
Date:
Thu Oct 25 14:53:29 2018 +0000
Revision:
9:e144fd53f606
Parent:
8:fd00916552e0
Child:
10:6b12d31b0fbf
Added pwm reset if no buttons are pressed

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 4:f36406c9e42f 27 volatile float x_position = 0.0;
CasperK 4:f36406c9e42f 28 volatile float y_position = 0.0;
CasperK 6:0162a633768d 29 volatile float y_position2 = 0.0;
CasperK 4:f36406c9e42f 30 volatile float old_y;
CasperK 6:0162a633768d 31 volatile float old_y2;
CasperK 4:f36406c9e42f 32 volatile float old_x;
CasperK 6:0162a633768d 33 volatile float motor1_angle = 0.0; //circular gear motor
CasperK 6:0162a633768d 34 volatile float motor2_angle = 0.0; //sawtooth gear motor
CasperK 5:14a68d0ee71a 35 volatile float direction;
CasperK 4:f36406c9e42f 36 volatile char c;
CasperK 1:fc216448bb57 37
CasperK 1:fc216448bb57 38 const float length = 0.300; //length in m (placeholder)
CasperK 6:0162a633768d 39 const float C1 = 3; //motor 1 gear ratio
CasperK 6:0162a633768d 40 const float C2 = 3; //motor 2 gear ratio/radius of the circular gear (placeholder)
CasperK 7:9e0ded88fe60 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 3:56cbed6caacc 64 void xDirection() {
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 4:f36406c9e42f 79 old_x = x_position;
CasperK 6:0162a633768d 80 x_position = old_x + (0.1f * direction);
CasperK 6:0162a633768d 81 motor1_angle = asin( x_position / (length * C1 )); //rotational motor angle in rad
CasperK 4:f36406c9e42f 82
CasperK 6:0162a633768d 83 old_y2 = y_position2;
CasperK 6:0162a633768d 84 y_position2 = old_y2 + (0.1f * direction);
CasperK 6:0162a633768d 85 motor2_angle = ( y_position / C2) + acos( x_position / length ); //sawtooth-gear motor angle in rad
CasperK 1:fc216448bb57 86 }
CasperK 9:e144fd53f606 87 else {
CasperK 9:e144fd53f606 88 pwmpin1 = 0;
CasperK 9:e144fd53f606 89 pwmpin2 = 0;
CasperK 9:e144fd53f606 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 3:56cbed6caacc 97 void yDirection () {
CasperK 4:f36406c9e42f 98 if (emg2Bool) { //if w is pressed, move up/down
CasperK 1:fc216448bb57 99 //direction of the motion
CasperK 4:f36406c9e42f 100 if (y_direction) {
CasperK 8:fd00916552e0 101 // directionpin2 = true;
CasperK 2:ffd0553701d3 102 direction = 1;
CasperK 1:fc216448bb57 103 }
CasperK 1:fc216448bb57 104 else if (!y_direction) {
CasperK 8:fd00916552e0 105 // directionpin2 = false;
CasperK 2:ffd0553701d3 106 direction = -1;
CasperK 1:fc216448bb57 107 }
CasperK 2:ffd0553701d3 108
CasperK 2:ffd0553701d3 109 //calculating the motion
CasperK 4:f36406c9e42f 110 old_y = y_position;
CasperK 6:0162a633768d 111 y_position = old_y + (0.1f * direction);
CasperK 6:0162a633768d 112 motor2_angle = y_position / C2; // sawtooth-gear motor angle in rad
CasperK 4:f36406c9e42f 113
CasperK 8:fd00916552e0 114 //reset the boolean, only for demo purposes
CasperK 4:f36406c9e42f 115 emg2Bool = false;
CasperK 1:fc216448bb57 116 }
CasperK 9:e144fd53f606 117 else {
CasperK 9:e144fd53f606 118 pwmpin1 = false;
CasperK 9:e144fd53f606 119 }
CasperK 1:fc216448bb57 120 }
CasperK 1:fc216448bb57 121
CasperK 7:9e0ded88fe60 122 void PIDController1() {
CasperK 9:e144fd53f606 123 P1 = motor1.getPosition() / 8400 * 360; // <-- needs to be in radians, or motor1_angle in degrees!!!
CasperK 7:9e0ded88fe60 124 e1 = e2;
CasperK 7:9e0ded88fe60 125 e2 = e3;
CasperK 8:fd00916552e0 126 e3 = motor1_angle - P1;
CasperK 7:9e0ded88fe60 127 Output_Last1 = Output1;
CasperK 7:9e0ded88fe60 128 Output1 = Kp * (e3 - e2) + Output_Last1 +Ki * e3 + Kd * (e3 - 2*e2 + e1);
CasperK 8:fd00916552e0 129 Y1 = Output1;
CasperK 7:9e0ded88fe60 130
CasperK 7:9e0ded88fe60 131 if (Output1 >= 1){
CasperK 8:fd00916552e0 132 Y1 = 1;
CasperK 7:9e0ded88fe60 133 }
CasperK 7:9e0ded88fe60 134 else if (Output1 <= -1){
CasperK 8:fd00916552e0 135 Y1 = -1;
CasperK 7:9e0ded88fe60 136 }
CasperK 7:9e0ded88fe60 137
CasperK 9:e144fd53f606 138 /* scope.set(0,Output1);
CasperK 8:fd00916552e0 139 scope.set(1,P1);
CasperK 7:9e0ded88fe60 140 scope.send();
CasperK 9:e144fd53f606 141 */ pc.printf("motor1 encoder: %f\r\n", P1);
CasperK 7:9e0ded88fe60 142 }
CasperK 7:9e0ded88fe60 143
CasperK 7:9e0ded88fe60 144 void PIDController2() {
CasperK 9:e144fd53f606 145 P2 = motor2.getPosition() / 8400 * 360; // <-- needs to be in radians, or motor2_angle in degrees!!!
CasperK 7:9e0ded88fe60 146 f1 = f2;
CasperK 7:9e0ded88fe60 147 f2 = f3;
CasperK 8:fd00916552e0 148 f3 = motor2_angle - P2;
CasperK 7:9e0ded88fe60 149 Output_Last2 = Output2;
CasperK 7:9e0ded88fe60 150 Output2 = Kp * (f3 - f2) + Output_Last2 +Ki * f3 + Kd * (f3 - 2*f2 + f1);
CasperK 8:fd00916552e0 151 Y2 = Output2;
CasperK 7:9e0ded88fe60 152
CasperK 7:9e0ded88fe60 153 if (Output2 >= 1){
CasperK 8:fd00916552e0 154 Y2 = 1;
CasperK 7:9e0ded88fe60 155 }
CasperK 7:9e0ded88fe60 156 else if (Output2 <= -1){
CasperK 8:fd00916552e0 157 Y2 = -1;
CasperK 7:9e0ded88fe60 158 }
CasperK 7:9e0ded88fe60 159
CasperK 9:e144fd53f606 160 /* scope.set(0,Output2);
CasperK 8:fd00916552e0 161 scope.set(1,P2);
CasperK 7:9e0ded88fe60 162 scope.send();
CasperK 9:e144fd53f606 163 */ pc.printf("motor2 encoder: %f\r\n", P2);
CasperK 8:fd00916552e0 164 }
CasperK 8:fd00916552e0 165
CasperK 8:fd00916552e0 166 void ControlMotor1() {
CasperK 9:e144fd53f606 167 if (Y1 > 0) {
CasperK 8:fd00916552e0 168 Y1 = 0.6f * Y1 + 0.4f;
CasperK 8:fd00916552e0 169 directionpin1 = true;
CasperK 8:fd00916552e0 170 }
CasperK 8:fd00916552e0 171 else if(Y1 < 0){
CasperK 8:fd00916552e0 172 Y1 = 0.6f - 0.4f * Y1;
CasperK 8:fd00916552e0 173 directionpin1 = false;
CasperK 9:e144fd53f606 174 }
CasperK 9:e144fd53f606 175 pwmpin1 = abs(Y1);
CasperK 8:fd00916552e0 176 }
CasperK 8:fd00916552e0 177
CasperK 8:fd00916552e0 178 void ControlMotor2() {
CasperK 9:e144fd53f606 179 if (Y1 > 0) {
CasperK 8:fd00916552e0 180 Y1 = 0.5f * Y1 + 0.5f;
CasperK 8:fd00916552e0 181 directionpin2 = true;
CasperK 8:fd00916552e0 182 }
CasperK 8:fd00916552e0 183 else if(Y1 < 0){
CasperK 8:fd00916552e0 184 Y1 = 0.5f - 0.5f * Y1;
CasperK 8:fd00916552e0 185 directionpin2 = false;
CasperK 8:fd00916552e0 186 }
CasperK 9:e144fd53f606 187 pwmpin2 = abs(Y2);
CasperK 7:9e0ded88fe60 188 }
CasperK 7:9e0ded88fe60 189
CasperK 0:dc2c63f663f8 190 int main() {
CasperK 4:f36406c9e42f 191 pc.baud(115200);
CasperK 0:dc2c63f663f8 192 pc.printf(" ** program reset **\n\r");
CasperK 8:fd00916552e0 193 ledred = true;
CasperK 4:f36406c9e42f 194
CasperK 0:dc2c63f663f8 195 while (true) {
CasperK 4:f36406c9e42f 196 //if the button is pressed, reverse the y direction
CasperK 4:f36406c9e42f 197 if (!button) {
CasperK 3:56cbed6caacc 198 y_direction = !y_direction;
CasperK 8:fd00916552e0 199 wait(0.5f); //wait for person to release the button
CasperK 3:56cbed6caacc 200 }
CasperK 3:56cbed6caacc 201
CasperK 4:f36406c9e42f 202 //testing the code from keyboard imputs: a-d: left to right, w: forward/backwards
CasperK 4:f36406c9e42f 203 a = pc.readable();
CasperK 4:f36406c9e42f 204 if (a) {
CasperK 4:f36406c9e42f 205 c = pc.getc();
CasperK 4:f36406c9e42f 206 switch (c){
CasperK 4:f36406c9e42f 207 case 'a': //move to the left
CasperK 4:f36406c9e42f 208 emg0Bool = true;
CasperK 4:f36406c9e42f 209 break;
CasperK 4:f36406c9e42f 210 case 'd': //move to the right
CasperK 4:f36406c9e42f 211 emg1Bool = true;
CasperK 4:f36406c9e42f 212 break;
CasperK 4:f36406c9e42f 213 case 'w': //move up/down
CasperK 4:f36406c9e42f 214 emg2Bool = true;
CasperK 4:f36406c9e42f 215 break;
CasperK 4:f36406c9e42f 216 }
CasperK 4:f36406c9e42f 217 }
CasperK 7:9e0ded88fe60 218 yDirection(); //call the function to move in the y direction
CasperK 9:e144fd53f606 219 pc.printf("yDirection: motor2angle=%f)", motor2_angle);
CasperK 4:f36406c9e42f 220 xDirection(); //call the function to move in the x direction
CasperK 9:e144fd53f606 221 pc.printf("xDirection: motor1angle=%f, motor2angle=%f", motor1_angle);
CasperK 7:9e0ded88fe60 222 PIDController1();
CasperK 7:9e0ded88fe60 223 PIDController2();
CasperK 8:fd00916552e0 224 ControlMotor1();
CasperK 8:fd00916552e0 225 ControlMotor2();
CasperK 0:dc2c63f663f8 226
CasperK 9:e144fd53f606 227 if (!KillSwitch) { //even in mekaar gebeund voor het hebben van een stop knop
CasperK 8:fd00916552e0 228 ledred = false;
CasperK 8:fd00916552e0 229 pwmpin1 = 0;
CasperK 8:fd00916552e0 230 pwmpin2 = 0;
CasperK 8:fd00916552e0 231 //for fun blink sos maybe???
CasperK 8:fd00916552e0 232 wait(2.0f);
CasperK 8:fd00916552e0 233 bool u = true;
CasperK 8:fd00916552e0 234
CasperK 8:fd00916552e0 235 while(u) {
CasperK 8:fd00916552e0 236 if (!KillSwitch) {
CasperK 8:fd00916552e0 237 u = false;
CasperK 8:fd00916552e0 238 ledred = true;
CasperK 8:fd00916552e0 239 wait(1.0f);
CasperK 8:fd00916552e0 240 }
CasperK 8:fd00916552e0 241 }
CasperK 8:fd00916552e0 242 }
CasperK 8:fd00916552e0 243
CasperK 5:14a68d0ee71a 244 // print the motor angles and coordinates
CasperK 5:14a68d0ee71a 245 pc.printf("position: (%f, %f)\n\r", x_position, y_position);
CasperK 5:14a68d0ee71a 246 pc.printf("motor1 angle: %f\n\r", motor1_angle);
CasperK 5:14a68d0ee71a 247 pc.printf("motor2 angle: %f\n\r\n", motor2_angle);
CasperK 4:f36406c9e42f 248
CasperK 9:e144fd53f606 249 wait(0.25f); //can also be done with ticker, to be sure that it happens exactly every 0.5 seconds
CasperK 9:e144fd53f606 250 pwmpin1 = 0;
CasperK 9:e144fd53f606 251 pwmpin2 = 0;
CasperK 0:dc2c63f663f8 252 }
CasperK 0:dc2c63f663f8 253 }