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@0:dc2c63f663f8, 2018-10-15 (annotated)

Committer:
CasperK
Date:
Mon Oct 15 13:28:55 2018 +0000
Revision:
0:dc2c63f663f8
Child:
1:fc216448bb57
First setup, all if statements, some mathematics functions still as comments

Who changed what in which revision?

UserRevisionLine numberNew contents of line
CasperK 0:dc2c63f663f8 1 #include "mbed.h"
CasperK 0:dc2c63f663f8 2 #include "MODSERIAL.h"
CasperK 0:dc2c63f663f8 3
CasperK 0:dc2c63f663f8 4 DigitalIn button(SW2);
CasperK 0:dc2c63f663f8 5 DigitalOut directionpin1(D7);
CasperK 0:dc2c63f663f8 6 DigitalOut directionpin2(D8);
CasperK 0:dc2c63f663f8 7 MODSERIAL pc(USBTX, USBRX);
CasperK 0:dc2c63f663f8 8
CasperK 0:dc2c63f663f8 9 volatile bool emg0;
CasperK 0:dc2c63f663f8 10 volatile bool emg1;
CasperK 0:dc2c63f663f8 11 volatile bool emg2;
CasperK 0:dc2c63f663f8 12 volatile bool y_direction;
CasperK 0:dc2c63f663f8 13 volatile int motor1_angle;
CasperK 0:dc2c63f663f8 14 volatile int motor2_angle;
CasperK 0:dc2c63f663f8 15
CasperK 0:dc2c63f663f8 16 int main() {
CasperK 0:dc2c63f663f8 17 pc.printf(" ** program reset **\n\r");
CasperK 0:dc2c63f663f8 18 while (true) {
CasperK 0:dc2c63f663f8 19
CasperK 0:dc2c63f663f8 20 //x direction
CasperK 0:dc2c63f663f8 21 if (emg0 || emg1){
CasperK 0:dc2c63f663f8 22 motor1_angle++;
CasperK 0:dc2c63f663f8 23 //cos function of motor1_angle
CasperK 0:dc2c63f663f8 24 motor2_angle++;
CasperK 0:dc2c63f663f8 25 //sin function of motor2_angle
CasperK 0:dc2c63f663f8 26 if (emg0 && !emg1) {
CasperK 0:dc2c63f663f8 27 directionpin1 = true;
CasperK 0:dc2c63f663f8 28 }
CasperK 0:dc2c63f663f8 29 else if (!emg0 && emg1) {
CasperK 0:dc2c63f663f8 30 directionpin1 = false;
CasperK 0:dc2c63f663f8 31 }
CasperK 0:dc2c63f663f8 32 }
CasperK 0:dc2c63f663f8 33
CasperK 0:dc2c63f663f8 34 //y direction
CasperK 0:dc2c63f663f8 35 if (emg2) {
CasperK 0:dc2c63f663f8 36 motor2_angle++;
CasperK 0:dc2c63f663f8 37 if (y_direction) {
CasperK 0:dc2c63f663f8 38 directionpin2 = true;
CasperK 0:dc2c63f663f8 39 }
CasperK 0:dc2c63f663f8 40 else if (!y_direction) {
CasperK 0:dc2c63f663f8 41 directionpin2 = false;
CasperK 0:dc2c63f663f8 42 }
CasperK 0:dc2c63f663f8 43 }
CasperK 0:dc2c63f663f8 44
CasperK 0:dc2c63f663f8 45 pc.printf("motor1 angle: %i\n\r", motor1_angle);
CasperK 0:dc2c63f663f8 46 pc.printf("motor2 angle: %i\n\r\n", motor2_angle);
CasperK 0:dc2c63f663f8 47 wait(0.5f);
CasperK 0:dc2c63f663f8 48 }
CasperK 0:dc2c63f663f8 49 }