Met kinematics

Dependencies:   MODSERIAL QEI mbed-dsp mbed

Fork of Motorcode by ProjectGroep23

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?

UserRevisionLine numberNew 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