Diff: Stepper_Motor_X27168.cpp

Revision:

1:e7c0920184e4

Parent:

0:c346170974bc

--- a/Stepper_Motor_X27168.cpp	Tue Oct 20 00:36:06 2015 +0000
+++ b/Stepper_Motor_X27168.cpp	Mon May 02 15:03:35 2016 +0000
@@ -6,6 +6,7 @@
         motor_control = 0x0;
         max_position = MAX_POS;
         speed = DEFAULT_SPEED;
+        max_speed = 1000;
         cur_state = STOP_MOTOR;
         cur_position = 0;
 }
@@ -21,10 +22,18 @@
     speed = s;
 }
 
+void StepperMotor_X27168::set_max_speed(float s) {
+    max_speed = s;
+    }
+
 int StepperMotor_X27168::get_speed() {
     return speed;
 }
 
+int StepperMotor_X27168::get_position() {
+    return cur_position;
+}
+
 void StepperMotor_X27168::set_max_position(int p) {
     if(p<MAX_POS) {
         max_position = p;
@@ -93,6 +102,42 @@
     step(2);
 }
 
+void StepperMotor_X27168::set_position_dynamic(float pos){
+     if(pos > max_position)
+        pos = max_position;
+    else if(pos < 0)
+        pos = 0;
+    
+
+    int delta = abs(pos - cur_position);
+    speed = 100;
+    
+    set_speed(speed);
+    
+   
+    while(cur_position < pos) {
+        if(abs(pos - cur_position) > delta/2 && speed < max_speed)
+            speed +=50;
+        if(abs(pos - cur_position) <= delta/2&& abs(pos - cur_position) <= 25 && speed >100)
+            speed -= 50;
+   
+        step(0);
+    }
+    while(cur_position > pos) {
+        if(abs(pos - cur_position) > delta/2 && speed < max_speed)
+            speed +=50;
+        if(abs(pos - cur_position) <= delta/2 && abs(pos - cur_position) <= 25 && speed >100)
+            speed -= 50;
+
+        step(1);
+    }
+    
+    step(2);
+    
+    
+    
+    }
+
 void StepperMotor_X27168::angle_position(float angle) {
-    step_position(int(angle*2));
+    set_position_dynamic(int(angle*2));
 }
\ No newline at end of file