Diff: main.cpp

Revision:

4:42dc950d54f4

Parent:

3:12f2ee26a43f

Child:

5:aae71a94cb41

--- a/main.cpp	Sun Mar 01 15:41:24 2015 +0000
+++ b/main.cpp	Mon Mar 02 05:53:20 2015 +0000
@@ -1,10 +1,11 @@
+#include "main.h"
 #include "mbed.h"
 #include "param.h"
 #include "pin_config.h"
 
-#include "iSerial.h"
-//#include "protocol.h"
+//#include "iSerial.h"
 #include "communication.h"
+#include "protocol.h"
 
 
 //control stearing
@@ -20,6 +21,12 @@
 Ticker Update_command;
 Ticker Samping;
 
+
+//Init communication serial 2chanal
+COMMUNICATION pan_a(TX_PAN_A, RX_PAN_A,115200,1000,1000); // tx, rx
+COMMUNICATION pan_b(TX_PAN_B, RX_PAN_B,115200,1000,100); // tx, rx
+
+
 void Init_servo()
 {
     heading_front.period_ms(20);
@@ -27,9 +34,66 @@
     drive_motor.period_ms(20);
 }
 
-void setControl()
+void Init_foreground()
+{
+    Update_command.attach(&getCommand,TIMER_UPDATE);
+    Samping.attach(&getSensor,TIMER_SAMPING);
+}
+
+
+int main()
 {
+    myled=0;
 
+
+    Init_servo();
+
+    ANDANTE_PROTOCOL_PACKET packet;
+
+    packet.robotId = 0xBB;
+    packet.length = 8;
+    packet.instructionErrorId = 0x01;
+    packet.parameter[0] = 0x02;
+    packet.parameter[1] = 0x03;
+    packet.parameter[2] = 0x04;
+    packet.parameter[3] = 0x05;
+    packet.parameter[4] = 0x06;
+    packet.parameter[5] = 0x07;
+
+    pan_a.sendCommunicatePacket(&packet);
+
+
+
+
+}
+
+
+void getCommand()
+{
+    static uint8_t count =0;
+
+    ANDANTE_PROTOCOL_PACKET packet;
+
+    uint8_t status = pan_a.receiveCommunicatePacket(&packet);
+
+    if(status == ANDANTE_ERRBIT_NONE) {
+        if(count >1) {
+            count--;
+        } else {
+            count=0;
+        }
+        //update command
+        setControl(&packet);
+
+    } else if(status == ANDANTE_ERRBIT_READ_TIMEOUT) {
+        count++;
+
+    }
+
+    if(count >= TIMEOUT_RESPONE_COMMAND) {
+        //stop robot
+        setSpeedControl(0.0);
+    }
 }
 
 void getSensor()
@@ -37,49 +101,68 @@
 
 }
 
-void Init_foreground()
+void setControl(ANDANTE_PROTOCOL_PACKET *packet)
 {
-    Update_command.attach(&setControl,TIMER_UPDATE);
-    Samping.attach(&getSensor,TIMER_SAMPING);
+    if(packet->instructionErrorId && ANDANTE_SPEED_CONTROL == ANDANTE_SPEED_CONTROL) {
+        setSpeedControl(((int16_t)packet->parameter[0]-127)/128.0);
+    }
+
+    if(packet->instructionErrorId && ANDANTE_FRONT_ANGLE == ANDANTE_FRONT_ANGLE) {
+        setFrontAngle(((int16_t)packet->parameter[1]-127)/128.0);
+    }
+    if(packet->instructionErrorId && ANDANTE_REAR_ANGLE == ANDANTE_REAR_ANGLE) {
+        setRearAngle(((int16_t)packet->parameter[2]-127)/128.0);
+    }
 }
 
 
-
-
-
-int main()
+void setFrontAngle(float precent)
 {
-    myled=0;
-    //Init communication serial 2chanal
-    COMMUNICATION pan_a(TX_PAN_A, RX_PAN_A,115200,1000,1000); // tx, rx
-    COMMUNICATION pan_b(TX_PAN_B, RX_PAN_B,115200,1000,100); // tx, rx
+    int us=FRONT_CENTER;
+
+    if(precent > (float)1.00 || precent < (float)-1.00) {
+        heading_front.pulsewidth_us(us); //STOP
+        return;
+    }
 
-    Init_servo();
-    
-        ANDANTE_PROTOCOL_PACKET packet;
+    if(precent <0) {
+        us = (FRONT_CENTER-FRONT_MIN)*precent +FRONT_CENTER;
+    } else if(precent >0) {
+        us = (FRONT_MAX-FRONT_CENTER)*precent +FRONT_CENTER;
+    }
+    heading_front.pulsewidth_us(us);
+}
+
+void setRearAngle(float precent)
+{
+    int us=REAR_CENTER;
 
-        packet.robotId = 0xBB;
-        packet.length = 8;
-        packet.instructionErrorId = 0x01;
-        packet.parameter[0] = 0x02;
-        packet.parameter[1] = 0x03;
-        packet.parameter[2] = 0x04;
-        packet.parameter[3] = 0x05;
-        packet.parameter[4] = 0x06;
-        packet.parameter[5] = 0x07;
+    if(precent > (float)1.0 || precent < (float)-1.0) {
+        heading_rear.pulsewidth_us(us); //STOP
+        return;
+    }
+
+    if(precent <0) {
+        us = (REAR_CENTER-REAR_MIN)*precent +REAR_CENTER;
+    } else if(precent >0) {
+        us = (REAR_MAX-REAR_CENTER)*precent +REAR_CENTER;
+    }
+    heading_rear.pulsewidth_us(us);
+}
 
-        pan_a.sendCommunicatePacket(&packet);
-     
-    while(1) {
-        //      if(pan_a->readable() !=0) {
-
-        uint8_t status = pan_a.receiveCommunicatePacket(&packet);
+void setSpeedControl(float precent)
+{
+    int us=RC_SPEED_CENTER;
 
-        if(status == ANDANTE_ERRBIT_NONE) {
-            myled =1;
-            wait(1);
-            myled=0;
-        }
+    if(precent > (float)1.0 || precent < (float)-1.0) {
+        drive_motor.pulsewidth_us(us); //STOP
+        return;
+    }
 
+    if(precent <0) {
+        us = (RC_SPEED_CENTER-RC_SPEED_MIN)*precent +RC_SPEED_CENTER;
+    } else if(precent >0) {
+        us = (RC_SPEED_MAX-RC_SPEED_CENTER)*precent +RC_SPEED_CENTER;
     }
-}
+    drive_motor.pulsewidth_us(us);
+}
\ No newline at end of file