Group 9 BioRobotics / Mbed 2 deprecated newRKI

RKI

Dependencies:   QEI mbed

main.cpp@2:d41aa6d0e3cc, 2018-10-31 (annotated)

Committer:
kweisbeek
Date:
Wed Oct 31 15:06:59 2018 +0000
Revision:
2:d41aa6d0e3cc
Parent:
1:125af627e307
Child:
3:a07e8f090946
voor 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 2:d41aa6d0e3cc 12 DigitalOut dirpin(D7);
kweisbeek 2:d41aa6d0e3cc 13 DigitalOut dirpin2(D4);
kweisbeek 2:d41aa6d0e3cc 14 PwmOut pwmpin(D6);
kweisbeek 2:d41aa6d0e3cc 15 PwmOut pwmpin2(D5);
kweisbeek 0:7789750c3b36 16 QEI encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
kweisbeek 0:7789750c3b36 17 QEI encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
kweisbeek 0:7789750c3b36 18
kweisbeek 0:7789750c3b36 19 //parameters
kweisbeek 2:d41aa6d0e3cc 20 const double delta_t=0.01;
kweisbeek 2:d41aa6d0e3cc 21 const double alpha=360.0/(25.0*8400.0);
kweisbeek 2:d41aa6d0e3cc 22 const double L1=370.0/2.0;
kweisbeek 2:d41aa6d0e3cc 23 const double L2=65.0/2.0;
kweisbeek 2:d41aa6d0e3cc 24 const double pi=3.14159265359;
kweisbeek 2:d41aa6d0e3cc 25 const double beta=(((2.0*L1)-(2.0*L2))*20.0*pi)/(305.0*8400.0);
kweisbeek 0:7789750c3b36 26
kweisbeek 2:d41aa6d0e3cc 27
kweisbeek 2:d41aa6d0e3cc 28 //functions
kweisbeek 0:7789750c3b36 29 double out1(){
kweisbeek 0:7789750c3b36 30 float a=(pot1*2.0f)-1.0f;
kweisbeek 0:7789750c3b36 31 return a;}
kweisbeek 0:7789750c3b36 32 double out2(){
kweisbeek 0:7789750c3b36 33 double a=(pot2*2.0f)-1.0f;
kweisbeek 0:7789750c3b36 34 return a;}
kweisbeek 0:7789750c3b36 35 int counts1(){
kweisbeek 0:7789750c3b36 36 int a=encoder1.getPulses();
kweisbeek 0:7789750c3b36 37 return a;}
kweisbeek 0:7789750c3b36 38 int counts2(){
kweisbeek 0:7789750c3b36 39 int a=encoder2.getPulses();
kweisbeek 0:7789750c3b36 40 return a;}
kweisbeek 0:7789750c3b36 41 double vdesx(){
kweisbeek 2:d41aa6d0e3cc 42 double a=out1()*20.0;
kweisbeek 0:7789750c3b36 43 return a;}
kweisbeek 0:7789750c3b36 44 double vdesy(){
kweisbeek 2:d41aa6d0e3cc 45 double a=out2()*20.0;
kweisbeek 0:7789750c3b36 46 return a;}
kweisbeek 0:7789750c3b36 47 double q1(){
kweisbeek 0:7789750c3b36 48 double a=counts1()*alpha;
kweisbeek 0:7789750c3b36 49 return a;}
kweisbeek 0:7789750c3b36 50 double q2(){
kweisbeek 0:7789750c3b36 51 double a=counts2()*beta;
kweisbeek 0:7789750c3b36 52 return a;}
kweisbeek 0:7789750c3b36 53 double MPe(){
kweisbeek 0:7789750c3b36 54 double a=L1-L2+q2();
kweisbeek 0:7789750c3b36 55 return a;}
kweisbeek 2:d41aa6d0e3cc 56 double xe(){
kweisbeek 2:d41aa6d0e3cc 57 double a=cos(q1())*MPe();
kweisbeek 2:d41aa6d0e3cc 58 return a;}
kweisbeek 2:d41aa6d0e3cc 59 double ye(){
kweisbeek 2:d41aa6d0e3cc 60 double a=sin(q1())*MPe();
kweisbeek 0:7789750c3b36 61 return a;}
kweisbeek 2:d41aa6d0e3cc 62 double gamma(){
kweisbeek 2:d41aa6d0e3cc 63 double a=1.0/((-1.0*ye()*sin(q1()))-(xe()*cos(q1())));
kweisbeek 2:d41aa6d0e3cc 64 return a;}
kweisbeek 2:d41aa6d0e3cc 65 double dq1(){
kweisbeek 2:d41aa6d0e3cc 66 double a=gamma()*delta_t*(sin(q1())*vdesx()-cos(q1())*vdesy());
kweisbeek 0:7789750c3b36 67 return a;}
kweisbeek 2:d41aa6d0e3cc 68 double dq2(){
kweisbeek 2:d41aa6d0e3cc 69 double a=gamma()*delta_t*(-1.0*xe()*vdesx()-ye()*vdesy());
kweisbeek 0:7789750c3b36 70 return a;}
kweisbeek 2:d41aa6d0e3cc 71 double dC1(){
kweisbeek 2:d41aa6d0e3cc 72 double a= dq1()/alpha;
kweisbeek 2:d41aa6d0e3cc 73 return a;}
kweisbeek 2:d41aa6d0e3cc 74 double dC2(){
kweisbeek 2:d41aa6d0e3cc 75 double a= dq2()/beta;
kweisbeek 0:7789750c3b36 76 return a;}
kweisbeek 2:d41aa6d0e3cc 77 double pwm1(){
kweisbeek 2:d41aa6d0e3cc 78 double a=dC1()/delta_t;
kweisbeek 2:d41aa6d0e3cc 79 double b=a/8400.0;
kweisbeek 2:d41aa6d0e3cc 80 return b*5.0;}
kweisbeek 2:d41aa6d0e3cc 81 double pwm2(){
kweisbeek 2:d41aa6d0e3cc 82 double a=dC2()/delta_t;
kweisbeek 2:d41aa6d0e3cc 83 double b=a/8400.0;
kweisbeek 2:d41aa6d0e3cc 84 return b*10.0;}
kweisbeek 0:7789750c3b36 85 void start(){
kweisbeek 0:7789750c3b36 86 dirpin.write(pwm1() < 0);
kweisbeek 0:7789750c3b36 87 pwmpin = fabs (pwm1());
kweisbeek 0:7789750c3b36 88 dirpin2.write(pwm2() < 0);
kweisbeek 0:7789750c3b36 89 pwmpin2 = fabs (pwm2());}
kweisbeek 0:7789750c3b36 90
kweisbeek 0:7789750c3b36 91 //main
kweisbeek 0:7789750c3b36 92 int main(){
kweisbeek 0:7789750c3b36 93 pc.baud(115200);
kweisbeek 0:7789750c3b36 94 pc.printf("start\r\n");
kweisbeek 0:7789750c3b36 95 pwmpin.period_us(60);
kweisbeek 0:7789750c3b36 96
kweisbeek 0:7789750c3b36 97 while(1){
s1725696 1:125af627e307 98 start();
kweisbeek 2:d41aa6d0e3cc 99 pc.printf("1=%f 2=%f \n\r",pwm1(),pwm2());
s1725696 1:125af627e307 100 wait(delta_t);
s1725696 1:125af627e307 101 }
kweisbeek 0:7789750c3b36 102 }
kweisbeek 0:7789750c3b36 103
kweisbeek 0:7789750c3b36 104
kweisbeek 0:7789750c3b36 105
kweisbeek 0:7789750c3b36 106
kweisbeek 0:7789750c3b36 107