Diff: main.cpp

Revision:

9:0de6ce956985

Parent:

8:8d30ce9a9463

Child:

10:53cb691e22bf

--- a/main.cpp	Fri Jul 17 12:07:19 2015 +0000
+++ b/main.cpp	Fri Jul 17 14:05:01 2015 +0000
@@ -4,8 +4,6 @@
 #include "communication.h"
 #include "protocol.h"
 
-#include "motor_relay.h"
-
 //set frequancy unit in Hz
 #define F_UPDATE 10.0f
 #define TIMER_UPDATE 1.0f/F_UPDATE
@@ -14,7 +12,6 @@
 #define TIMEOUT_RESPONE_COMMAND 5
 
 //define pin class
-/*
 DigitalOut dirA_LU(INA_L_U);
 DigitalOut dirB_LU(INB_L_U);
 
@@ -26,13 +23,7 @@
 
 DigitalOut dirA_RL(INA_R_L);
 DigitalOut dirB_RL(INB_R_L);
-*/
-MOTOR_RELAY leg_left_upper(INA_L_U,INB_L_U);
-MOTOR_RELAY leg_left_lower(INA_L_L,INB_L_L);
-MOTOR_RELAY leg_right_upper(INA_R_U,INB_R_U);
-MOTOR_RELAY leg_right_lower(INA_R_L,INB_R_L);
 
-/*
 DigitalIn sw_LU_U(LIMIT_LU_U,PullUp);
 DigitalIn sw_LU_D(LIMIT_LU_D,PullUp);
 
@@ -44,7 +35,6 @@
 
 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);
@@ -98,7 +88,7 @@
     uint16_t vr_lu,vr_ru;
     uint16_t vr_ll,vr_rl;
     pc.printf("wait\n");
-    //motor_stop(0);
+    motor_stop(0);
     //  wait(10);
     /*
         while(1) {
@@ -114,12 +104,6 @@
             wait(1);
         }
     */
-    while(1) {
-        leg_left_upper.SetMotor(1);
-        leg_left_lower.SetMotor(1);
-        leg_right_upper.SetMotor(1);
-        leg_right_lower.SetMotor(1);
-    }
 
     /*
         while(1) {
@@ -250,17 +234,17 @@
     }
     */
 
-    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");
-    }
+      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");
+       }
 
     Update_command.attach(&getCommand,TIMER_UPDATE);
 
@@ -317,7 +301,7 @@
 }
 
 
-/*
+
 void motor_set(uint8_t id, uint8_t direct)
 {
     //direct: Should be between 0 and 3, with the following result
@@ -424,9 +408,9 @@
             break;
     }
 }
-*/
+
 
-/*
+
 uint8_t limit_motor(uint8_t id, uint8_t dirction)
 {
     switch(id) {
@@ -542,9 +526,9 @@
         case 2:
 
             pc.printf("motor[2] run up\n");
-            do {
+            do{
                 motor_set(id,1);
-            } while(sw_LL_U==0) ;
+            }while(sw_LL_U==0) ;
             motor_stop(id);
             wait_ms(500);
             pc.printf("motor[2] stop up\n");
@@ -556,9 +540,9 @@
             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 {
+            do{
                 motor_set(id,2);
-            } while(sw_LL_D==0) ;
+            }while(sw_LL_D==0) ;
             motor_stop(id);
             wait_ms(500);
             pc.printf("motor[2] stop down\n");
@@ -576,13 +560,14 @@
             pc.printf("motor[3] run up\n");
             do {
                 motor_set(id,1);
-            } while(sw_RU_U==0);
+            }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);
@@ -591,13 +576,20 @@
 
             pc.printf("motor[3] run down\n");
 
-            do {
+            /*while(count >1) {
                 motor_set(id,2);
-            } while(sw_RU_D==0);
+                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);
@@ -612,7 +604,7 @@
             pc.printf("motor[4] run up\n");
             do {
                 motor_set(id,1);
-            } while(sw_RL_U==0);
+            }while(sw_RL_U==0);
             motor_stop(id);
             wait_ms(500);
             pc.printf("motor[4] stop up\n");
@@ -624,9 +616,9 @@
             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 {
+            do{
                 motor_set(id,2);
-            } while(sw_RL_D==0);
+            }while(sw_RL_D==0);
             motor_stop(id);
             wait_ms(500);
             pc.printf("motor[4] stop down\n");
@@ -641,10 +633,6 @@
     }
 
 }
-
-*/
-
-
 /*
 uint16_t convert(uint16_t data)
 {
@@ -669,7 +657,7 @@
 
     uint8_t status = pan_a.receiveCommunicatePacket(&packet);
     myled=0;
-
+    
 
 
     if(status == ANDANTE_ERRBIT_NONE) {
@@ -677,7 +665,7 @@
             count--;
         } else {
             count=0;
-
+            
         }
         pan_a.sendCommunicatePacket(&packet);
         //update command
@@ -691,6 +679,6 @@
         //stop robot
         count++;
         myled=1;
-        //   setSpeedControl(0.0);
+     //   setSpeedControl(0.0);
     }
 }
\ No newline at end of file