Group 9 BioRobotics / Mbed 2 deprecated newRKI

RKI

Dependencies:   QEI mbed

main.cpp@0:7789750c3b36, 2018-10-31 (annotated)

Committer:
kweisbeek
Date:
Wed Oct 31 10:40:46 2018 +0000
Revision:
0:7789750c3b36
Child:
1:125af627e307
newRKI;

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 0:7789750c3b36 12 DigitalOut dirpin(D4);
kweisbeek 0:7789750c3b36 13 DigitalOut dirpin2(D7);
kweisbeek 0:7789750c3b36 14 PwmOut pwmpin(D5);
kweisbeek 0:7789750c3b36 15 PwmOut pwmpin2(D6);
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 0:7789750c3b36 20 double delta_t=0.01;
kweisbeek 0:7789750c3b36 21 double alpha=360/(25*8400);
kweisbeek 0:7789750c3b36 22 double L1=370/2;
kweisbeek 0:7789750c3b36 23 double L2=65/2;
kweisbeek 0:7789750c3b36 24 double pi=3.14159265359;
kweisbeek 0:7789750c3b36 25 double beta=(((2*L1)-(2*L2))*20*pi)/(305*8400);
kweisbeek 0:7789750c3b36 26
kweisbeek 0:7789750c3b36 27 //functions
kweisbeek 0:7789750c3b36 28 double out1(){
kweisbeek 0:7789750c3b36 29 float a=(pot1*2.0f)-1.0f;
kweisbeek 0:7789750c3b36 30 return a;}
kweisbeek 0:7789750c3b36 31 double out2(){
kweisbeek 0:7789750c3b36 32 double a=(pot2*2.0f)-1.0f;
kweisbeek 0:7789750c3b36 33 return a;}
kweisbeek 0:7789750c3b36 34 int counts1(){
kweisbeek 0:7789750c3b36 35 int a=encoder1.getPulses();
kweisbeek 0:7789750c3b36 36 return a;}
kweisbeek 0:7789750c3b36 37 int counts2(){
kweisbeek 0:7789750c3b36 38 int a=encoder2.getPulses();
kweisbeek 0:7789750c3b36 39 return a;}
kweisbeek 0:7789750c3b36 40 double vdesx(){
kweisbeek 0:7789750c3b36 41 double a=out1()*20;
kweisbeek 0:7789750c3b36 42 return a;}
kweisbeek 0:7789750c3b36 43 double vdesy(){
kweisbeek 0:7789750c3b36 44 double a=out1()*20;
kweisbeek 0:7789750c3b36 45 return a;}
kweisbeek 0:7789750c3b36 46 double q1(){
kweisbeek 0:7789750c3b36 47 double a=counts1()*alpha;
kweisbeek 0:7789750c3b36 48 return a;}
kweisbeek 0:7789750c3b36 49 double q2(){
kweisbeek 0:7789750c3b36 50 double a=counts2()*beta;
kweisbeek 0:7789750c3b36 51 return a;}
kweisbeek 0:7789750c3b36 52 double MPe(){
kweisbeek 0:7789750c3b36 53 double a=L1-L2+q2();
kweisbeek 0:7789750c3b36 54 return a;}
kweisbeek 0:7789750c3b36 55 double dq1(){
kweisbeek 0:7789750c3b36 56 double a=((1/sin(q1()))*vdesx()-(1/cos(q1()))*vdesy())*0.5*delta_t*MPe();
kweisbeek 0:7789750c3b36 57 return a;}
kweisbeek 0:7789750c3b36 58 double dq2(){
kweisbeek 0:7789750c3b36 59 double a=((1/sin(q1()))*vdesx()+(1/cos(q1()))*vdesy())*0.5*delta_t*MPe();
kweisbeek 0:7789750c3b36 60 return a;}
kweisbeek 0:7789750c3b36 61 int dC1(){
kweisbeek 0:7789750c3b36 62 int a= dq1()/alpha;
kweisbeek 0:7789750c3b36 63 return a;}
kweisbeek 0:7789750c3b36 64 int dC2(){
kweisbeek 0:7789750c3b36 65 int a= dq2()/beta;
kweisbeek 0:7789750c3b36 66 return a;}
kweisbeek 0:7789750c3b36 67 float pwm1(){
kweisbeek 0:7789750c3b36 68 float a=dC1()/delta_t;
kweisbeek 0:7789750c3b36 69 float b=a/8400;
kweisbeek 0:7789750c3b36 70 return b;}
kweisbeek 0:7789750c3b36 71 float pwm2(){
kweisbeek 0:7789750c3b36 72 float a=dC2()/delta_t;
kweisbeek 0:7789750c3b36 73 float b=a/8400;
kweisbeek 0:7789750c3b36 74 return b;}
kweisbeek 0:7789750c3b36 75 void start(){
kweisbeek 0:7789750c3b36 76 dirpin.write(pwm1() < 0);
kweisbeek 0:7789750c3b36 77 pwmpin = fabs (pwm1());
kweisbeek 0:7789750c3b36 78 dirpin2.write(pwm2() < 0);
kweisbeek 0:7789750c3b36 79 pwmpin2 = fabs (pwm2());}
kweisbeek 0:7789750c3b36 80
kweisbeek 0:7789750c3b36 81 //main
kweisbeek 0:7789750c3b36 82 int main(){
kweisbeek 0:7789750c3b36 83 pc.baud(115200);
kweisbeek 0:7789750c3b36 84 pc.printf("start\r\n");
kweisbeek 0:7789750c3b36 85 pwmpin.period_us(60);
kweisbeek 0:7789750c3b36 86
kweisbeek 0:7789750c3b36 87 while(1){
kweisbeek 0:7789750c3b36 88 start();}
kweisbeek 0:7789750c3b36 89
kweisbeek 0:7789750c3b36 90 }
kweisbeek 0:7789750c3b36 91
kweisbeek 0:7789750c3b36 92
kweisbeek 0:7789750c3b36 93
kweisbeek 0:7789750c3b36 94
kweisbeek 0:7789750c3b36 95