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@6:0162a633768d, 2018-10-22 (annotated)

Committer:
CasperK
Date:
Mon Oct 22 09:18:30 2018 +0000
Revision:
6:0162a633768d
Parent:
5:14a68d0ee71a
Child:
7:9e0ded88fe60
No more errors, and gear ratio's are implemented correctly now.; Still no programmed solution for nan outcomes

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