Diff: motion_control.cpp

Revision:

7:94518aee126b

Parent:

6:41d7cf11fdb1

Child:

8:371686666f4d

Child:

9:e3895130e56f

--- a/motion_control.cpp	Mon Jul 20 09:15:28 2015 +0000
+++ b/motion_control.cpp	Fri Jul 24 21:15:45 2015 +0000
@@ -8,6 +8,8 @@
     //MOTOR_RELAY *motor = new MOTOR_RELAY(dirA,dirB);
     error=0;
     MARGIN=1;
+    mode_limit=0;
+    OFFSET_MAX_POSITION=0;
     //scale =1000;
 }
 
@@ -16,16 +18,24 @@
 {
 
     if(_limit_up) {
-        //while(_limit_up!=0) {
-        motor.SetMotor(2);
-        //}
-        return 1;
+
+        if(mode_limit == 0) {
+            motor.SetMotor(2);
+            return 1;
+        } else if(mode_limit == 1) {
+
+            if(Scale(_position.read_u16()) > 1) {
+                motor.SetMotor(dirction);
+            } else {
+                motor.StopMotor();
+            }
+        } else {
+            motor.SetMotor(dirction);
+        }
+        return 0;//Normally
     } else if(_limit_down) {
-
         motor.SetMotor(1);
-
         // }
-        //motor.StopMotor();
         return -1;
     } else {
         motor.SetMotor(dirction);
@@ -72,6 +82,8 @@
 
 void MOTION_CONTROL::calibration()
 {
+    uint8_t buff =GetMode();
+    SetMode(0);
     int state=0;
     uint32_t sum=0;
 
@@ -107,6 +119,8 @@
         sum += _position.read_u16();
     }
     MIN_POSITION = sum/LOOP;
+    
+    SetMode(buff);
 }
 
 int MOTION_CONTROL::GetLimitUp()
@@ -149,7 +163,7 @@
 
 uint16_t MOTION_CONTROL::Scale(uint16_t data)
 {
-    return ((float)(data-MIN_POSITION)/(MAX_POSITION - MIN_POSITION))*scale;
+    return ((float)(data-MIN_POSITION)/((MAX_POSITION+OFFSET_MAX_POSITION) - MIN_POSITION))*scale;
 }
 
 uint16_t MOTION_CONTROL::GetAnalog()
@@ -165,4 +179,22 @@
 void MOTION_CONTROL::stop()
 {
     motor.StopMotor();
+}
+
+void MOTION_CONTROL::SetMode(uint8_t mode)
+{
+    mode_limit = mode;
+}
+uint8_t MOTION_CONTROL::GetMode()
+{
+    return mode_limit;
+}
+
+void MOTION_CONTROL::SetOffset(uint16_t value)
+{
+    OFFSET_MAX_POSITION = value;
+}
+uint16_t MOTION_CONTROL::GetOffset()
+{
+    return OFFSET_MAX_POSITION;
 }
\ No newline at end of file