Diff: main.cpp

Revision:

10:53cb691e22bf

Parent:

9:0de6ce956985

Child:

11:336dd293daa1

--- a/main.cpp	Fri Jul 17 14:05:01 2015 +0000
+++ b/main.cpp	Sat Jul 18 05:53:10 2015 +0000
@@ -4,13 +4,16 @@
 #include "communication.h"
 #include "protocol.h"
 
+#include "motor_relay.h"
+#include "motion_control.h"
+
 //set frequancy unit in Hz
 #define F_UPDATE 10.0f
 #define TIMER_UPDATE 1.0f/F_UPDATE
 
 //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);
@@ -40,7 +43,20 @@
 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);
 
@@ -51,8 +67,8 @@
 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);
+//void motor_set(uint8_t id, uint8_t direct);
+//void motor_stop(uint8_t id);
 
 uint8_t limit_motor(uint8_t id, uint8_t dirction);
 uint8_t position_control(uint8_t id, uint16_t current, uint16_t target);
@@ -61,23 +77,6 @@
 
 void getCommand();
 
-
-//Globle Variable
-uint16_t max_pos_LU= 10000;
-uint16_t min_pos_LU= 6000;
-uint16_t max_pos_LL= 50000;
-uint16_t min_pos_LL= 37000;
-
-uint16_t max_pos_RU= 17800;
-uint16_t min_pos_RU= 9000;
-uint16_t max_pos_RL= 51000;
-uint16_t min_pos_RL= 11000;
-
-uint16_t offset_pos =1000;
-uint16_t test_variable =1000;
-
-int16_t MARGIN = 500;
-
 //set foreground
 Ticker Update_command;
 
@@ -85,570 +84,127 @@
 //Main function
 int main()
 {
-    uint16_t vr_lu,vr_ru;
-    uint16_t vr_ll,vr_rl;
-    pc.printf("wait\n");
-    motor_stop(0);
-    //  wait(10);
-    /*
-        while(1) {
-            motor_set(1,1);
-            motor_set(2,1);
-            motor_set(3,1);
-            motor_set(4,1);
-            wait(1);
-            motor_set(1,2);
-            motor_set(2,2);
-            motor_set(3,2);
-            motor_set(4,2);
-            wait(1);
+    
+    int state;
+    int state_count=0;
+    unsigned int count=0;
+   
+/*   
+    while(1) {
+
+        if(count < 800000) {
+            count++;
+        } else {
+            count=0;
+            state_count= ~state_count;
         }
+
+        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);
+        }
+    }
+
     */
-
     /*
         while(1) {
             //Read position
-            vr_ll = position_LL.read_u16();
-            vr_lu = position_LU.read_u16();
-            vr_rl = position_RL.read_u16();
-            vr_ru = position_RU.read_u16();
-            pc.printf("vr_LL = %d\t",vr_ll);
-            pc.printf("vr_LU = %d\t",vr_lu);
-            pc.printf("vr_RL = %d\t",vr_rl);
-            pc.printf("vr_RU = %d\n",vr_ru);
-        }
-    */
-
-    /*
-        while(1) {
-            if(sw_LU_U) {
-                pc.printf("sw_LU_U = 1");
-            } else {
-                pc.printf("sw_LU_U = 0");
-            }
-
-            if(sw_LU_D) {
-                pc.printf(" sw_LU_D = 1");
-            } else {
-                pc.printf(" sw_LU_D = 0");
-            }
-            pc.printf("\n");
-    ///////////////////////////////////
-            if(sw_LL_U) {
-                pc.printf("sw_LL_U = 1");
-            } else {
-                pc.printf("sw_LL_U = 0");
-            }
-
-            if(sw_LL_D) {
-                pc.printf(" sw_LL_D = 1");
-            } else {
-                pc.printf(" sw_LL_D = 0");
-            }
-            pc.printf("\n");
-            //////////////////////////////////////
-            ///////////////////////////////////
-
-            /////////////////////
-            if(sw_RU_U) {
-                pc.printf("sw_RU_U = 1");
-            } else {
-                pc.printf("sw_RU_U = 0");
-            }
-
-            if(sw_RU_D) {
-                pc.printf(" sw_RU_D = 1");
-            } else {
-                pc.printf(" sw_RU_D = 0");
-            }
-    pc.printf("\n");
-    ///////////////////////////////////
-            if(sw_RL_U) {
-                pc.printf("sw_RL_U = 1");
-            } else {
-                pc.printf("sw_RL_U = 0");
-            }
-
-            if(sw_RL_D) {
-                pc.printf(" sw_RL_D = 1");
-            } else {
-                pc.printf(" sw_RL_D = 0");
-            }
-            //////////////////////////////////////
-    pc.printf("\n");
-    pc.printf("\n");
-            wait(1);
+            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());
         }
     */
 
-    /*
-    while(1) {
-        myled =1;
-        wait_ms(200);
-
-        if(mybutton == 0) {
-            myled =0;
-            wait_ms(200);
-        }
-
-        if(sw_LU_U == 0) {
-            myled =0;
-            wait_ms(200);
-        }
-
-        if(sw_LU_D == 0) {
-            myled =0;
-            wait_ms(200);
-        }
-
-        if(sw_LL_U == 0) {
-            myled =0;
-            wait_ms(200);
-        }
-
-        if(sw_LL_D == 0) {
-            myled =0;
-            wait_ms(200);
-        }
+ //   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(sw_RU_U == 0) {
-            myled =0;
-            wait_ms(200);
-        }
-
-        if(sw_RU_D == 0) {
-            myled =0;
-            wait_ms(200);
-        }
-
-        if(sw_RL_U == 0) {
-            myled =0;
-            wait_ms(200);
-        }
-
-        if(sw_RL_D == 0) {
-            myled =0;
-            wait_ms(200);
-        }
-
-    }
-    */
-
-      while(1) {
-    //calibration
-    pc.printf("Welcome to DOGWHEELSCHAIR\n");
-    pc.printf("Calibration [START]\n");
-    calibration(1);
-    calibration(2);
-    calibration(3);
-    calibration(4);
-    pc.printf("Calibration [FINISH]\n");
-    pc.printf("RUN mode [START]\n");
-       }
+        pc.printf("Calibration [FINISH]\n");
+        pc.printf("RUN mode [START]\n");
+        wait(1);
+ //   }
 
     Update_command.attach(&getCommand,TIMER_UPDATE);
-
-    uint8_t count=0;
-
-    while(1) {
-        uint8_t state=0;
-        pc.printf("Count %d",count);
-        do {
-            state=0;
-
-            if(position_control(1,position_LU.read_u16(),max_pos_LU-offset_pos) == 1) {
-                state++;
-            }
-
-            if(position_control(2,position_LL.read_u16(),max_pos_LL-offset_pos) == 1) {
-                state++;
-            }
-
-            if(position_control(3,position_RU.read_u16(),max_pos_RU-offset_pos) == 1) {
-                state++;
-            }
-
-            if(position_control(4,position_RL.read_u16(),max_pos_RL-offset_pos) == 1) {
-                state++;
-            }
-
-            pc.printf("state = %d",state);
-        } while(state <= 4 );
-
-        do {
-            state=0;
-
-            if(position_control(1,position_LU.read_u16(),min_pos_LU+offset_pos) == 1) {
-                state++;
-            }
-
-            if(position_control(2,position_LL.read_u16(),min_pos_LL+offset_pos) == 1) {
-                state++;
-            }
-
-            if(position_control(3,position_RU.read_u16(),min_pos_RU+offset_pos) == 1) {
-                state++;
-            }
-
-            if(position_control(4,position_RL.read_u16(),min_pos_RL+offset_pos) == 1) {
-                state++;
-            }
-
-            pc.printf("state = %d",state);
-        } while(state <= 4 );
-        count++;
-    }
-}
-
-
-
-void motor_set(uint8_t id, uint8_t direct)
-{
-    //direct: Should be between 0 and 3, with the following result
-    //0: Brake to VCC
-    //1: Clockwise
-    //2: CounterClockwise
-    //3: Brake to GND
-
-    if(direct <=4) {
-        switch(id) {
-            case 1:
-                // Set inA[motor]
-                if (direct <=1)
-                    dirA_LU=0;
-                else
-                    dirA_LU=1;
-
-                // Set inB[motor]
-                if ((direct==0)||(direct==2))
-                    dirB_LU=0;
-                else
-                    dirB_LU=1;
-                break;
-
-            case 2:
-                // Set inA[motor]
-                if (direct <=1)
-                    dirA_LL=0;
-                else
-                    dirA_LL=1;
-
-                // Set inB[motor]
-                if ((direct==0)||(direct==2))
-                    dirB_LL=0;
-                else
-                    dirB_LL=1;
-                break;
-
-            case 3:
-                // Set inA[motor]
-                if (direct <=1)
-                    dirA_RU=0;
-                else
-                    dirA_RU=1;
-
-                // Set inB[motor]
-                if ((direct==0)||(direct==2))
-                    dirB_RU=0;
-                else
-                    dirB_RU=1;
-                break;
-
-            case 4:
-                // Set inA[motor]
-                if (direct <=1)
-                    dirA_RL=0;
-                else
-                    dirA_RL=1;
-                // Set inB[motor]
-                if ((direct==0)||(direct==2))
-                    dirB_RL=0;
-                else
-                    dirB_RL=1;
-                break;
-        }
-    }
-}
-
-void motor_stop(uint8_t id)
-{
-    switch(id) {
-        case 1:
-            dirA_LU=1;
-            dirB_LU=1;
-            break;
-
-        case 2:
-            dirA_LL=1;
-            dirB_LL=1;
-            break;
-
-        case 3:
-            dirA_RU=1;
-            dirB_RU=1;
-            break;
-
-        case 4:
-            dirA_RL=1;
-            dirB_RL=1;
-            break;
-
-        case 0:
-            dirA_LU=1;
-            dirB_LU=1;
-
-            dirA_LL=1;
-            dirB_LL=1;
-
-            dirA_RU=1;
-            dirB_RU=1;
-
-            dirA_RL=1;
-            dirB_RL=1;
-            break;
-    }
+    
+    do
+    {
+        /*
+        state=0;
+        //leg_left_upper.position_control(500);
+        if(leg_left_lower.position_control(500) ==2)
+            state++;
+        if(leg_right_upper.position_control(500) == 2)
+            state++;
+        if(leg_right_lower.position_control(500) == 2)
+            state++;
+        */
+        state = leg_left_upper.position_control(32);
+        pc.printf("state_lu %d\n",state);
+        state = leg_left_lower.position_control(32);
+        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);
+        pc.printf("state_rl %d\n",state);
+        state=0;
+    }while(state != 2);
+    pc.printf("[Finish test]\n");
 }
 
-
-
-uint8_t limit_motor(uint8_t id, uint8_t dirction)
-{
-    switch(id) {
-        case 1://Left Upper
-            if(~sw_LU_U && ~sw_LU_D) {
-                motor_set(id,dirction);
-            } else {
-                motor_stop(id);
-                return 0;
-            }
-            break;
-
-        case 2://Left Lowwer
-            if(~sw_LL_U && ~sw_LL_D) {
-                motor_set(id,dirction);
-            } else {
-                motor_stop(id);
-                return 0;
-            }
-            break;
-
-        case 3://Right Upper
-            if(~sw_RU_U && ~sw_RU_D) {
-                motor_set(id,dirction);
-            } else {
-                motor_stop(id);
-                return 0;
-            }
-            break;
-
-        case 4://Right Lowwer
-            if(~sw_RL_U && ~sw_RL_D) {
-                motor_set(id,dirction);
-            } else {
-                motor_stop(id);
-                return 0;
-            }
-            break;
-    }
-    return 1;//normally
-}
-
-
-uint8_t position_control(uint8_t id, uint16_t current, uint16_t target)
-{
-    //uint8_t state=0;
-    int16_t error = target-current;
-
-    pc.printf("error[%d]=%d\n",id,error);
-    if(error > MARGIN) {
-        if(limit_motor(id,1)==0 ) { //limit sens
-
-            pc.printf("motor[%d]=limit error\n",id);
-
-            return 2;
-        }
-
-
-    } else if(error < -MARGIN) {
-        if(limit_motor(id,2)==0 ) { //limit sens
-
-            pc.printf("motor[%d]=limit error\n",id);
-
-            return 2;
-        }
-
-    } else {    //in zone
-        motor_stop(2);
-
-        pc.printf("motor[%d]=complete\n",id);
-
-        return 1; //in zone complete
-    }
-
-    pc.printf("motor[%d]=in process\n",id);
-    return 0; //in process
-}
-
-void calibration(uint8_t id)
-{
-    switch(id) {
-        case 1:
-            pc.printf("motor[1] run up\n");
-            do {
-                motor_set(id,1);
-            } while(sw_LU_U ==0);
-            pc.printf("motor[1] stop up\n");
-            //wait_ms(500);
-            do {
-                motor_set(id,2);
-            } while(sw_LU_U);
-            motor_stop(id);
-            wait_ms(500);
-            pc.printf("motor[1] read position\n");
-            max_pos_LU = position_LU.read_u16();
-            pc.printf("max_pos_LU= %d\n",max_pos_LU);
-
-            pc.printf("motor[1] run down\n");
-            do {
-                motor_set(id,2);
-            } while(sw_LU_D==0) ;
-            pc.printf("motor[1] stop down\n");
-            do {
-                motor_set(id,1);
-            } while(sw_LU_D);
-            motor_stop(id);
-            wait_ms(500);
-            pc.printf("motor[1] read position\n");
-            min_pos_LU = position_LU.read_u16();
-            pc.printf("min_pos_LU= %d\n",min_pos_LU);
-            break;
-
-        case 2:
-
-            pc.printf("motor[2] run up\n");
-            do{
-                motor_set(id,1);
-            }while(sw_LL_U==0) ;
-            motor_stop(id);
-            wait_ms(500);
-            pc.printf("motor[2] stop up\n");
-            do {
-                motor_set(id,2);
-            } while(sw_LL_U );
-            motor_stop(id);
-            wait_ms(500);
-            max_pos_LL = position_LL.read_u16();
-            pc.printf("max_pos_LL= %d\n",max_pos_LL);
-            pc.printf("motor[2] run down\n");
-            do{
-                motor_set(id,2);
-            }while(sw_LL_D==0) ;
-            motor_stop(id);
-            wait_ms(500);
-            pc.printf("motor[2] stop down\n");
-            do {
-                motor_set(id,1);
-            } while(sw_LL_D);
-            motor_stop(id);
-            wait_ms(500);
-            min_pos_LL = position_LL.read_u16();
-            pc.printf("min_pos_LL= %d\n",min_pos_LL);
-            break;
-
-        case 3:
-            //    uint8_t count=1000;
-            pc.printf("motor[3] run up\n");
-            do {
-                motor_set(id,1);
-            }while(sw_RU_U==0);
-            motor_stop(id);
-            wait_ms(500);
-            pc.printf("motor[3] stop up\n");
-
-            do {
-                motor_set(id,2);
-
-            } while(sw_RU_U);
-            motor_stop(id);
-            wait_ms(500);
-            max_pos_RU = position_RU.read_u16();
-            pc.printf("max_pos_RU= %d\n",max_pos_RU);
-
-            pc.printf("motor[3] run down\n");
-
-            /*while(count >1) {
-                motor_set(id,2);
-                if(sw_RU_D)
-                {
-                    count--;
-                }
-            }*/
-            do {
-
-                motor_set(id,2);
-            }while(sw_RU_D==0);
-            motor_stop(id);
-            wait_ms(500);
-            pc.printf("motor[3] stop down\n");
-            do {
-                motor_set(id,1);
-            } while(sw_RU_D);
-            motor_stop(id);
-            wait_ms(500);
-            min_pos_RU = position_RU.read_u16();
-            pc.printf("min_pos_RU= %d\n",min_pos_RU);
-            break;
-
-        case 4:
-
-            pc.printf("motor[4] run up\n");
-            do {
-                motor_set(id,1);
-            }while(sw_RL_U==0);
-            motor_stop(id);
-            wait_ms(500);
-            pc.printf("motor[4] stop up\n");
-            do {
-                motor_set(id,2);
-            } while(sw_RL_U);
-            motor_stop(id);
-            wait_ms(500);
-            max_pos_RL = position_RL.read_u16();
-            pc.printf("max_pos_RL= %d\n",max_pos_RL);
-            pc.printf("motor[4] run down\n");
-            do{
-                motor_set(id,2);
-            }while(sw_RL_D==0);
-            motor_stop(id);
-            wait_ms(500);
-            pc.printf("motor[4] stop down\n");
-            do {
-                motor_set(id,1);
-            } while(sw_RL_D);
-            motor_stop(id);
-            wait_ms(500);
-            min_pos_RL = position_RL.read_u16();
-            pc.printf("min_pos_RL= %d\n",min_pos_RL);
-            break;
-    }
-
-}
-/*
-uint16_t convert(uint16_t data)
-{
-    uint16_t ans=0;
-
-    //ans =
-
-    return
-}
-
-    uint16_t scale(uint16_t data)
-{
-
-}
-*/
-
 void getCommand()
 {
     static uint8_t count =0;
@@ -657,7 +213,7 @@
 
     uint8_t status = pan_a.receiveCommunicatePacket(&packet);
     myled=0;
-    
+
 
 
     if(status == ANDANTE_ERRBIT_NONE) {
@@ -665,7 +221,7 @@
             count--;
         } else {
             count=0;
-            
+
         }
         pan_a.sendCommunicatePacket(&packet);
         //update command
@@ -679,6 +235,6 @@
         //stop robot
         count++;
         myled=1;
-     //   setSpeedControl(0.0);
+        //   setSpeedControl(0.0);
     }
 }
\ No newline at end of file