Group 9 BioRobotics
/
newRKI
RKI
main.cpp
- Committer:
- kweisbeek
- Date:
- 2018-10-31
- Revision:
- 0:7789750c3b36
- Child:
- 1:125af627e307
File content as of revision 0:7789750c3b36:
#include "mbed.h" #include "QEI.h" #define SERIAL_BAUD 115200 //Initial allocations Serial pc(USBTX,USBRX); AnalogIn pot1(A1); AnalogIn pot2(A2); DigitalOut dirpin(D4); DigitalOut dirpin2(D7); PwmOut pwmpin(D5); PwmOut pwmpin2(D6); QEI encoder1(D12,D13,NC,64,QEI::X4_ENCODING); QEI encoder2(D10,D11,NC,64,QEI::X4_ENCODING); //parameters double delta_t=0.01; double alpha=360/(25*8400); double L1=370/2; double L2=65/2; double pi=3.14159265359; double beta=(((2*L1)-(2*L2))*20*pi)/(305*8400); //functions double out1(){ float a=(pot1*2.0f)-1.0f; return a;} double out2(){ double a=(pot2*2.0f)-1.0f; return a;} int counts1(){ int a=encoder1.getPulses(); return a;} int counts2(){ int a=encoder2.getPulses(); return a;} double vdesx(){ double a=out1()*20; return a;} double vdesy(){ double a=out1()*20; return a;} double q1(){ double a=counts1()*alpha; return a;} double q2(){ double a=counts2()*beta; return a;} double MPe(){ double a=L1-L2+q2(); return a;} double dq1(){ double a=((1/sin(q1()))*vdesx()-(1/cos(q1()))*vdesy())*0.5*delta_t*MPe(); return a;} double dq2(){ double a=((1/sin(q1()))*vdesx()+(1/cos(q1()))*vdesy())*0.5*delta_t*MPe(); return a;} int dC1(){ int a= dq1()/alpha; return a;} int dC2(){ int a= dq2()/beta; return a;} float pwm1(){ float a=dC1()/delta_t; float b=a/8400; return b;} float pwm2(){ float a=dC2()/delta_t; float b=a/8400; return b;} void start(){ dirpin.write(pwm1() < 0); pwmpin = fabs (pwm1()); dirpin2.write(pwm2() < 0); pwmpin2 = fabs (pwm2());} //main int main(){ pc.baud(115200); pc.printf("start\r\n"); pwmpin.period_us(60); while(1){ start();} }