Diff: motion_control.cpp

Revision:

5:91d905f8bef7

Parent:

4:6509fec6a6fb

Child:

6:41d7cf11fdb1

--- a/motion_control.cpp	Sat Jul 18 05:52:56 2015 +0000
+++ b/motion_control.cpp	Sun Jul 19 17:38:21 2015 +0000
@@ -41,11 +41,10 @@
 {
     uint16_t current =Scale(_position.read_u16());
     //target = Scale(target);
-    if(target > scale || target < 0)
-    {
+    if(target > scale || target < 0) {
         return -3;
     }
-    
+
     error = target-current;
     if(error >scale || error < -scale) {
         // pc.printf("data error\n");
@@ -73,60 +72,25 @@
 
 void MOTION_CONTROL::calibration()
 {
-    /*
-    //pc.printf("motor[1] run up\n");
-    do {
-        if(_limit_up) {
-            motor.StopMotor();
-        } else {
-            motor.SetMotor(1);
-        }
-    } while(_limit_up);
-    motor.StopMotor();
-    //pc.printf("motor[1] stop up\n");
-    wait_ms(500);
-    do {
-        motor.SetMotor(2);
-    } while(_limit_up);
-    motor.StopMotor();
-    wait_ms(500);
-    //pc.printf("motor[1] read position\n");
-
-    MAX_POSITION = _position.read_u16();
-
-    //pc.printf("max_pos_LU= %d\n",max_pos_LU);
-    //pc.printf("motor[1] run down\n");
-
-    do {
-        if(_limit_down == 0) {
-            motor.StopMotor();
-        } else {
-            motor.SetMotor(2);
-        }
-    } while(_limit_down==0) ;
-    //pc.printf("motor[1] stop down\n");
-    do {
-        motor.SetMotor(1);
-    } while(_limit_down);
-    motor.StopMotor();
-    wait_ms(500);
-    //pc.printf("motor[1] read position\n");
-    MIN_POSITION = _position.read_u16();
-    //pc.printf("min_pos_LU= %d\n",min_pos_LU);
-    */
     int state=0;
+    uint32_t sum=0;
 
     do {
         state = limit_motor(1);
     } while(state==0);
-    //motor.StopMotor();
+
     do {
         state = limit_motor(2);
     } while(state!=1);
     wait_ms(200);
     motor.StopMotor();
     wait_ms(500);
-    MAX_POSITION = _position.read_u16();
+
+    sum=0;
+    for(int a=0; a<LOOP; a++) {
+        sum += _position.read_u16();
+    }
+    MAX_POSITION = sum/LOOP;
 
     do {
         state = limit_motor(2);
@@ -137,7 +101,12 @@
     } while(state!=0);
     wait_ms(100);
     motor.StopMotor();
-    MIN_POSITION = _position.read_u16();
+
+    sum=0;
+    for(int a=0; a<LOOP; a++) {
+        sum += _position.read_u16();
+    }
+    MIN_POSITION = sum/LOOP;
 }
 
 int MOTION_CONTROL::GetLimitUp()
@@ -169,6 +138,15 @@
     return MIN_POSITION;
 }
 
+void MOTION_CONTROL::SetMaxPosition(uint16_t value)
+{
+    MAX_POSITION = value;
+}
+void MOTION_CONTROL::SetMinPosition(uint16_t value)
+{
+    MIN_POSITION = value;
+}
+
 uint16_t MOTION_CONTROL::Scale(uint16_t data)
 {
     return ((float)(data-MIN_POSITION)/(MAX_POSITION - MIN_POSITION))*scale;