Diff: lin_step_mtr.cpp

Revision:

3:2138b69ee3bd

Parent:

2:6c324fded7c1

Child:

4:1dc350268172

--- a/lin_step_mtr.cpp	Mon Apr 20 02:54:17 2020 +0000
+++ b/lin_step_mtr.cpp	Mon Apr 20 03:54:54 2020 +0000
@@ -4,11 +4,12 @@
 #include "debug.h"
 
 //Construtor
-LinStepMtr::LinStepMtr(PinName A_f, PinName A_r, PinName B_f, PinName B_r, int m_speed=MAX_SPEED)
-        :mtr_ctrl(B_r, A_r, B_f,A_f), max_speed((m_speed > MAX_SPEED) ? MAX_SPEED:m_speed)
+LinStepMtr::LinStepMtr(PinName A_f, PinName A_r, PinName B_f, PinName B_r, int m_rpm=MAX_RPM)
+        :mtr_ctrl(B_r, A_r, B_f,A_f), max_speed((m_rpm > MAX_RPM) ? floor((float)MAX_RPM * 10/3):floor((float)m_rpm * 10/3)),
+        max_rpm(m_rpm)
 {
     mtr_ctrl = 0x0;
-    speed = floor((double)DEFAULT_SPEED * 10 / 3);
+    speed = floor((float)DEFAULT_RPM * 10 / 3);
     dir = CW;
     cur_step = ONE;
     
@@ -20,10 +21,10 @@
 }
 
 LinStepMtr::LinStepMtr(PinName A_f, PinName A_r, PinName B_f, PinName B_r)
-    :mtr_ctrl(B_r,A_r,B_f,A_f), max_speed(MAX_SPEED)
+    :mtr_ctrl(B_r,A_r,B_f,A_f), max_speed(floor((float)MAX_RPM * 10/3)), max_rpm(MAX_RPM)
 {
     mtr_ctrl = 0x0;
-    speed = floor((double)DEFAULT_SPEED * 10 / 3);
+    speed = floor((double)DEFAULT_RPM * 10 / 3);
     dir = CW;
     cur_step = ONE;
     
@@ -69,11 +70,14 @@
 }
 
 void LinStepMtr::start() {
-    stop_mtr = false;   
+    stop_mtr = false;
+    spin_up();   
 }
 
 void LinStepMtr::stop() {
+    int s = spin_down();
     stop_mtr = true;   
+    speed = s;
 }
 
 void LinStepMtr::rotate_help(void const *args)
@@ -83,6 +87,26 @@
     instPtr->rotate();
 }
 
+void LinStepMtr::change_dir(Direction d)
+{
+    if(dir != d) {
+        if(stop_mtr) {
+            dir = d;
+        } else {
+            int s = spin_down();
+            stop_mtr = true;
+            dir = d;
+            stop_mtr = false;
+            spin_up(s);   
+        }
+    }
+}
+
+void LinStepMtr::change_speed(float rpm)
+{
+    speed = floor(rpm * 10 / 3);
+}
+
 void LinStepMtr::rotate()
 {
     pc.printf("Called rotate()\n");
@@ -116,6 +140,42 @@
 
 }
 
+#define SPIN_INCR 83 // steps
+#define SPIN_WAIT 10 // ms
+void LinStepMtr::spin_up(float rpm) {
+    int end_speed;
+    
+    if(rpm == -1) {
+        end_speed = speed;
+    } else {
+        end_speed = (rpm > max_rpm) ? max_speed:floor(rpm * 10 / 3);
+    }
+    for(int i = min_speed; i < end_speed-SPIN_INCR; i+=SPIN_INCR){
+        speed = i;
+        Thread::wait(SPIN_WAIT);
+    }
+    speed = end_speed;
+    
+}
+
+int LinStepMtr::spin_down(float rpm) {
+    int end_speed;
+    int s = speed;
+    
+    if(rpm == -1) {
+        end_speed = min_speed;
+    } else {
+        end_speed = (rpm < min_rpm) ? min_speed:floor(rpm * 10 / 3);
+    }
+    
+    for(int i = speed; i > end_speed+SPIN_INCR; i-=SPIN_INCR){
+        speed = i;
+        Thread::wait(SPIN_WAIT);
+    }
+    speed = end_speed;
+    return s;
+}
+
 // Private Step Class functions
 /*
 LinStepMtr::Step::Step() : cur_step(ONE)