Diff: main.cpp

Revision:

6:9e9143e97bae

Parent:

3:07a21d1d16e4

--- a/main.cpp	Wed Apr 06 22:40:31 2016 +0000
+++ b/main.cpp	Thu Apr 14 17:17:56 2016 +0000
@@ -1,39 +1,205 @@
 #include "mbed.h"
 
+#define ERROR 0
+#define VALID 1
+
+DigitalOut R(LED_RED);
+Ticker TimeoutTimer;
+Serial pc(USBTX, USBRX);
+Serial bt(PTE0, PTE1);
+DigitalOut motorFront_1(D11);  //pines de sentido del motor delantero y trasero
+DigitalOut motorFront_2(D12);
+
+DigitalOut motorTurn_1(D9);  //pines de sentido del motor volante
+DigitalOut motorTurn_2(D8);
+
+PwmOut pwm_motorUPDOWN(D7);
+PwmOut pwm_motorTURN(D5);
+
+void SetMOTOR(int *, int *, float *, float *,int *, int *,int *, int *);
+void go_forward(void);
+void go_reverse(void);
+void stop(void);
+void turn_left(void);
+void turn_right(void);
+void turn_none(void);
+//Global variables
+char c;
+
+////Functions/////
+//Timeout
+void TimeOutRoutine()
+{
+    pwm_motorUPDOWN.write(0.0);
+    pwm_motorTURN.write(0.0);
+    R=0;
+}
+
+//Coversion to PWM vals
+float min(float a, float b){
+return (a>b)?b:a;      
+}
+float ConvertSteer(uint8_t val)
+{
+    float temp;
+    if(val>127)
+        temp=(val-127)/128.0;
+    else
+        temp=(127-val)/128.0;
+
+    return temp;
+}
+
+//Decode frame from Bluetooth
+uint8_t DecodeBT(void)
+{
+    float SteerPWMVal, PedalPWMVal;
+    uint8_t Steer=0,Pedal=0;
+    c = bt.getc();
+    if(c=='S') { //Check Start of frame
+        while(!bt.readable());
+        c = bt.getc();
+        if(c=='T') { //Check Start of frame
+            while(!bt.readable());
+            //Read and convert data
+            Steer = bt.getc();
+            while(!bt.readable());
+            Pedal = bt.getc();
+                     
+            if(Steer>146)
+                turn_right();
+            else if(Steer<110)
+                turn_left();
+            else
+                turn_none();
+            if(Pedal>146)
+                go_forward();
+            else if(Pedal<110)
+                go_reverse();
+            else
+                stop();
+            SteerPWMVal=min(ConvertSteer(Steer)*2.3,.9);
+            PedalPWMVal=min(ConvertSteer(Pedal)*2.3,1);
+        
+            //*sw2=(SteerPWMVal>0);
+            //*sw1=(PedalPWMVal>0);
+            
+                
+            pwm_motorUPDOWN=PedalPWMVal;
+            pwm_motorTURN=SteerPWMVal;
+            PedalPWMVal=pwm_motorUPDOWN;
+            SteerPWMVal=pwm_motorTURN;
+            pc.printf("Steer : %f\t  SW1=%i \t ", SteerPWMVal, Steer);
+            pc.printf("Pedal : %f\t SW2=%i\n\r", PedalPWMVal, Pedal);
+            R=1;
+            return VALID; //Valid frame received
+        }
+    }
+    return ERROR;
+}
+
+
+int main()
+{
+    int Currnt_st_UPDOWN = 0;
+    int Befr_st_UPDOWN  = 1;
+    int Currnt_st_LEFT_RIGTH = 0;
+    int Befr_st_LEFT_RIGTH = 1;
+    int mySW_UPDOWN =0;
+    int mySW_LEFT_RIGTH = 0;
+
+    float PWM_UPDOWN = 0.0;
+    float PWM_LEFT_RIGTH = 0.0;
+    R=1;
+    pwm_motorUPDOWN.period_us(50);   //10 kHz
+    pwm_motorTURN.period_us(50);   //10 kHz
+    pwm_motorUPDOWN.write(PWM_UPDOWN);
+    pwm_motorTURN.write(PWM_LEFT_RIGTH);
+    
+
 
 
-//AnalogOut r(LED_RED);
-//AnalogOut b(LED_BLUE);
-Serial pc(USBTX, USBRX);
-Serial bt(PTE0, PTE1);
-
-char c;
-
-int main()
-{
-//    r=0;
-//    b=0;
     bt.baud(9600);
     while(1) {
         while(!bt.readable());
-        c = bt.getc();
-        if(c=='S') {
-            while(!bt.readable());
-            c = bt.getc();
-            if(c=='T') {
-                while(!bt.readable());
-                c = bt.getc();
-                pc.printf("Axis 1: %d\t\t", c);
-                //r=c/255.0;
-                while(!bt.readable());
-                c = bt.getc();
-                pc.printf("Axis 2: %d\n\r", c);
-                //b=c/255.0;
+        if(DecodeBT()==VALID) {
+            //SetMOTOR(&mySW_UPDOWN, &mySW_LEFT_RIGTH, &PWM_UPDOWN, &PWM_LEFT_RIGTH, &Currnt_st_UPDOWN, &Befr_st_UPDOWN, &Currnt_st_LEFT_RIGTH, &Befr_st_LEFT_RIGTH);
+            TimeoutTimer.detach();
+            TimeoutTimer.attach_us(&TimeOutRoutine,200000);
+        }
+    }
+
+}
+void SetMOTOR(int *ptr_mySW_UPDOWN, int *ptr_mySW_LEFT_RIGTH, float *ptr_PWM_UPDOWN, float *ptr_PWM_LEFT_RIGTH,int *ptr_Currnt_st_UPDOWN, int *ptr_Befr_st_UPDOWN,int *ptr_Currnt_st_LEFT_RIGTH, int *ptr_Befr_st_LEFT_RIGTH)
+{
+
 
-            }
+    if(*ptr_mySW_UPDOWN==1) {
+        go_forward();
+        *ptr_Currnt_st_UPDOWN = 1;
+        if(*ptr_Currnt_st_UPDOWN!=*ptr_Befr_st_UPDOWN) {
+            pwm_motorUPDOWN.write(*ptr_PWM_UPDOWN);
+            *ptr_Befr_st_UPDOWN = *ptr_Currnt_st_UPDOWN;
+        }
+
+    } else {
+        go_reverse();
+        *ptr_Currnt_st_UPDOWN = 0;
+        if(*ptr_Currnt_st_UPDOWN!=*ptr_Befr_st_UPDOWN) {
+            pwm_motorUPDOWN.write(*ptr_PWM_UPDOWN);
+            *ptr_Befr_st_UPDOWN = *ptr_Currnt_st_UPDOWN;
+        }
+
+    }
+    if(*ptr_mySW_LEFT_RIGTH==1) {
+        turn_left();
+        *ptr_Currnt_st_LEFT_RIGTH = 1;
+        if(*ptr_Currnt_st_LEFT_RIGTH!=*ptr_Befr_st_LEFT_RIGTH) {
+            pwm_motorTURN.write(*ptr_PWM_LEFT_RIGTH);
+            *ptr_Befr_st_LEFT_RIGTH = *ptr_Currnt_st_LEFT_RIGTH;
         }
 
-
+    } else {
+        turn_right();
+        *ptr_Currnt_st_LEFT_RIGTH = 0;
+        if(*ptr_Currnt_st_LEFT_RIGTH!=*ptr_Befr_st_LEFT_RIGTH) {
+            pwm_motorTURN.write(*ptr_PWM_LEFT_RIGTH);
+            *ptr_Befr_st_LEFT_RIGTH = *ptr_Currnt_st_LEFT_RIGTH;
+        }
     }
+}
+void stop(void)
+{
+    motorFront_1 = 0;
+    motorFront_2 = 0;
 
+}
+
+void go_forward(void)
+{
+    motorFront_1 = 1;
+    motorFront_2 = 0;
+
+}
+void go_reverse(void)
+{
+    motorFront_1 = 0;
+    motorFront_2 = 1;
+
+}
+
+void turn_left(void)
+{
+    motorTurn_1 = 0;
+    motorTurn_2 = 1;
+}
+void turn_right(void)
+{
+    motorTurn_1 = 1;
+    motorTurn_2 = 0;
+}
+void turn_none(void)
+{
+    motorTurn_1 = 0;
+    motorTurn_2 = 0;
 }
\ No newline at end of file