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@5:14a68d0ee71a, 2018-10-21 (annotated)

Committer:
CasperK
Date:
Sun Oct 21 12:20:59 2018 +0000
Revision:
5:14a68d0ee71a
Parent:
4:f36406c9e42f
Child:
6:0162a633768d
nan

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 0:dc2c63f663f8 4
CasperK 0:dc2c63f663f8 5 DigitalIn button(SW2);
CasperK 0:dc2c63f663f8 6 DigitalOut directionpin1(D7);
CasperK 0:dc2c63f663f8 7 DigitalOut directionpin2(D8);
CasperK 0:dc2c63f663f8 8 MODSERIAL pc(USBTX, USBRX);
CasperK 0:dc2c63f663f8 9
CasperK 4:f36406c9e42f 10 Ticker ticker;
CasperK 4:f36406c9e42f 11
CasperK 3:56cbed6caacc 12 volatile bool emg0Bool = false;
CasperK 3:56cbed6caacc 13 volatile bool emg1Bool = false;
CasperK 3:56cbed6caacc 14 volatile bool emg2Bool = false;
CasperK 3:56cbed6caacc 15 volatile bool y_direction = true;
CasperK 4:f36406c9e42f 16 volatile bool a;
CasperK 1:fc216448bb57 17
CasperK 4:f36406c9e42f 18 volatile float x_position = 0.0;
CasperK 4:f36406c9e42f 19 volatile float y_position = 0.0;
CasperK 4:f36406c9e42f 20 volatile float old_y;
CasperK 4:f36406c9e42f 21 volatile float old_x;
CasperK 5:14a68d0ee71a 22 volatile float motor1_angle = 0.0;
CasperK 5:14a68d0ee71a 23 volatile float motor2_angle = 0.0;
CasperK 5:14a68d0ee71a 24 volatile float direction;
CasperK 4:f36406c9e42f 25 volatile char c;
CasperK 1:fc216448bb57 26
CasperK 1:fc216448bb57 27 const float length = 0.300; //length in m (placeholder)
CasperK 5:14a68d0ee71a 28 const float C1 = 3; //motor 1 gear ratio (placeholder)
CasperK 5:14a68d0ee71a 29 const float C2 = 3; //motor 2 gear ratio
CasperK 4:f36406c9e42f 30
CasperK 3:56cbed6caacc 31 void xDirection() {
CasperK 3:56cbed6caacc 32 //direction of the motion
CasperK 4:f36406c9e42f 33 if (emg0Bool && !emg1Bool) { //if a is pressed and not d, move to the left
CasperK 3:56cbed6caacc 34 directionpin1 = true;
CasperK 3:56cbed6caacc 35 directionpin2 = true;
CasperK 4:f36406c9e42f 36 direction = -1;
CasperK 3:56cbed6caacc 37 }
CasperK 4:f36406c9e42f 38 else if (!emg0Bool && emg1Bool) { //if d is pressed and not a, move to the right
CasperK 3:56cbed6caacc 39 directionpin1 = false;
CasperK 3:56cbed6caacc 40 directionpin2 = false;
CasperK 4:f36406c9e42f 41 direction = 1;
CasperK 3:56cbed6caacc 42 }
CasperK 1:fc216448bb57 43
CasperK 3:56cbed6caacc 44 if (emg0Bool || emg1Bool){
CasperK 1:fc216448bb57 45 //calculating the motion
CasperK 4:f36406c9e42f 46 old_x = x_position;
CasperK 4:f36406c9e42f 47 x_position = old_x + direction;
CasperK 5:14a68d0ee71a 48 motor1_angle = asin( x_position / length); //x = L*sin(theta) -> theta = arcsin(x/L)
CasperK 4:f36406c9e42f 49
CasperK 4:f36406c9e42f 50 old_y = y_position;
CasperK 4:f36406c9e42f 51 y_position = old_y + direction;
CasperK 5:14a68d0ee71a 52 motor2_angle = acos( x_position / length ); //theta = arccos(x/L)
CasperK 1:fc216448bb57 53 }
CasperK 4:f36406c9e42f 54
CasperK 4:f36406c9e42f 55 //reset the booleans
CasperK 4:f36406c9e42f 56 emg0Bool = false;
CasperK 4:f36406c9e42f 57 emg1Bool = false;
CasperK 1:fc216448bb57 58 }
CasperK 1:fc216448bb57 59
CasperK 3:56cbed6caacc 60 void yDirection () {
CasperK 4:f36406c9e42f 61 if (emg2Bool) { //if w is pressed, move up/down
CasperK 1:fc216448bb57 62 //direction of the motion
CasperK 4:f36406c9e42f 63 if (y_direction) {
CasperK 1:fc216448bb57 64 directionpin2 = true;
CasperK 2:ffd0553701d3 65 direction = 1;
CasperK 1:fc216448bb57 66 }
CasperK 1:fc216448bb57 67 else if (!y_direction) {
CasperK 1:fc216448bb57 68 directionpin2 = false;
CasperK 2:ffd0553701d3 69 direction = -1;
CasperK 1:fc216448bb57 70 }
CasperK 2:ffd0553701d3 71
CasperK 2:ffd0553701d3 72 //calculating the motion
CasperK 4:f36406c9e42f 73 old_y = y_position;
CasperK 4:f36406c9e42f 74 y_position = old_y + direction;
CasperK 2:ffd0553701d3 75 motor2_angle = C2 * y_position;
CasperK 4:f36406c9e42f 76
CasperK 4:f36406c9e42f 77 //reset the boolean
CasperK 4:f36406c9e42f 78 emg2Bool = false;
CasperK 1:fc216448bb57 79 }
CasperK 1:fc216448bb57 80 }
CasperK 1:fc216448bb57 81
CasperK 0:dc2c63f663f8 82 int main() {
CasperK 4:f36406c9e42f 83 pc.baud(115200);
CasperK 0:dc2c63f663f8 84 pc.printf(" ** program reset **\n\r");
CasperK 4:f36406c9e42f 85
CasperK 0:dc2c63f663f8 86 while (true) {
CasperK 4:f36406c9e42f 87 //if the button is pressed, reverse the y direction
CasperK 4:f36406c9e42f 88 if (!button) {
CasperK 3:56cbed6caacc 89 y_direction = !y_direction;
CasperK 4:f36406c9e42f 90 wait(0.5);
CasperK 3:56cbed6caacc 91 }
CasperK 3:56cbed6caacc 92
CasperK 4:f36406c9e42f 93 //testing the code from keyboard imputs: a-d: left to right, w: forward/backwards
CasperK 4:f36406c9e42f 94 a = pc.readable();
CasperK 4:f36406c9e42f 95 if (a) {
CasperK 4:f36406c9e42f 96 c = pc.getc();
CasperK 4:f36406c9e42f 97 switch (c){
CasperK 4:f36406c9e42f 98 case 'a': //move to the left
CasperK 4:f36406c9e42f 99 emg0Bool = true;
CasperK 4:f36406c9e42f 100 break;
CasperK 4:f36406c9e42f 101 case 'd': //move to the right
CasperK 4:f36406c9e42f 102 emg1Bool = true;
CasperK 4:f36406c9e42f 103 break;
CasperK 4:f36406c9e42f 104 case 'w': //move up/down
CasperK 4:f36406c9e42f 105 emg2Bool = true;
CasperK 4:f36406c9e42f 106 break;
CasperK 4:f36406c9e42f 107 }
CasperK 4:f36406c9e42f 108 }
CasperK 4:f36406c9e42f 109 xDirection(); //call the function to move in the x direction
CasperK 4:f36406c9e42f 110 yDirection(); //call the function to move in the y direction
CasperK 0:dc2c63f663f8 111
CasperK 5:14a68d0ee71a 112 // print the motor angles and coordinates
CasperK 5:14a68d0ee71a 113 pc.printf("position: (%f, %f)\n\r", x_position, y_position);
CasperK 5:14a68d0ee71a 114 pc.printf("motor1 angle: %f\n\r", motor1_angle);
CasperK 5:14a68d0ee71a 115 pc.printf("motor2 angle: %f\n\r\n", motor2_angle);
CasperK 4:f36406c9e42f 116
CasperK 5:14a68d0ee71a 117 wait(0.5f); //can also be done with ticker, to be sure that it happens exactly every 0.5 seconds
CasperK 0:dc2c63f663f8 118 }
CasperK 0:dc2c63f663f8 119 }