Casper Kroon / Mbed 2 deprecated Inverse_kinematics

The motor angles/kinematics and inverse kinematics

Dependencies:   MODSERIAL mbed

main.cpp@10:8b0d8d4016a1, 2018-10-25 (annotated)

Committer:
CasperK
Date:
Thu Oct 25 14:44:32 2018 +0000
Revision:
10:8b0d8d4016a1
Parent:
6:0162a633768d 
Corrections in the x and y position and angle 1 and 2 calculations

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