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

Committer:
CasperK
Date:
Mon Oct 22 10:00:09 2018 +0000
Revision:
7:9e0ded88fe60
Parent:
6:0162a633768d
Child:
8:fd00916552e0
First issue without errors (not tested yet)

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 0:dc2c63f663f8 7 DigitalIn button(SW2);
CasperK 0:dc2c63f663f8 8 DigitalOut directionpin1(D7);
CasperK 0:dc2c63f663f8 9 DigitalOut directionpin2(D8);
CasperK 0:dc2c63f663f8 10 MODSERIAL pc(USBTX, USBRX);
CasperK 7:9e0ded88fe60 11 HIDScope scope(2);
CasperK 7:9e0ded88fe60 12 Encoder motor1(D13,D12);
CasperK 7:9e0ded88fe60 13 Encoder motor2(D13,D12); //has to be changed!!!
CasperK 0:dc2c63f663f8 14
CasperK 7:9e0ded88fe60 15 //values of inverse kinematics
CasperK 3:56cbed6caacc 16 volatile bool emg0Bool = false;
CasperK 3:56cbed6caacc 17 volatile bool emg1Bool = false;
CasperK 3:56cbed6caacc 18 volatile bool emg2Bool = false;
CasperK 3:56cbed6caacc 19 volatile bool y_direction = true;
CasperK 4:f36406c9e42f 20 volatile bool a;
CasperK 1:fc216448bb57 21
CasperK 4:f36406c9e42f 22 volatile float x_position = 0.0;
CasperK 4:f36406c9e42f 23 volatile float y_position = 0.0;
CasperK 6:0162a633768d 24 volatile float y_position2 = 0.0;
CasperK 4:f36406c9e42f 25 volatile float old_y;
CasperK 6:0162a633768d 26 volatile float old_y2;
CasperK 4:f36406c9e42f 27 volatile float old_x;
CasperK 6:0162a633768d 28 volatile float motor1_angle = 0.0; //circular gear motor
CasperK 6:0162a633768d 29 volatile float motor2_angle = 0.0; //sawtooth gear motor
CasperK 5:14a68d0ee71a 30 volatile float direction;
CasperK 4:f36406c9e42f 31 volatile char c;
CasperK 1:fc216448bb57 32
CasperK 1:fc216448bb57 33 const float length = 0.300; //length in m (placeholder)
CasperK 6:0162a633768d 34 const float C1 = 3; //motor 1 gear ratio
CasperK 6:0162a633768d 35 const float C2 = 3; //motor 2 gear ratio/radius of the circular gear (placeholder)
CasperK 7:9e0ded88fe60 36
CasperK 7:9e0ded88fe60 37 //values of PID controller
CasperK 7:9e0ded88fe60 38 const float Kp = 2;
CasperK 7:9e0ded88fe60 39 const float Ki = 0.2;
CasperK 7:9e0ded88fe60 40 const float Kd = 0;
CasperK 7:9e0ded88fe60 41 const float Timestep = 0.001;
CasperK 7:9e0ded88fe60 42 float G ; //desired motor angle
CasperK 7:9e0ded88fe60 43 float Output1 = 0 ; //Starting value
CasperK 7:9e0ded88fe60 44 float Output2 = 0 ; //Starting value
CasperK 7:9e0ded88fe60 45 float P = 0; //encoder value
CasperK 7:9e0ded88fe60 46 float e1 = 0 ; //Starting value
CasperK 7:9e0ded88fe60 47 float e2 = 0 ; //Starting value
CasperK 7:9e0ded88fe60 48 float e3;
CasperK 7:9e0ded88fe60 49 float f1 = 0 ; //Starting value
CasperK 7:9e0ded88fe60 50 float f2 = 0 ; //Starting value
CasperK 7:9e0ded88fe60 51 float f3;
CasperK 7:9e0ded88fe60 52 float Output_Last1; // Remember previous position
CasperK 7:9e0ded88fe60 53 float Output_Last2; // Remember previous position
CasperK 7:9e0ded88fe60 54 float Y; // Value that is outputted to motor control
CasperK 7:9e0ded88fe60 55 float P_Last = 0; // Starting position
CasperK 7:9e0ded88fe60 56 const float Max_Speed = 400; //Max speed of the motor
CasperK 4:f36406c9e42f 57
CasperK 3:56cbed6caacc 58 void xDirection() {
CasperK 3:56cbed6caacc 59 //direction of the motion
CasperK 4:f36406c9e42f 60 if (emg0Bool && !emg1Bool) { //if a is pressed and not d, move to the left
CasperK 3:56cbed6caacc 61 directionpin1 = true;
CasperK 3:56cbed6caacc 62 directionpin2 = true;
CasperK 4:f36406c9e42f 63 direction = -1;
CasperK 3:56cbed6caacc 64 }
CasperK 4:f36406c9e42f 65 else if (!emg0Bool && emg1Bool) { //if d is pressed and not a, move to the right
CasperK 3:56cbed6caacc 66 directionpin1 = false;
CasperK 3:56cbed6caacc 67 directionpin2 = false;
CasperK 4:f36406c9e42f 68 direction = 1;
CasperK 3:56cbed6caacc 69 }
CasperK 1:fc216448bb57 70
CasperK 3:56cbed6caacc 71 if (emg0Bool || emg1Bool){
CasperK 1:fc216448bb57 72 //calculating the motion
CasperK 4:f36406c9e42f 73 old_x = x_position;
CasperK 6:0162a633768d 74 x_position = old_x + (0.1f * direction);
CasperK 6:0162a633768d 75 motor1_angle = asin( x_position / (length * C1 )); //rotational motor angle in rad
CasperK 4:f36406c9e42f 76
CasperK 6:0162a633768d 77 old_y2 = y_position2;
CasperK 6:0162a633768d 78 y_position2 = old_y2 + (0.1f * direction);
CasperK 6:0162a633768d 79 motor2_angle = ( y_position / C2) + acos( x_position / length ); //sawtooth-gear motor angle in rad
CasperK 1:fc216448bb57 80 }
CasperK 4:f36406c9e42f 81
CasperK 4:f36406c9e42f 82 //reset the booleans
CasperK 4:f36406c9e42f 83 emg0Bool = false;
CasperK 4:f36406c9e42f 84 emg1Bool = false;
CasperK 1:fc216448bb57 85 }
CasperK 1:fc216448bb57 86
CasperK 3:56cbed6caacc 87 void yDirection () {
CasperK 4:f36406c9e42f 88 if (emg2Bool) { //if w is pressed, move up/down
CasperK 1:fc216448bb57 89 //direction of the motion
CasperK 4:f36406c9e42f 90 if (y_direction) {
CasperK 1:fc216448bb57 91 directionpin2 = true;
CasperK 2:ffd0553701d3 92 direction = 1;
CasperK 1:fc216448bb57 93 }
CasperK 1:fc216448bb57 94 else if (!y_direction) {
CasperK 1:fc216448bb57 95 directionpin2 = false;
CasperK 2:ffd0553701d3 96 direction = -1;
CasperK 1:fc216448bb57 97 }
CasperK 2:ffd0553701d3 98
CasperK 2:ffd0553701d3 99 //calculating the motion
CasperK 4:f36406c9e42f 100 old_y = y_position;
CasperK 6:0162a633768d 101 y_position = old_y + (0.1f * direction);
CasperK 6:0162a633768d 102 motor2_angle = y_position / C2; // sawtooth-gear motor angle in rad
CasperK 4:f36406c9e42f 103
CasperK 4:f36406c9e42f 104 //reset the boolean
CasperK 4:f36406c9e42f 105 emg2Bool = false;
CasperK 1:fc216448bb57 106 }
CasperK 1:fc216448bb57 107 }
CasperK 1:fc216448bb57 108
CasperK 7:9e0ded88fe60 109 void PIDController1() {
CasperK 7:9e0ded88fe60 110 P = motor1.getPosition() / 8400 * 360;
CasperK 7:9e0ded88fe60 111 e1 = e2;
CasperK 7:9e0ded88fe60 112 e2 = e3;
CasperK 7:9e0ded88fe60 113 e3 = G - P;
CasperK 7:9e0ded88fe60 114 Output_Last1 = Output1;
CasperK 7:9e0ded88fe60 115 Output1 = Kp * (e3 - e2) + Output_Last1 +Ki * e3 + Kd * (e3 - 2*e2 + e1);
CasperK 7:9e0ded88fe60 116 Y = Output1;
CasperK 7:9e0ded88fe60 117
CasperK 7:9e0ded88fe60 118 if (Output1 >= 1){
CasperK 7:9e0ded88fe60 119 Y = 1;
CasperK 7:9e0ded88fe60 120 }
CasperK 7:9e0ded88fe60 121 else if (Output1 <= -1){
CasperK 7:9e0ded88fe60 122 Y = -1;
CasperK 7:9e0ded88fe60 123 }
CasperK 7:9e0ded88fe60 124 P = P_Last + Y * Timestep * Max_Speed;
CasperK 7:9e0ded88fe60 125
CasperK 7:9e0ded88fe60 126 scope.set(0,Output1);
CasperK 7:9e0ded88fe60 127 scope.set(1,P);
CasperK 7:9e0ded88fe60 128 scope.send();
CasperK 7:9e0ded88fe60 129 }
CasperK 7:9e0ded88fe60 130
CasperK 7:9e0ded88fe60 131 void PIDController2() {
CasperK 7:9e0ded88fe60 132 P = motor2.getPosition() / 8400 * 360;
CasperK 7:9e0ded88fe60 133 f1 = f2;
CasperK 7:9e0ded88fe60 134 f2 = f3;
CasperK 7:9e0ded88fe60 135 f3 = G - P;
CasperK 7:9e0ded88fe60 136 Output_Last2 = Output2;
CasperK 7:9e0ded88fe60 137 Output2 = Kp * (f3 - f2) + Output_Last2 +Ki * f3 + Kd * (f3 - 2*f2 + f1);
CasperK 7:9e0ded88fe60 138 Y = Output2;
CasperK 7:9e0ded88fe60 139
CasperK 7:9e0ded88fe60 140 if (Output2 >= 1){
CasperK 7:9e0ded88fe60 141 Y = 1;
CasperK 7:9e0ded88fe60 142 }
CasperK 7:9e0ded88fe60 143 else if (Output2 <= -1){
CasperK 7:9e0ded88fe60 144 Y = -1;
CasperK 7:9e0ded88fe60 145 }
CasperK 7:9e0ded88fe60 146 P = P_Last + Y * Timestep * Max_Speed;
CasperK 7:9e0ded88fe60 147
CasperK 7:9e0ded88fe60 148 scope.set(0,Output2);
CasperK 7:9e0ded88fe60 149 scope.set(1,P);
CasperK 7:9e0ded88fe60 150 scope.send();
CasperK 7:9e0ded88fe60 151 }
CasperK 7:9e0ded88fe60 152
CasperK 0:dc2c63f663f8 153 int main() {
CasperK 4:f36406c9e42f 154 pc.baud(115200);
CasperK 0:dc2c63f663f8 155 pc.printf(" ** program reset **\n\r");
CasperK 4:f36406c9e42f 156
CasperK 0:dc2c63f663f8 157 while (true) {
CasperK 4:f36406c9e42f 158 //if the button is pressed, reverse the y direction
CasperK 4:f36406c9e42f 159 if (!button) {
CasperK 3:56cbed6caacc 160 y_direction = !y_direction;
CasperK 4:f36406c9e42f 161 wait(0.5);
CasperK 3:56cbed6caacc 162 }
CasperK 3:56cbed6caacc 163
CasperK 4:f36406c9e42f 164 //testing the code from keyboard imputs: a-d: left to right, w: forward/backwards
CasperK 4:f36406c9e42f 165 a = pc.readable();
CasperK 4:f36406c9e42f 166 if (a) {
CasperK 4:f36406c9e42f 167 c = pc.getc();
CasperK 4:f36406c9e42f 168 switch (c){
CasperK 4:f36406c9e42f 169 case 'a': //move to the left
CasperK 4:f36406c9e42f 170 emg0Bool = true;
CasperK 4:f36406c9e42f 171 break;
CasperK 4:f36406c9e42f 172 case 'd': //move to the right
CasperK 4:f36406c9e42f 173 emg1Bool = true;
CasperK 4:f36406c9e42f 174 break;
CasperK 4:f36406c9e42f 175 case 'w': //move up/down
CasperK 4:f36406c9e42f 176 emg2Bool = true;
CasperK 4:f36406c9e42f 177 break;
CasperK 4:f36406c9e42f 178 }
CasperK 4:f36406c9e42f 179 }
CasperK 7:9e0ded88fe60 180 yDirection(); //call the function to move in the y direction
CasperK 4:f36406c9e42f 181 xDirection(); //call the function to move in the x direction
CasperK 7:9e0ded88fe60 182 PIDController1();
CasperK 7:9e0ded88fe60 183 PIDController2();
CasperK 0:dc2c63f663f8 184
CasperK 5:14a68d0ee71a 185 // print the motor angles and coordinates
CasperK 5:14a68d0ee71a 186 pc.printf("position: (%f, %f)\n\r", x_position, y_position);
CasperK 5:14a68d0ee71a 187 pc.printf("motor1 angle: %f\n\r", motor1_angle);
CasperK 5:14a68d0ee71a 188 pc.printf("motor2 angle: %f\n\r\n", motor2_angle);
CasperK 4:f36406c9e42f 189
CasperK 5:14a68d0ee71a 190 wait(0.5f); //can also be done with ticker, to be sure that it happens exactly every 0.5 seconds
CasperK 0:dc2c63f663f8 191 }
CasperK 0:dc2c63f663f8 192 }