Group 9 BioRobotics
/
newRKI
RKI
Diff: main.cpp
- Revision:
- 0:7789750c3b36
- Child:
- 1:125af627e307
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Oct 31 10:40:46 2018 +0000 @@ -0,0 +1,95 @@ +#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();} + +} + + + + +