Diff: main.cpp

Revision:

1:768d359e9d96

Parent:

0:2433ddae2772

Child:

2:264ad36f1ad4

Child:

3:bb5fbe510fa5

--- a/main.cpp	Wed Jul 15 05:07:26 2015 +0000
+++ b/main.cpp	Wed Jul 15 07:54:46 2015 +0000
@@ -58,6 +58,8 @@
 uint16_t max_pos_RL= 51000;
 uint16_t min_pos_RL= 11000;
 
+uint16_t offset_pos =1000;
+
 int16_t MARGIN = 500;
 
 //Main function
@@ -150,17 +152,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");
+    //   }
 
 
     uint8_t count=0;
@@ -171,19 +173,19 @@
         do {
             state=0;
 
-            if(position_control(1,position_LU.read_u16(),max_pos_LU) == 1) {
+            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) == 1) {
+            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) == 1) {
+            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) == 1) {
+            if(position_control(4,position_RL.read_u16(),max_pos_RL-offset_pos) == 1) {
                 state++;
             }
 
@@ -193,19 +195,19 @@
         do {
             state=0;
 
-            if(position_control(1,position_LU.read_u16(),min_pos_LU) == 1) {
+            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) == 1) {
+            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) == 1) {
+            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) == 1) {
+            if(position_control(4,position_RL.read_u16(),min_pos_RL+offset_pos) == 1) {
                 state++;
             }
 
@@ -372,8 +374,6 @@
 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);
@@ -481,7 +481,7 @@
             motor_stop(id);
             wait_ms(500);
             pc.printf("motor[3] stop up\n");
-            
+
             do {
                 motor_set(id,2);
 
@@ -492,7 +492,7 @@
             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)
@@ -500,11 +500,10 @@
                     count--;
                 }
             }*/
-            while(sw_RU_D)
-            {
-                
+            while(sw_RU_D) {
+
                 motor_set(id,2);
-                }
+            }
             motor_stop(id);
             wait_ms(500);
             pc.printf("motor[3] stop down\n");
@@ -550,4 +549,19 @@
             break;
     }
 
-}
\ No newline at end of file
+}
+/*
+uint16_t convert(uint16_t data)
+{
+    uint16_t ans=0;
+
+    //ans =
+
+    return
+}
+
+    uint16_t scale(uint16_t data)
+{
+    
+}
+*/
\ No newline at end of file