Group 9 BioRobotics / Mbed 2 deprecated newRKI

RKI

Dependencies:   QEI mbed

main.cpp@3:a07e8f090946, 2018-10-31 (annotated)

Committer:
kweisbeek
Date:
Wed Oct 31 16:07:04 2018 +0000
Revision:
3:a07e8f090946
Parent:
2:d41aa6d0e3cc
Child:
4:d568f61be1d1
nadat simon kwam

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kweisbeek 0:7789750c3b36 1 #include "mbed.h"
kweisbeek 0:7789750c3b36 2 #include "QEI.h"
kweisbeek 0:7789750c3b36 3
kweisbeek 0:7789750c3b36 4 #define SERIAL_BAUD 115200
kweisbeek 0:7789750c3b36 5
kweisbeek 0:7789750c3b36 6 //Initial allocations
kweisbeek 0:7789750c3b36 7 Serial pc(USBTX,USBRX);
kweisbeek 0:7789750c3b36 8
kweisbeek 0:7789750c3b36 9 AnalogIn pot1(A1);
kweisbeek 0:7789750c3b36 10 AnalogIn pot2(A2);
kweisbeek 0:7789750c3b36 11
kweisbeek 3:a07e8f090946 12 DigitalOut dirpin(D7); //Motor 1 (Rotatie)
kweisbeek 2:d41aa6d0e3cc 13 PwmOut pwmpin(D6);
kweisbeek 3:a07e8f090946 14
kweisbeek 3:a07e8f090946 15 DigitalOut dirpin2(D4); //Motor 2 (Translatie)
kweisbeek 2:d41aa6d0e3cc 16 PwmOut pwmpin2(D5);
kweisbeek 3:a07e8f090946 17
kweisbeek 3:a07e8f090946 18 QEI encoder1(D12,D13,NC,64,QEI::X4_ENCODING); //Encoder motor 1
kweisbeek 3:a07e8f090946 19 QEI encoder2(D10,D11,NC,64,QEI::X4_ENCODING); // Encoder motor 2
kweisbeek 0:7789750c3b36 20
kweisbeek 0:7789750c3b36 21 //parameters
kweisbeek 3:a07e8f090946 22 const double delta_t = 0.01;
kweisbeek 3:a07e8f090946 23 const double L1 = 370.0 / 2.0;
kweisbeek 3:a07e8f090946 24 const double L2 = 65.0/2.0;
kweisbeek 3:a07e8f090946 25 const double pi = 3.14159265359;
kweisbeek 3:a07e8f090946 26 const double alpha = (2.0 * pi) /(25.0*8400.0);
kweisbeek 3:a07e8f090946 27 const double beta = (((2.0 * L1) - (2.0 * L2)) * 20.0 * pi) / (305.0 * 8400.0);
kweisbeek 0:7789750c3b36 28
kweisbeek 3:a07e8f090946 29 //variables motors
kweisbeek 3:a07e8f090946 30 double out1;
kweisbeek 3:a07e8f090946 31 double out2;
kweisbeek 3:a07e8f090946 32 int counts1;
kweisbeek 3:a07e8f090946 33 int counts2;
kweisbeek 3:a07e8f090946 34 double vdesx;
kweisbeek 3:a07e8f090946 35 double vdesy;
kweisbeek 3:a07e8f090946 36 double q1;
kweisbeek 3:a07e8f090946 37 double q2;
kweisbeek 3:a07e8f090946 38 double MPe;
kweisbeek 3:a07e8f090946 39 double xe;
kweisbeek 3:a07e8f090946 40 double ye;
kweisbeek 3:a07e8f090946 41 double gamma;
kweisbeek 3:a07e8f090946 42 double dq1;
kweisbeek 3:a07e8f090946 43 double dq2;
kweisbeek 3:a07e8f090946 44 double dC1;
kweisbeek 3:a07e8f090946 45 double dC2;
kweisbeek 3:a07e8f090946 46 double pwm1;
kweisbeek 3:a07e8f090946 47 double pwm2;
kweisbeek 2:d41aa6d0e3cc 48
kweisbeek 3:a07e8f090946 49 void motor_control()
kweisbeek 3:a07e8f090946 50 {
kweisbeek 3:a07e8f090946 51 out1 = (pot1*2.0f)-1.0f; //control x-direction
kweisbeek 3:a07e8f090946 52 out2 = (pot2*2.0f)-1.0f; //control y-direction
kweisbeek 3:a07e8f090946 53 counts1 = encoder1.getPulses(); //counts encoder 1
kweisbeek 3:a07e8f090946 54 counts2 = encoder2.getPulses(); //counts encoder 2
kweisbeek 3:a07e8f090946 55 vdesx = out1 * 20.0; //speed x-direction
kweisbeek 3:a07e8f090946 56 vdesy = out2 * 20.0; //speed y-direction
kweisbeek 3:a07e8f090946 57 q1 = counts1 * alpha; //counts to rotation (rad)
kweisbeek 3:a07e8f090946 58 q2 = counts2 * beta; //counts to translation (mm)
kweisbeek 3:a07e8f090946 59 MPe = L1 - L2 + q2; //x location end effector, x-axis along the translation
kweisbeek 3:a07e8f090946 60 xe = cos(q1) * MPe; //x location in frame 0
kweisbeek 3:a07e8f090946 61 ye = sin(q1) * MPe; //y location in frame 0
kweisbeek 3:a07e8f090946 62 gamma = 1.0 /((-1.0 * ye * sin(q1)) - (xe * cos(q1))); //(1 / det(J'')inverse)
kweisbeek 3:a07e8f090946 63 dq1 = gamma * delta_t * (sin(q1) * vdesx - cos(q1) * vdesy); //target rotation
kweisbeek 3:a07e8f090946 64 dq2 = gamma * delta_t * (-1.0 * xe * vdesx - ye * vdesy); //target translation
kweisbeek 3:a07e8f090946 65 dC1 = dq1 / alpha; //target rotation to counts
kweisbeek 3:a07e8f090946 66 dC2 = dq2 / beta; //target translation to counts
kweisbeek 3:a07e8f090946 67 pwm1 = 3 * (dC1 / delta_t) / 8400.0; //
kweisbeek 3:a07e8f090946 68 pwm2 = 3 * (dC2 / delta_t) / 8400.0;
kweisbeek 3:a07e8f090946 69 dirpin.write(pwm1 < 0);
kweisbeek 3:a07e8f090946 70 pwmpin = fabs (pwm1);
kweisbeek 3:a07e8f090946 71 dirpin2.write(pwm2 < 0);
kweisbeek 3:a07e8f090946 72 pwmpin2 = fabs (pwm2);
kweisbeek 3:a07e8f090946 73 }
kweisbeek 3:a07e8f090946 74
kweisbeek 0:7789750c3b36 75 //main
kweisbeek 0:7789750c3b36 76 int main(){
kweisbeek 0:7789750c3b36 77 pc.baud(115200);
kweisbeek 0:7789750c3b36 78 pc.printf("start\r\n");
kweisbeek 0:7789750c3b36 79 pwmpin.period_us(60);
kweisbeek 0:7789750c3b36 80
kweisbeek 0:7789750c3b36 81 while(1){
kweisbeek 3:a07e8f090946 82 motor_control();
kweisbeek 3:a07e8f090946 83 pc.printf("1=%f 2=%f \n\r",pwm1,pwm2);
s1725696 1:125af627e307 84 wait(delta_t);
s1725696 1:125af627e307 85 }
kweisbeek 0:7789750c3b36 86 }
kweisbeek 0:7789750c3b36 87
kweisbeek 0:7789750c3b36 88
kweisbeek 0:7789750c3b36 89
kweisbeek 0:7789750c3b36 90
kweisbeek 0:7789750c3b36 91