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@8:fd00916552e0, 2018-10-23 (annotated)

Committer:
CasperK
Date:
Tue Oct 23 13:15:22 2018 +0000
Revision:
8:fd00916552e0
Parent:
7:9e0ded88fe60
Child:
9:e144fd53f606
Added Killswitch; PIDController doesn't work properly

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