Diff: lin_step_mtr.cpp

Revision:

0:54c5be5f26f4

Child:

1:757a52db1604

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lin_step_mtr.cpp	Sun Apr 19 22:16:24 2020 +0000
@@ -0,0 +1,107 @@
+// Code for the lin_step_mtr driver
+
+#include "lin_step_mtr.h"
+#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)
+{
+    mtr_ctrl = 0x0;
+    speed = floor((double)DEFAULT_SPEED * 10 / 3);
+    dir = CW;
+    
+    stop_mtr = true;
+    terminate = false;
+    
+    
+    
+    
+    
+    cur_state = STOP_MOTOR;
+}
+
+
+float LinStepMtr::get_speed()
+{
+    return (float) speed * 3 / 10;
+}
+
+LinStepMtr::Direction LinStepMtr::get_dir()
+{
+    return dir;   
+}
+
+void LinStepMtr::rotate()
+{
+    while(!terminate) {
+        if(!stop_mtr){
+            switch(dir) {
+                case CW:
+                    mtr_ctrl = ++cur_step;
+                    rev_cnt +=.005;
+                    break;
+                case CCW:
+                    mtr_ctrl = --cur_step;
+                    rev_cnt-=.005;
+                    break;
+            }
+            Thread::wait(30 / speed);   
+        } else {
+            mtr_ctrl = STOP;   
+            Thread::yield();
+        }
+    }   
+}
+
+// Private Step Class functions
+/*
+LinStepMtr::Step::Step() : cur_step(ONE)
+{
+            pc.printf("\n\nCalled Constructor\n  cur_step: %x\n",cur_step);  
+            
+            pc.printf("Step_Num val:\n  ONE = %x\n  TWO = %x\n  THREE = %x\n  FOUR = %x\n\n", ONE, TWO, THREE, FOUR); 
+};
+*/
+LinStepMtr::Step_Num LinStepMtr::Step::get_cur_step()
+{
+        return cur_step;
+}
+        
+LinStepMtr::Step_Num LinStepMtr::Step::operator++()
+{    
+    switch(cur_step){
+        case ONE:
+            cur_step=TWO;
+            break;
+        case TWO:
+            cur_step = THREE;
+            break;
+        case THREE:
+            cur_step = FOUR;
+            break;
+        case FOUR:
+            cur_step = ONE; 
+            break;      
+    }
+    return cur_step;
+}
+
+LinStepMtr::Step_Num LinStepMtr::Step::operator--()
+{
+    switch(cur_step){
+        case ONE:
+            cur_step=FOUR;
+            break;
+        case TWO:
+            cur_step = ONE;
+            break;
+        case THREE:
+            cur_step = TWO;
+            break;
+        case FOUR:
+            cur_step = THREE; 
+            break;      
+    }
+    return cur_step;
+}
\ No newline at end of file