Diff: motion_control.cpp

Revision:

0:77ab14788110

Child:

1:5b313fd2ca6f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motion_control.cpp	Fri Jul 17 12:07:01 2015 +0000
@@ -0,0 +1,98 @@
+#include "mbed.h"
+#include "motion_control.h"
+#include "motor_relay.h"
+
+//int16_t MOTION_CONTROL::error=0;
+//int16_t MOTION_CONTROL::MARGIN=500;
+
+
+MOTION_CONTROL::MOTION_CONTROL(PinName dirA, PinName dirB, PinName limitUp, PinName limitDown, PinName vr): _limit_up(limitUp), _limit_down(limitDown), _position(vr)
+{
+    MOTOR_RELAY *motor = new MOTOR_RELAY(dirA,dirB);
+    error=0;
+    MARGIN=500;
+}
+
+
+int8_t MOTION_CONTROL::limit_motor(uint8_t dirction)
+{
+    if(_limit_up) {
+        motor->StopMotor();
+        return 1;
+    } else {
+        if(_limit_down) {
+            motor->StopMotor();
+            return -1;
+        } else {
+            motor->SetMotor(dirction);
+            return 0;//Normally
+        }
+    }
+}
+
+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);
+        }
+    } 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
+}
+
+void MOTION_CONTROL::calibration()
+{
+    //pc.printf("motor[1] run up\n");
+    do {
+        if(_limit_up == 0) {
+            motor->StopMotor();
+        } else {
+            motor->SetMotor(1);
+        }
+    } while(_limit_up ==0);
+
+    //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);
+
+}
\ No newline at end of file