Met kinematics
Dependencies: MODSERIAL QEI mbed-dsp mbed
Fork of Motorcode by
main.cpp@10:d78f5d556cf7, 2018-10-31 (annotated)
- Committer:
- Jopper0575
- Date:
- Wed Oct 31 08:12:31 2018 +0000
- Revision:
- 10:d78f5d556cf7
- Parent:
- 9:466dff9ae128
Foward & Backward kinematics
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
vsluiter | 0:c8f15874531b | 1 | #include "mbed.h" |
vsluiter | 0:c8f15874531b | 2 | #include "MODSERIAL.h" |
SimonRez | 6:1ee8795b7578 | 3 | #include "QEI.h" |
rachieldb | 9:466dff9ae128 | 4 | #include "math.h" |
rachieldb | 9:466dff9ae128 | 5 | #define _USE_MATH_DEFINES |
rachieldb | 9:466dff9ae128 | 6 | # define M_PI 3.14159265358979323846 /* pi */ |
SimonRez | 2:52b3c0b95388 | 7 | |
SimonRez | 2:52b3c0b95388 | 8 | DigitalOut ledr(LED_RED); |
SimonRez | 2:52b3c0b95388 | 9 | DigitalOut ledg(LED_GREEN); |
SimonRez | 2:52b3c0b95388 | 10 | DigitalOut ledb(LED_BLUE); |
SimonRez | 4:651d06e860e7 | 11 | |
rachieldb | 9:466dff9ae128 | 12 | PwmOut motor1_pwm(D5); |
rachieldb | 9:466dff9ae128 | 13 | PwmOut motor2_pwm(D6); |
rachieldb | 9:466dff9ae128 | 14 | DigitalOut motor1_dir(D4); |
rachieldb | 9:466dff9ae128 | 15 | DigitalOut motor2_dir(D7); |
SimonRez | 6:1ee8795b7578 | 16 | AnalogIn pot1(A0); |
rachieldb | 9:466dff9ae128 | 17 | AnalogIn pot2(A1); |
SimonRez | 6:1ee8795b7578 | 18 | |
rachieldb | 9:466dff9ae128 | 19 | QEI motor1_enc(D12,D13,NC,8400,QEI::X4_ENCODING); |
rachieldb | 9:466dff9ae128 | 20 | QEI motor2_enc(D10,D11,NC,8400,QEI::X4_ENCODING); |
SimonRez | 5:f07bafaf11d7 | 21 | |
SimonRez | 4:651d06e860e7 | 22 | InterruptIn sw2(SW2); |
SimonRez | 4:651d06e860e7 | 23 | InterruptIn sw3(SW3); |
SimonRez | 2:52b3c0b95388 | 24 | |
vsluiter | 0:c8f15874531b | 25 | MODSERIAL pc(USBTX, USBRX); |
vsluiter | 0:c8f15874531b | 26 | |
rachieldb | 9:466dff9ae128 | 27 | Ticker sample_timer; |
rachieldb | 9:466dff9ae128 | 28 | |
rachieldb | 9:466dff9ae128 | 29 | const float sample_time = 0.05; //s |
rachieldb | 9:466dff9ae128 | 30 | const float sample_frequency = 200; //hz |
rachieldb | 9:466dff9ae128 | 31 | |
Jopper0575 | 10:d78f5d556cf7 | 32 | |
rachieldb | 9:466dff9ae128 | 33 | double countstoangle(int counts){ |
rachieldb | 9:466dff9ae128 | 34 | double angle; |
rachieldb | 9:466dff9ae128 | 35 | int counts_abs = abs(counts); |
rachieldb | 9:466dff9ae128 | 36 | if(counts_abs >= 8400){ |
rachieldb | 9:466dff9ae128 | 37 | int partial_counts = counts_abs % 8400; |
rachieldb | 9:466dff9ae128 | 38 | angle = (2 * M_PI * partial_counts)/8400; |
rachieldb | 9:466dff9ae128 | 39 | return angle; |
rachieldb | 9:466dff9ae128 | 40 | }else{ |
rachieldb | 9:466dff9ae128 | 41 | angle = (2 * M_PI * counts_abs)/8400; |
rachieldb | 9:466dff9ae128 | 42 | return angle; |
rachieldb | 9:466dff9ae128 | 43 | } |
rachieldb | 9:466dff9ae128 | 44 | } |
SimonRez | 4:651d06e860e7 | 45 | |
Jopper0575 | 10:d78f5d556cf7 | 46 | |
Jopper0575 | 10:d78f5d556cf7 | 47 | // KINEMATICS |
Jopper0575 | 10:d78f5d556cf7 | 48 | // Variables |
Jopper0575 | 10:d78f5d556cf7 | 49 | const int L1 = 300; // Length arm 1 in mm |
Jopper0575 | 10:d78f5d556cf7 | 50 | const int L2 = 482; // Length arm 2 in mm |
Jopper0575 | 10:d78f5d556cf7 | 51 | |
Jopper0575 | 10:d78f5d556cf7 | 52 | // Gets current postition of end effector | FOWARD KINEMATICS |
Jopper0575 | 10:d78f5d556cf7 | 53 | double getX1pos(double q1){ // X-pos of joint |
Jopper0575 | 10:d78f5d556cf7 | 54 | return x1_c = L1*cosd(q1); |
Jopper0575 | 10:d78f5d556cf7 | 55 | } |
Jopper0575 | 10:d78f5d556cf7 | 56 | double getY1pos(double q1){ // Y-pos of joint |
Jopper0575 | 10:d78f5d556cf7 | 57 | return y1_c = L1*sind(q1); |
Jopper0575 | 10:d78f5d556cf7 | 58 | } |
Jopper0575 | 10:d78f5d556cf7 | 59 | double getX2pos(double q1, double q2){ // X-pos of end-effector |
Jopper0575 | 10:d78f5d556cf7 | 60 | double x1_c = getX1pos(q1) |
Jopper0575 | 10:d78f5d556cf7 | 61 | return x2_c = x1_c + L2*cosd(q1+q2); |
Jopper0575 | 10:d78f5d556cf7 | 62 | } |
Jopper0575 | 10:d78f5d556cf7 | 63 | double getY2pos(double q1, double q2){ // Y-pos of end-effector |
Jopper0575 | 10:d78f5d556cf7 | 64 | double y1_c = getY1pos(q1) |
Jopper0575 | 10:d78f5d556cf7 | 65 | return y2_c = y1_c + L2*sind(q1+q2); |
Jopper0575 | 10:d78f5d556cf7 | 66 | } |
Jopper0575 | 10:d78f5d556cf7 | 67 | |
Jopper0575 | 10:d78f5d556cf7 | 68 | // Calculate new position | BACKWARD KINEMATICS |
Jopper0575 | 10:d78f5d556cf7 | 69 | double getNewq2(double x2_d, double y2_d){ // requires desired x and y position of end-effector |
Jopper0575 | 10:d78f5d556cf7 | 70 | double frac = ( pow(x2_d,2) + pow(y2_d,2) - pow(L1,2) - pow(L2,2) ) / ( 2 * L1 * L2 ); // define kinematic cosine fraction for easier calculation |
Jopper0575 | 10:d78f5d556cf7 | 71 | double q2_d = atan2d( -sqrt(1 - pow(frac,2)) , frac ); // new desired q2 joint angle |
Jopper0575 | 10:d78f5d556cf7 | 72 | return q2_d; |
Jopper0575 | 10:d78f5d556cf7 | 73 | } |
Jopper0575 | 10:d78f5d556cf7 | 74 | double getNewq1(double x2_d, double y2_d){ // requires desired x and y position of end-effector |
Jopper0575 | 10:d78f5d556cf7 | 75 | double q2_d = getNewq2(x2_d, y2_d); // first calculate q2 |
Jopper0575 | 10:d78f5d556cf7 | 76 | double k1 = L1 + L2*cosd(q2_d); // kinematic relation 1 |
Jopper0575 | 10:d78f5d556cf7 | 77 | double k2 = L2*sind(q2_d); // kinematic relation 2 |
Jopper0575 | 10:d78f5d556cf7 | 78 | double q1_d = atan2d( y2_d, x2_d ) - atan2d( k2, k1); // new desired q1 joint angle |
Jopper0575 | 10:d78f5d556cf7 | 79 | return q1_d; |
Jopper0575 | 10:d78f5d556cf7 | 80 | } |
Jopper0575 | 10:d78f5d556cf7 | 81 | double getNewq3(double x2_d, double y2_d){ // Cup holder |
Jopper0575 | 10:d78f5d556cf7 | 82 | double q3_d = getNewq2(x2_d, y2_d) + getNewq1(x2_d, y2_d); // q2_d+q1_d |
Jopper0575 | 10:d78f5d556cf7 | 83 | } |
Jopper0575 | 10:d78f5d556cf7 | 84 | |
Jopper0575 | 10:d78f5d556cf7 | 85 | |
Jopper0575 | 10:d78f5d556cf7 | 86 | |
Jopper0575 | 10:d78f5d556cf7 | 87 | |
Jopper0575 | 10:d78f5d556cf7 | 88 | |
Jopper0575 | 10:d78f5d556cf7 | 89 | |
vsluiter | 0:c8f15874531b | 90 | int main() |
vsluiter | 0:c8f15874531b | 91 | { |
SimonRez | 2:52b3c0b95388 | 92 | ledr = 1; |
SimonRez | 2:52b3c0b95388 | 93 | ledg = 1; |
SimonRez | 2:52b3c0b95388 | 94 | ledb = 1; |
SimonRez | 2:52b3c0b95388 | 95 | |
SimonRez | 4:651d06e860e7 | 96 | pc.printf("\r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n ~~~A$$De$troyer69~~~ \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n"); |
SimonRez | 2:52b3c0b95388 | 97 | |
rachieldb | 9:466dff9ae128 | 98 | motor1_pwm.period_us(60); |
rachieldb | 9:466dff9ae128 | 99 | motor1_pwm = 0; |
rachieldb | 9:466dff9ae128 | 100 | motor2_pwm.period_us(60); |
rachieldb | 9:466dff9ae128 | 101 | motor2_pwm = 0; |
SimonRez | 5:f07bafaf11d7 | 102 | |
SimonRez | 2:52b3c0b95388 | 103 | while (true) |
SimonRez | 2:52b3c0b95388 | 104 | { |
rachieldb | 8:9b517db94f49 | 105 | float pot1_float = pot1.read(); |
rachieldb | 8:9b517db94f49 | 106 | float pot1_motor = (pot1_float * 2.0f) - 1.0f; |
rachieldb | 8:9b517db94f49 | 107 | int mot1_direction = pot1_motor >= 0; |
rachieldb | 9:466dff9ae128 | 108 | motor1_pwm.write(fabs(pot1_motor)); |
rachieldb | 9:466dff9ae128 | 109 | motor1_dir = !mot1_direction; |
rachieldb | 9:466dff9ae128 | 110 | |
rachieldb | 9:466dff9ae128 | 111 | float pot2_float = pot2.read(); |
rachieldb | 9:466dff9ae128 | 112 | float pot2_motor = (pot2_float * 2.0f) - 1.0f; |
rachieldb | 9:466dff9ae128 | 113 | int mot2_direction = pot2_motor >= 0; |
rachieldb | 9:466dff9ae128 | 114 | motor2_pwm.write(fabs(pot2_motor)); |
rachieldb | 9:466dff9ae128 | 115 | motor2_dir = mot2_direction; |
rachieldb | 9:466dff9ae128 | 116 | |
rachieldb | 9:466dff9ae128 | 117 | double motor1_ang = countstoangle(motor1_enc.getPulses()); |
rachieldb | 9:466dff9ae128 | 118 | double motor2_ang = countstoangle(motor2_enc.getPulses()); |
rachieldb | 9:466dff9ae128 | 119 | |
rachieldb | 9:466dff9ae128 | 120 | pc.printf("Angle motor 1: %f pi rad, angle motor 2: %f pi rad.\r\n", motor1_ang/(M_PI), motor2_ang/(M_PI)); |
rachieldb | 9:466dff9ae128 | 121 | |
SimonRez | 5:f07bafaf11d7 | 122 | wait(0.2f); |
vsluiter | 0:c8f15874531b | 123 | } |
vsluiter | 0:c8f15874531b | 124 | } |
SimonRez | 2:52b3c0b95388 | 125 | |
SimonRez | 2:52b3c0b95388 | 126 |