group14 - k9 / Mbed 2 deprecated InverseKinematics_test

Blinky brinky

Dependencies:   MODSERIAL mbed

main.cpp@0:80b7dec1c50e, 2015-10-23 (annotated)

Committer:
Elsje3006
Date:
Fri Oct 23 07:56:18 2015 +0000
Revision:
0:80b7dec1c50e
Child:
1:8eb1c39a91be
Blinky brinky

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Elsje3006 0:80b7dec1c50e 1 #include "mbed.h"
Elsje3006 0:80b7dec1c50e 2 #include "math.h"
Elsje3006 0:80b7dec1c50e 3 #include "MODSERIAL.h"
Elsje3006 0:80b7dec1c50e 4 #include <string>
Elsje3006 0:80b7dec1c50e 5
Elsje3006 0:80b7dec1c50e 6 //Define important constants in memory
Elsje3006 0:80b7dec1c50e 7 #define PI 3.14159265
Elsje3006 0:80b7dec1c50e 8 #define SAMPLE_RATE 0.002 //500 Hz EMG sample rate
Elsje3006 0:80b7dec1c50e 9 #define CONTROL_RATE 0.01 //100 Hz Control rate
Elsje3006 0:80b7dec1c50e 10 #define ENCODER1_CPR 4200 //encoders have 64 (X4), 32 (X2) counts per revolution of motor shaft
Elsje3006 0:80b7dec1c50e 11 #define ENCODER2_CPR 4200 //gearbox 1:131.25 -> 4200 counts per revolution of the output shaft (X2),
Elsje3006 0:80b7dec1c50e 12 #define PWM_PERIOD 0.0001 //10k Hz pwm motor frequency. Higher -> too hot, lower -> motor doesnt respond correctly
Elsje3006 0:80b7dec1c50e 13
Elsje3006 0:80b7dec1c50e 14 DigitalIn button(PTA4);
Elsje3006 0:80b7dec1c50e 15 MODSERIAL pc(USBTX,USBRX); //serial communication
Elsje3006 0:80b7dec1c50e 16
Elsje3006 0:80b7dec1c50e 17
Elsje3006 0:80b7dec1c50e 18 Ticker signal_emg; //Ticker for signal simulation
Elsje3006 0:80b7dec1c50e 19
Elsje3006 0:80b7dec1c50e 20 double emg_1 = 0.1;
Elsje3006 0:80b7dec1c50e 21 double emg_2 = 0.05;
Elsje3006 0:80b7dec1c50e 22 double l1 = 0.25;
Elsje3006 0:80b7dec1c50e 23 double l2 = 0.25;
Elsje3006 0:80b7dec1c50e 24 double x = 30;
Elsje3006 0:80b7dec1c50e 25 double y = 20;
Elsje3006 0:80b7dec1c50e 26 double rpc;
Elsje3006 0:80b7dec1c50e 27 double theta1;
Elsje3006 0:80b7dec1c50e 28 double theta2;
Elsje3006 0:80b7dec1c50e 29 double current_x;
Elsje3006 0:80b7dec1c50e 30 double current_y;
Elsje3006 0:80b7dec1c50e 31 double x_error;
Elsje3006 0:80b7dec1c50e 32 double y_error;
Elsje3006 0:80b7dec1c50e 33 double costheta2;
Elsje3006 0:80b7dec1c50e 34 double sintheta2;
Elsje3006 0:80b7dec1c50e 35 double dtheta1;
Elsje3006 0:80b7dec1c50e 36 double dtheta2;
Elsje3006 0:80b7dec1c50e 37 double m1_error;
Elsje3006 0:80b7dec1c50e 38 double m2_error;
Elsje3006 0:80b7dec1c50e 39
Elsje3006 0:80b7dec1c50e 40 void take_sample()
Elsje3006 0:80b7dec1c50e 41 {
Elsje3006 0:80b7dec1c50e 42 double emg_1 = emg_1 + 0.1;
Elsje3006 0:80b7dec1c50e 43 double emg_2 = emg_2 + 0.05;
Elsje3006 0:80b7dec1c50e 44 }
Elsje3006 0:80b7dec1c50e 45
Elsje3006 0:80b7dec1c50e 46 //Current position - Encoder counts -> current angle -> Forward Kinematics
Elsje3006 0:80b7dec1c50e 47 int main()
Elsje3006 0:80b7dec1c50e 48 {
Elsje3006 0:80b7dec1c50e 49 pc.baud(115200); //serial baudrate
Elsje3006 0:80b7dec1c50e 50 if (button.read() == 1)
Elsje3006 0:80b7dec1c50e 51 {
Elsje3006 0:80b7dec1c50e 52 while(emg_1<10){
Elsje3006 0:80b7dec1c50e 53 signal_emg.attach(take_sample,1/500);
Elsje3006 0:80b7dec1c50e 54
Elsje3006 0:80b7dec1c50e 55 rpc=(2*PI)/ENCODER1_CPR; //radians per count (resolution) - 2pi divided by 4200
Elsje3006 0:80b7dec1c50e 56 theta1 = emg_1 * rpc; //multiply resolution with number of counts
Elsje3006 0:80b7dec1c50e 57 theta2 = emg_2 * rpc;
Elsje3006 0:80b7dec1c50e 58 current_x = l1 * cos(theta1) + l2 * cos(theta1 + theta2);
Elsje3006 0:80b7dec1c50e 59 current_y = l1 * sin(theta1) + l2 * sin(theta1 + theta2);
Elsje3006 0:80b7dec1c50e 60
Elsje3006 0:80b7dec1c50e 61 pc.printf("Theta1 = %f, theta2 = %f \r\n",theta1,theta2);
Elsje3006 0:80b7dec1c50e 62
Elsje3006 0:80b7dec1c50e 63 //calculate error (refpos-currentpos) currentpos = forward kinematics
Elsje3006 0:80b7dec1c50e 64 x_error = x - current_x;
Elsje3006 0:80b7dec1c50e 65 y_error = y - current_y;
Elsje3006 0:80b7dec1c50e 66
Elsje3006 0:80b7dec1c50e 67
Elsje3006 0:80b7dec1c50e 68 //inverse kinematics (refpos to refangle)
Elsje3006 0:80b7dec1c50e 69
Elsje3006 0:80b7dec1c50e 70 costheta2 = (pow(x,2) + pow(y,2) - pow(l1,2) - pow(l2,2)) / (2*l1*l2) ;
Elsje3006 0:80b7dec1c50e 71 sintheta2 = sqrt( abs( 1 - pow(costheta2,2) ) );
Elsje3006 0:80b7dec1c50e 72
Elsje3006 0:80b7dec1c50e 73
Elsje3006 0:80b7dec1c50e 74 dtheta2 = atan2(sintheta2,costheta2);
Elsje3006 0:80b7dec1c50e 75
Elsje3006 0:80b7dec1c50e 76 double k1 = l1 + l2*costheta2;
Elsje3006 0:80b7dec1c50e 77 double k2 = l2*sintheta2;
Elsje3006 0:80b7dec1c50e 78
Elsje3006 0:80b7dec1c50e 79 dtheta1 = atan2(y, x) - atan2(k2, k1);
Elsje3006 0:80b7dec1c50e 80
Elsje3006 0:80b7dec1c50e 81 /* alternative:
Elsje3006 0:80b7dec1c50e 82 costheta1 = ( x * (l1 + l2 * costheta2) + y * l2 * sintheta2 ) / ( pow(x,2) + pow(y,2) );
Elsje3006 0:80b7dec1c50e 83 sintheta1 = sqrt( abs( 1 - pow(costheta1,2) ) );
Elsje3006 0:80b7dec1c50e 84
Elsje3006 0:80b7dec1c50e 85 dtheta1 = atan2(sintheta1,costheta1);
Elsje3006 0:80b7dec1c50e 86 */
Elsje3006 0:80b7dec1c50e 87
Elsje3006 0:80b7dec1c50e 88 //Angle error
Elsje3006 0:80b7dec1c50e 89 m1_error = dtheta1-theta1;
Elsje3006 0:80b7dec1c50e 90 m2_error = dtheta2-theta2;
Elsje3006 0:80b7dec1c50e 91
Elsje3006 0:80b7dec1c50e 92 pc.printf("Motor 1 error is %f, motor 2 error is %f /r/n",m1_error,m2_error);
Elsje3006 0:80b7dec1c50e 93 }
Elsje3006 0:80b7dec1c50e 94 }
Elsje3006 0:80b7dec1c50e 95 }