Diff: main.cpp

Revision:

11:336dd293daa1

Parent:

10:53cb691e22bf

Child:

12:2564eac22e0a

--- a/main.cpp	Sat Jul 18 05:53:10 2015 +0000
+++ b/main.cpp	Sun Jul 19 17:38:38 2015 +0000
@@ -3,6 +3,7 @@
 
 #include "communication.h"
 #include "protocol.h"
+#include "iSerial.h"
 
 #include "motor_relay.h"
 #include "motion_control.h"
@@ -13,175 +14,184 @@
 
 //counter not receive from station
 #define TIMEOUT_RESPONE_COMMAND 5
-/*
-//define pin class
-DigitalOut dirA_LU(INA_L_U);
-DigitalOut dirB_LU(INB_L_U);
-
-DigitalOut dirA_LL(INA_L_L);
-DigitalOut dirB_LL(INB_L_L);
-
-DigitalOut dirA_RU(INA_R_U);
-DigitalOut dirB_RU(INB_R_U);
-
-DigitalOut dirA_RL(INA_R_L);
-DigitalOut dirB_RL(INB_R_L);
-
-DigitalIn sw_LU_U(LIMIT_LU_U,PullUp);
-DigitalIn sw_LU_D(LIMIT_LU_D,PullUp);
-
-DigitalIn sw_LL_U(LIMIT_LL_U,PullUp);
-DigitalIn sw_LL_D(LIMIT_LL_D,PullUp);
-
-DigitalIn sw_RU_U(LIMIT_RU_U,PullUp);
-DigitalIn sw_RU_D(LIMIT_RU_D,PullUp);
-
-DigitalIn sw_RL_U(LIMIT_RL_U,PullUp);
-DigitalIn sw_RL_D(LIMIT_RL_D,PullUp);
-
-AnalogIn position_LU(VR_LU);
-AnalogIn position_LL(VR_LL);
-AnalogIn position_RU(VR_RU);
-AnalogIn position_RL(VR_RL);
-*/
-
 
 MOTION_CONTROL leg_left_upper(INA_L_U,INB_L_U,LIMIT_LU_U,LIMIT_LU_D,VR_LU);
 MOTION_CONTROL leg_left_lower(INA_L_L,INB_L_L,LIMIT_LL_U,LIMIT_LL_D,VR_LL);
 MOTION_CONTROL leg_right_upper(INA_R_U,INB_R_U,LIMIT_RU_U,LIMIT_RU_D,VR_RU);
 MOTION_CONTROL leg_right_lower(INA_R_L,INB_R_L,LIMIT_RL_U,LIMIT_RL_D,VR_RL);
 
-/*
-MOTOR_RELAY leg_left_upper(INA_L_U,INB_L_U);
-MOTION_CONTROL leg_left_lower(INA_L_L,INB_L_L,LIMIT_LL_U,LIMIT_LL_D,VR_LL);
-MOTION_CONTROL leg_right_upper(INA_R_U,INB_R_U,LIMIT_RU_U,LIMIT_RU_D,VR_RU);
-MOTION_CONTROL leg_right_lower(INA_R_L,INB_R_L,LIMIT_RL_U,LIMIT_RL_D,VR_RL);
-*/
 DigitalOut myled(LED1);
 DigitalIn mybutton(USER_BUTTON);
 
 //communication config
 //serial for debug
-Serial pc(USBTX, USBRX);
+iSerial pc(USBTX, USBRX);
 //serial for xbee
 COMMUNICATION pan_a(TX_XBEE, RX_XBEE,115200,1000,1000); // tx, rx
 
-//Function Prototype
-//void motor_set(uint8_t id, uint8_t direct);
-//void motor_stop(uint8_t id);
+//Fuction prototye
+void getCommand();
+//  init function
+void calibration();
+void test_position();
 
-uint8_t limit_motor(uint8_t id, uint8_t dirction);
-uint8_t position_control(uint8_t id, uint16_t current, uint16_t target);
 
-void calibration(uint8_t id);
+void copy_data(ANDANTE_PROTOCOL_PACKET *scr, ANDANTE_PROTOCOL_PACKET *dst);
 
-void getCommand();
 
 //set foreground
 Ticker Update_command;
 
+Timer t;
+
+//variable
+volatile ANDANTE_PROTOCOL_PACKET *param;
+volatile uint8_t status=0;
 
 //Main function
 int main()
 {
-    
-    int state;
-    int state_count=0;
-    unsigned int count=0;
-   
-/*   
+    int state=0;
+    pc.baud(115200);
+    pc.printf("Welcome to DOGWHEELSCHAIR\n");
+    if(mybutton == 0) {
+        calibration();
+    } else {
+        pc.printf("Lock position Min-Max...");
+        leg_left_upper.SetMaxPosition(56244);
+        leg_left_upper.SetMinPosition(20806);
+
+        leg_left_lower.SetMaxPosition(50996);
+        leg_left_lower.SetMinPosition(5371);
+
+        leg_right_upper.SetMaxPosition(38985);
+        leg_right_upper.SetMinPosition(8545);
+
+        leg_right_lower.SetMaxPosition(38027);
+        leg_right_lower.SetMinPosition(40);
+        pc.printf("pass\n");
+    }
+
+    Update_command.attach(&getCommand,TIMER_UPDATE);
+
+    t.start();
     while(1) {
 
-        if(count < 800000) {
-            count++;
-        } else {
-            count=0;
-            state_count= ~state_count;
+
+        if(t.read() > 10.0f) {
+            t.reset();
+            if(status >3) {
+                status =0;
+            } else {
+                status++;
+            }
         }
 
-        if(state_count ==0) {
-            state = leg_left_upper.limit_motor(1);
-
-            state = leg_left_lower.limit_motor(1);
-
-            //           if(state ==1) {
-            //       pc.printf("state[LU1]=1\n");
-            //  } else if(state == -1) {
-            //       pc.printf("state[LU1]=-1\n");
-            //   } else {
-            //       pc.printf("state[LU1]=0\n");
-            //   }
-    //       pc.printf("state[LL]=%d,state\n");
-
-            state = leg_right_upper.limit_motor(1);
-            //      pc.printf("state[RU]=%d,state\n");
-            state = leg_right_lower.limit_motor(1);
-            //     pc.printf("state[RL]=%d,state\n");
-
-            //wait(1);
-        } else {
-            state = leg_left_upper.limit_motor(2);
-
-            //     pc.printf("state[LU]=%d,state\n");
-            state = leg_left_lower.limit_motor(2);
-    //                               if(state ==1) {
-    //               pc.printf("state[LU2]=1\n");
-            //          } else if(state == -1) {
-            //              pc.printf("state[LU2]=-1\n");
-            //          } else {
-            //              pc.printf("state[LU2]=0\n");
-            //          }
-            //     pc.printf("state[LL]=%d,state\n");
-
-            state = leg_right_upper.limit_motor(2);
-            //     pc.printf("state[RU]=%d,state\n");
-            state = leg_right_lower.limit_motor(2);
-            //     pc.printf("state[RL]=%d,state\n\n");
-            //wait(1);
+        if(status == 0) {
+            state =0;
+            // sleep
+            state += leg_left_upper.position_control(0);
+            state += leg_right_upper.position_control(0);
+            state += leg_left_lower.position_control(64);
+            state += leg_right_lower.position_control(64);
+            if(state == 8) {
+                myled=1;
+            } else {
+                myled=0;
+            }
+        } else if(status == 1 || status ==3) {
+            state =0;
+            // sit
+            state += leg_left_upper.position_control(64);
+            state += leg_right_upper.position_control(64);
+            state += leg_left_lower.position_control(64);
+            state += leg_right_lower.position_control(64);
+            if(state == 8) {
+                myled=1;
+            } else {
+                myled=0;
+            }
+        } else if(status == 2) {
+            state =0;
+            // stand
+            state += leg_left_upper.position_control(64);
+            state += leg_right_upper.position_control(64);
+            state += leg_left_lower.position_control(0);
+            state += leg_right_lower.position_control(0);
+            if(state == 8) {
+                myled=1;
+            } else {
+                myled=0;
+            }
         }
     }
 
-    */
-    /*
-        while(1) {
-            //Read position
-            pc.printf("vr_LL = %d\t",leg_left_upper.GetAnalog());
-            pc.printf("vr_LU = %d\t",leg_left_lower.GetAnalog());
-            pc.printf("vr_RL = %d\t",leg_right_upper.GetAnalog());
-            pc.printf("vr_RU = %d\n",leg_right_lower.GetAnalog());
+}
+
+void getCommand()
+{
+    static uint8_t count =0;
+
+    ANDANTE_PROTOCOL_PACKET packet;
+
+    uint8_t status = pan_a.receiveCommunicatePacket(&packet);
+    myled=0;
+
+    if(status == ANDANTE_ERRBIT_NONE) {
+        if(count >2 && count <10) {
+            count--;
+        } else {
+            count=0;
         }
-    */
+
+
+        //    pan_a.sendCommunicatePacket(&packet);
+
+        //update command
+        //setControl(&packet);
+
+    } else if(status == ANDANTE_ERRBIT_READ_TIMEOUT) {
+        count++;
+    }
 
- //   while(1) {
-        //calibration
-        pc.printf("Welcome to DOGWHEELSCHAIR\n");
-        pc.printf("Calibration [START]\n");
-        leg_left_upper.calibration();
-        pc.printf("Left_UPPER\n");
-        pc.printf("max_position = %d\n",leg_left_upper.GetMaxPosition());
-        pc.printf("min_position = %d\n",leg_left_upper.GetMinPosition());
-        leg_left_lower.calibration();
-        pc.printf("Left_Lower\n");
-        pc.printf("max_position = %d\n",leg_left_lower.GetMaxPosition());
-        pc.printf("min_position = %d\n",leg_left_lower.GetMinPosition());
-        leg_right_upper.calibration();
-        pc.printf("right_UPPER\n");
-        pc.printf("max_position = %d\n",leg_right_upper.GetMaxPosition());
-        pc.printf("min_position = %d\n",leg_right_upper.GetMinPosition());
-        leg_right_lower.calibration();
-        pc.printf("right_Lower\n");
-        pc.printf("max_position = %d\n",leg_right_lower.GetMaxPosition());
-        pc.printf("min_position = %d\n",leg_right_lower.GetMinPosition());
+    if(count >= TIMEOUT_RESPONE_COMMAND) {
+        //stop robot
+        count++;
+        myled=1;
+        //   setSpeedControl(0.0);
+    }
+}
 
-        pc.printf("Calibration [FINISH]\n");
-        pc.printf("RUN mode [START]\n");
-        wait(1);
- //   }
+void calibration()
+{
+    //calibration
+    pc.printf("Calibration [START]\n");
+    leg_left_upper.calibration();
+    pc.printf("Left_UPPER\n");
+    pc.printf("max_position = %d\n",leg_left_upper.GetMaxPosition());
+    pc.printf("min_position = %d\n",leg_left_upper.GetMinPosition());
+    leg_left_lower.calibration();
+    pc.printf("Left_Lower\n");
+    pc.printf("max_position = %d\n",leg_left_lower.GetMaxPosition());
+    pc.printf("min_position = %d\n",leg_left_lower.GetMinPosition());
+    leg_right_upper.calibration();
+    pc.printf("right_UPPER\n");
+    pc.printf("max_position = %d\n",leg_right_upper.GetMaxPosition());
+    pc.printf("min_position = %d\n",leg_right_upper.GetMinPosition());
+    leg_right_lower.calibration();
+    pc.printf("right_Lower\n");
+    pc.printf("max_position = %d\n",leg_right_lower.GetMaxPosition());
+    pc.printf("min_position = %d\n",leg_right_lower.GetMinPosition());
 
-    Update_command.attach(&getCommand,TIMER_UPDATE);
-    
-    do
-    {
+    pc.printf("Calibration [FINISH]\n");
+    pc.printf("RUN mode [START]\n");
+    wait(1);
+}
+
+void test_position()
+{
+    uint8_t state;
+    do {
         /*
         state=0;
         //leg_left_upper.position_control(500);
@@ -198,43 +208,9 @@
         pc.printf("state_ll %d\n",state);
         state = leg_right_upper.position_control(32);
         pc.printf("state_ru %d\n",state);
-       state = leg_right_lower.position_control(32);
+        state = leg_right_lower.position_control(32);
         pc.printf("state_rl %d\n",state);
         state=0;
-    }while(state != 2);
+    } while(state != 2);
     pc.printf("[Finish test]\n");
-}
-
-void getCommand()
-{
-    static uint8_t count =0;
-
-    ANDANTE_PROTOCOL_PACKET packet;
-
-    uint8_t status = pan_a.receiveCommunicatePacket(&packet);
-    myled=0;
-
-
-
-    if(status == ANDANTE_ERRBIT_NONE) {
-        if(count >2 && count <10) {
-            count--;
-        } else {
-            count=0;
-
-        }
-        pan_a.sendCommunicatePacket(&packet);
-        //update command
-        //setControl(&packet);
-
-    } else if(status == ANDANTE_ERRBIT_READ_TIMEOUT) {
-        count++;
-    }
-
-    if(count >= TIMEOUT_RESPONE_COMMAND) {
-        //stop robot
-        count++;
-        myled=1;
-        //   setSpeedControl(0.0);
-    }
 }
\ No newline at end of file