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();}
+        
+}
+
+
+
+
+