BE@R lab / motion_control

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   motor_relay

Dependents:   dog_V3_3_testmotor

Diff: motion_control.cpp

Revision:
1:5b313fd2ca6f
Parent:
0:77ab14788110
Child:
3:4fa191f2194d
--- a/motion_control.cpp	Fri Jul 17 12:07:01 2015 +0000
+++ b/motion_control.cpp	Fri Jul 17 14:04:47 2015 +0000
@@ -33,24 +33,29 @@
 int8_t MOTION_CONTROL::position_control(uint16_t current, uint16_t target)
 {
     error = target-current;
-
-    if(error > MARGIN) {
-        if(limit_motor(1)==0 ) { //limit sens
-            //pc.printf("motor[%d]=limit error\n",id);
-            return limit_motor(1);
+    if(error >scale || error < -scale) {
+        //pc.printf("data error\n");
+        
+    } else {
+        if(error > MARGIN) {
+            if(limit_motor(1)==0 ) { //limit sens
+                //pc.printf("motor[%d]=limit error\n",id);
+                return limit_motor(1);
+            }
+        } else if(error < -MARGIN) {
+            if(limit_motor(2)!=0 ) { //limit sens
+                //pc.printf("motor[%d]=limit error\n",id);
+                return limit_motor(2);
+            }
+        } else {    //in zone
+            motor->StopMotor();
+            //pc.printf("motor[%d]=complete\n",id);
+            return 2; //in zone complete
         }
-    } else if(error < -MARGIN) {
-        if(limit_motor(2)!=0 ) { //limit sens
-            //pc.printf("motor[%d]=limit error\n",id);
-            return limit_motor(2);
-        }
-    } else {    //in zone
-        motor->StopMotor();
-        //pc.printf("motor[%d]=complete\n",id);
-        return 1; //in zone complete
+
+        //pc.printf("motor[%d]=in process\n",id);
+        return 0; //in process
     }
-    //pc.printf("motor[%d]=in process\n",id);
-    return 0; //in process
 }
 
 void MOTION_CONTROL::calibration()