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@1:fc216448bb57, 2018-10-17 (annotated)

Committer:
CasperK
Date:
Wed Oct 17 10:09:28 2018 +0000
Revision:
1:fc216448bb57
Parent:
0:dc2c63f663f8
Child:
2:ffd0553701d3
taking direction into account for angle

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 0:dc2c63f663f8 10 volatile bool emg0;
CasperK 0:dc2c63f663f8 11 volatile bool emg1;
CasperK 0:dc2c63f663f8 12 volatile bool emg2;
CasperK 0:dc2c63f663f8 13 volatile bool y_direction;
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 1:fc216448bb57 19 volatile double 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 1:fc216448bb57 25 void xDirection(double &motor1_angle, double &motor2_angle) {
CasperK 1:fc216448bb57 26 //direction of the motion
CasperK 1:fc216448bb57 27 if (emg0 && !emg1) {
CasperK 1:fc216448bb57 28 directionpin1 = true;
CasperK 1:fc216448bb57 29 directionpin2 = true;
CasperK 1:fc216448bb57 30 direction = 1;
CasperK 1:fc216448bb57 31 }
CasperK 1:fc216448bb57 32 else if (!emg0 && emg1) {
CasperK 1:fc216448bb57 33 directionpin1 = false;
CasperK 1:fc216448bb57 34 directionpin2 = false;
CasperK 1:fc216448bb57 35 direction = -1;
CasperK 1:fc216448bb57 36 }
CasperK 1:fc216448bb57 37
CasperK 1:fc216448bb57 38 if (emg0 || emg1){
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 1:fc216448bb57 47 void yDirection (int &motor2_angle) {
CasperK 1:fc216448bb57 48 if (emg2) {
CasperK 1:fc216448bb57 49 //calculating the motion
CasperK 1:fc216448bb57 50 y_position++;
CasperK 1:fc216448bb57 51 motor2_angle = C2 * y_position;
CasperK 1:fc216448bb57 52
CasperK 1:fc216448bb57 53 //direction of the motion
CasperK 1:fc216448bb57 54 if (y_direction) {
CasperK 1:fc216448bb57 55 directionpin2 = true;
CasperK 1:fc216448bb57 56 }
CasperK 1:fc216448bb57 57 else if (!y_direction) {
CasperK 1:fc216448bb57 58 directionpin2 = false;
CasperK 1:fc216448bb57 59 }
CasperK 1:fc216448bb57 60 }
CasperK 1:fc216448bb57 61 }
CasperK 1:fc216448bb57 62
CasperK 0:dc2c63f663f8 63 int main() {
CasperK 0:dc2c63f663f8 64 pc.printf(" ** program reset **\n\r");
CasperK 0:dc2c63f663f8 65 while (true) {
CasperK 1:fc216448bb57 66 xDirection(motor1_angle, motor2_angle);
CasperK 1:fc216448bb57 67 yDirection(motor2_angle);
CasperK 0:dc2c63f663f8 68
CasperK 0:dc2c63f663f8 69 pc.printf("motor1 angle: %i\n\r", motor1_angle);
CasperK 0:dc2c63f663f8 70 pc.printf("motor2 angle: %i\n\r\n", motor2_angle);
CasperK 1:fc216448bb57 71
CasperK 1:fc216448bb57 72 wait(0.5f); //can also be done with ticker, to be sure that it happens exactly every 0.5 seconds
CasperK 0:dc2c63f663f8 73 }
CasperK 0:dc2c63f663f8 74 }