Diff: main.cpp

Revision:

0:165e50fc913f

Child:

1:4f5a22371fff

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Apr 12 12:28:01 2016 +0000
@@ -0,0 +1,134 @@
+#include "mbed.h"
+
+#define ratio 0.3
+#define conratio (1.0-ratio)
+
+#define PI 3.141592
+#define qPI 0.785398
+
+DigitalOut leds[8]={PA_11,PB_12,PB_2,PB_1,PB_15,PB_14,PB_13,PC_4};
+I2C i2c(I2C_SDA,I2C_SCL);
+Serial FEP02(PC_6,PA_12);
+Serial pc(USBTX,USBRX);
+
+char data[5]={0};
+bool recFrag=0;
+const int addr[4]={0x10,0x23,0x56,0x76};
+
+void ledsReset(){
+    int i=0;
+    while(i<8){
+        leds[i]=0;
+        i++;
+    }
+}
+
+void getConData(){
+    int i=0;
+    if(FEP02.getc()==255){
+        recFrag=1;
+        while(i<5){
+            data[i]=FEP02.getc();
+            i++;
+        }
+    }
+    //pc.printf("Recieved\r\n");
+}
+
+void MotorOut(char MDFL[],char MDFR[],char MDRL[],char MDRR[]){
+    int dig;
+    double rad,turn;
+    double prePwmDuty[4],PwmDuty[4];
+    int i=0,n=0;
+    
+    dig = data[0]+data[1]*128;
+    rad = (dig/180.0)*PI;
+    if(dig==360){
+        while(n<4){
+            prePwmDuty[n]=0;
+            n++;
+        }
+    }else{
+        prePwmDuty[0]=1.41421356*sin(rad+qPI)*conratio;
+        prePwmDuty[1]=1.41421356*sin(rad-qPI)*conratio;
+        prePwmDuty[2]=prePwmDuty[1];
+        prePwmDuty[3]=prePwmDuty[0];
+    }
+    
+    turn = (data[2]-63)/64.0;
+    turn = 0;
+    
+    prePwmDuty[0]+=turn*ratio;
+    prePwmDuty[1]+=turn*ratio;
+    prePwmDuty[2]-=turn*ratio;
+    prePwmDuty[3]-=turn*ratio;
+    
+    while(i<4){
+        if(prePwmDuty[i]<-1.0) prePwmDuty[i]=-1.0;
+        else if(prePwmDuty[i]>1.0) prePwmDuty[i]=1.0;
+        PwmDuty[i]=(fabs(prePwmDuty[i]+1.0))/2;
+        i++;
+    }
+    
+    MDFL[0] = PwmDuty[0]*255;
+    MDFR[0] = PwmDuty[1]*255;
+    MDRL[0] = PwmDuty[2]*255;
+    MDRR[0] = PwmDuty[3]*255;
+    
+    //printf("%f,%f,%f,%f\r\n",PwmDuty[0],PwmDuty[1],PwmDuty[2],PwmDuty[3]);
+}
+
+void MotorReset(){
+    char data[2]={127,0};
+    int i=0;
+    while(i<4){
+        i2c.write(addr[i],data,2,false);
+        i++;
+    }
+}
+
+void MotorRun(){
+    char data[2]={255,0};
+    int i=0;
+    while(i<4){
+        i2c.write(addr[i],data,2,false);
+        i++;
+    }
+}
+
+int main() {
+    char MDFL[2],MDFR[2],MDRL[2],MDRR[2];
+    int i2cVAL=0;
+    int i=0;
+    
+    i2c.frequency(300000);
+    ledsReset();
+    FEP02.baud(19200);
+    MotorRun();
+    wait(3);
+    MotorReset();
+    //FEP02.attach(&getConData);
+    leds[0]=1;
+    
+    
+    while(1) {
+        i2cVAL=0;
+        i=0;
+        if(FEP02.getc()==255){
+            while(i<5){
+                data[i]=FEP02.getc();
+                i++;
+            }
+            MotorOut(MDFL,MDFR,MDRL,MDRR);
+            
+            i2cVAL += i2c.write(addr[0],MDFL,2,false);
+            i2cVAL += i2c.write(addr[1],MDFR,2,false);
+            i2cVAL += i2c.write(addr[2],MDRL,2,false);
+            i2cVAL += i2c.write(addr[3],MDRR,2,false);
+            
+            printf("%d\r\n",i2cVAL);
+            printf("%d,%d,%d,%d\r\n",MDFL[0],MDFR[0],MDRL[0],MDRR[0]);
+            leds[0]=!leds[0];
+        }            
+    }
+}