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@3:56cbed6caacc, 2018-10-17 (annotated)

Committer:
CasperK
Date:
Wed Oct 17 13:36:47 2018 +0000
Revision:
3:56cbed6caacc
Parent:
2:ffd0553701d3
Child:
4:f36406c9e42f
Now without errors

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 3:56cbed6caacc 10 volatile bool emg0Bool = false;
CasperK 3:56cbed6caacc 11 volatile bool emg1Bool = false;
CasperK 3:56cbed6caacc 12 volatile bool emg2Bool = false;
CasperK 3:56cbed6caacc 13 volatile bool y_direction = true;
CasperK 1:fc216448bb57 14
CasperK 1:fc216448bb57 15 volatile double x_position = 0;
CasperK 1:fc216448bb57 16 volatile double y_position = 0;
CasperK 1:fc216448bb57 17 volatile double motor1_angle;
CasperK 1:fc216448bb57 18 volatile double motor2_angle;
CasperK 3:56cbed6caacc 19 volatile int direction;
CasperK 1:fc216448bb57 20
CasperK 1:fc216448bb57 21 const float length = 0.300; //length in m (placeholder)
CasperK 1:fc216448bb57 22 const int C1 = 3; //motor 1 gear ratio (placeholder)
CasperK 1:fc216448bb57 23 const int C2 = 3; //motor 2 gear ratio
CasperK 0:dc2c63f663f8 24
CasperK 3:56cbed6caacc 25 void xDirection() {
CasperK 3:56cbed6caacc 26 //direction of the motion
CasperK 3:56cbed6caacc 27 if (emg0Bool && !emg1Bool) {
CasperK 3:56cbed6caacc 28 directionpin1 = true;
CasperK 3:56cbed6caacc 29 directionpin2 = true;
CasperK 3:56cbed6caacc 30 direction = 1;
CasperK 3:56cbed6caacc 31 }
CasperK 3:56cbed6caacc 32 else if (!emg0Bool && emg1Bool) {
CasperK 3:56cbed6caacc 33 directionpin1 = false;
CasperK 3:56cbed6caacc 34 directionpin2 = false;
CasperK 3:56cbed6caacc 35 direction = -1;
CasperK 3:56cbed6caacc 36 }
CasperK 1:fc216448bb57 37
CasperK 3:56cbed6caacc 38 if (emg0Bool || emg1Bool){
CasperK 1:fc216448bb57 39 //calculating the motion
CasperK 1:fc216448bb57 40 x_position = x_position + direction;
CasperK 1:fc216448bb57 41 motor1_angle = pow(sin( x_position ), -1) / length;
CasperK 1:fc216448bb57 42 y_position = y_position + direction;
CasperK 1:fc216448bb57 43 motor2_angle = pow(cos( x_position ), -1) / length;
CasperK 1:fc216448bb57 44 }
CasperK 1:fc216448bb57 45 }
CasperK 1:fc216448bb57 46
CasperK 3:56cbed6caacc 47 void yDirection () {
CasperK 3:56cbed6caacc 48 if (emg2Bool) {
CasperK 1:fc216448bb57 49 //direction of the motion
CasperK 1:fc216448bb57 50 if (y_direction) {
CasperK 1:fc216448bb57 51 directionpin2 = true;
CasperK 2:ffd0553701d3 52 direction = 1;
CasperK 1:fc216448bb57 53 }
CasperK 1:fc216448bb57 54 else if (!y_direction) {
CasperK 1:fc216448bb57 55 directionpin2 = false;
CasperK 2:ffd0553701d3 56 direction = -1;
CasperK 1:fc216448bb57 57 }
CasperK 2:ffd0553701d3 58
CasperK 2:ffd0553701d3 59 //calculating the motion
CasperK 2:ffd0553701d3 60 y_position = y_position + direction;
CasperK 2:ffd0553701d3 61 motor2_angle = C2 * y_position;
CasperK 1:fc216448bb57 62 }
CasperK 1:fc216448bb57 63 }
CasperK 1:fc216448bb57 64
CasperK 0:dc2c63f663f8 65 int main() {
CasperK 0:dc2c63f663f8 66 pc.printf(" ** program reset **\n\r");
CasperK 0:dc2c63f663f8 67 while (true) {
CasperK 3:56cbed6caacc 68 if (button) {
CasperK 3:56cbed6caacc 69 y_direction = !y_direction;
CasperK 3:56cbed6caacc 70 }
CasperK 3:56cbed6caacc 71
CasperK 3:56cbed6caacc 72 xDirection();
CasperK 3:56cbed6caacc 73 yDirection();
CasperK 0:dc2c63f663f8 74
CasperK 0:dc2c63f663f8 75 pc.printf("motor1 angle: %i\n\r", motor1_angle);
CasperK 0:dc2c63f663f8 76 pc.printf("motor2 angle: %i\n\r\n", motor2_angle);
CasperK 1:fc216448bb57 77
CasperK 1:fc216448bb57 78 wait(0.5f); //can also be done with ticker, to be sure that it happens exactly every 0.5 seconds
CasperK 0:dc2c63f663f8 79 }
CasperK 0:dc2c63f663f8 80 }