Diff: motion_control.cpp

Revision:

4:6509fec6a6fb

Parent:

3:4fa191f2194d

Child:

5:91d905f8bef7

--- a/motion_control.cpp	Fri Jul 17 15:35:09 2015 +0000
+++ b/motion_control.cpp	Sat Jul 18 05:52:56 2015 +0000
@@ -2,40 +2,54 @@
 #include "motion_control.h"
 #include "motor_relay.h"
 
-//int16_t MOTION_CONTROL::error=0;
-//int16_t MARGIN;
 
-
-MOTION_CONTROL::MOTION_CONTROL(PinName dirA, PinName dirB, PinName limitUp, PinName limitDown, PinName vr): _limit_up(limitUp), _limit_down(limitDown), _position(vr)
+MOTION_CONTROL::MOTION_CONTROL(PinName dirA, PinName dirB, PinName limitUp, PinName limitDown, PinName vr): motor(dirA,dirB), _limit_up(limitUp), _limit_down(limitDown), _position(vr)
 {
-    MOTOR_RELAY *motor = new MOTOR_RELAY(dirA,dirB);
+    //MOTOR_RELAY *motor = new MOTOR_RELAY(dirA,dirB);
     error=0;
-    MARGIN=500;
+    MARGIN=1;
+    //scale =1000;
 }
 
 
-int8_t MOTION_CONTROL::limit_motor(uint8_t dirction)
+int MOTION_CONTROL::limit_motor(uint8_t dirction)
 {
+
     if(_limit_up) {
-        motor->StopMotor();
+        //while(_limit_up!=0) {
+        motor.SetMotor(2);
+        //}
         return 1;
+    } else if(_limit_down) {
+
+        motor.SetMotor(1);
+
+        // }
+        //motor.StopMotor();
+        return -1;
     } else {
-        if(_limit_down) {
-            motor->StopMotor();
-            return -1;
-        } else {
-            motor->SetMotor(dirction);
-            return 0;//Normally
-        }
+        motor.SetMotor(dirction);
+        return 0;//Normally
+
     }
+
+//motor.SetMotor(dirction);
+//return 0;
 }
 
-int8_t MOTION_CONTROL::position_control(uint16_t current, uint16_t target)
+int8_t MOTION_CONTROL::position_control(uint16_t target)
 {
+    uint16_t current =Scale(_position.read_u16());
+    //target = Scale(target);
+    if(target > scale || target < 0)
+    {
+        return -3;
+    }
+    
     error = target-current;
     if(error >scale || error < -scale) {
-        //pc.printf("data error\n");
-
+        // pc.printf("data error\n");
+        return -2;
     } else {
         if(error > MARGIN) {
             if(limit_motor(1)==0 ) { //limit sens
@@ -48,33 +62,33 @@
                 return limit_motor(2);
             }
         } else {    //in zone
-            motor->StopMotor();
+            motor.StopMotor();
             //pc.printf("motor[%d]=complete\n",id);
             return 2; //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()
 {
+    /*
     //pc.printf("motor[1] run up\n");
     do {
-        if(_limit_up == 0) {
-            motor->StopMotor();
+        if(_limit_up) {
+            motor.StopMotor();
         } else {
-            motor->SetMotor(1);
+            motor.SetMotor(1);
         }
-    } while(_limit_up ==0);
-
+    } while(_limit_up);
+    motor.StopMotor();
     //pc.printf("motor[1] stop up\n");
     wait_ms(500);
     do {
-        motor->SetMotor(2);
+        motor.SetMotor(2);
     } while(_limit_up);
-    motor->StopMotor();
+    motor.StopMotor();
     wait_ms(500);
     //pc.printf("motor[1] read position\n");
 
@@ -85,21 +99,45 @@
 
     do {
         if(_limit_down == 0) {
-            motor->StopMotor();
+            motor.StopMotor();
         } else {
-            motor->SetMotor(2);
+            motor.SetMotor(2);
         }
     } while(_limit_down==0) ;
     //pc.printf("motor[1] stop down\n");
     do {
-        motor->SetMotor(1);
+        motor.SetMotor(1);
     } while(_limit_down);
-    motor->StopMotor();
+    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;
 
+    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();
+
+    do {
+        state = limit_motor(2);
+    } while(state==0);
+    //motor.StopMotor();
+    do {
+        state = limit_motor(1);
+    } while(state!=0);
+    wait_ms(100);
+    motor.StopMotor();
+    MIN_POSITION = _position.read_u16();
 }
 
 int MOTION_CONTROL::GetLimitUp()
@@ -130,3 +168,23 @@
 {
     return MIN_POSITION;
 }
+
+uint16_t MOTION_CONTROL::Scale(uint16_t data)
+{
+    return ((float)(data-MIN_POSITION)/(MAX_POSITION - MIN_POSITION))*scale;
+}
+
+uint16_t MOTION_CONTROL::GetAnalog()
+{
+    return _position.read_u16();
+}
+
+uint16_t MOTION_CONTROL::GetPosition()
+{
+    return Scale(_position.read_u16());
+}
+
+void MOTION_CONTROL::stop()
+{
+    motor.StopMotor();
+}
\ No newline at end of file