Diff: lin_step_mtr.cpp

Revision:

1:757a52db1604

Parent:

0:54c5be5f26f4

Child:

2:6c324fded7c1

--- a/lin_step_mtr.cpp	Sun Apr 19 22:16:24 2020 +0000
+++ b/lin_step_mtr.cpp	Mon Apr 20 02:52:30 2020 +0000
@@ -10,17 +10,34 @@
     mtr_ctrl = 0x0;
     speed = floor((double)DEFAULT_SPEED * 10 / 3);
     dir = CW;
+    cur_step = ONE;
+    
+    stop_mtr = true;
+    terminate = false;
+    rotate_th = NULL;
+    
+    cur_state = STOP_MOTOR;
+}
+
+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 = 0x0;
+    speed = floor((double)DEFAULT_SPEED * 10 / 3);
+    dir = CW;
+    cur_step = ONE;
     
     stop_mtr = true;
     terminate = false;
     
-    
-    
-    
-    
     cur_state = STOP_MOTOR;
 }
 
+LinStepMtr::~LinStepMtr()
+{
+    this->end();
+}
+
 
 float LinStepMtr::get_speed()
 {
@@ -32,8 +49,52 @@
     return dir;   
 }
 
+void LinStepMtr::init(double rpm, Direction d)
+{
+    terminate = false;
+    speed = floor(rpm * 10 / 3);
+    dir = d;
+    if (!rotate_th) {
+        rotate_th = new Thread(LinStepMtr::rotate_help, this); 
+    }
+}
+
+void LinStepMtr::end() {
+    terminate = true;
+    Thread::wait(100);
+    if (rotate_th) {
+        delete rotate_th;
+        rotate_th = NULL;   
+    }
+}
+
+void LinStepMtr::start() {
+    mtr_stp = false;   
+}
+
+void LinStepMtr::stop() {
+    mtr_stop = true;   
+}
+
+void LinStepMtr::rotate_help(void const *args)
+{
+    LinStepMtr *instPtr = static_cast<LinStepMtr *>(const_cast<void *>(args));
+    
+    instPtr->rotate();
+}
+
 void LinStepMtr::rotate()
 {
+    pc.printf("Called rotate()\n");
+ /*
+    while(1) {
+        
+        mtr_ctrl = ++cur_step;
+        rev_cnt +=.005;
+        wait(pause);
+    }
+*/
+ 
     while(!terminate) {
         if(!stop_mtr){
             switch(dir) {
@@ -46,12 +107,13 @@
                     rev_cnt-=.005;
                     break;
             }
-            Thread::wait(30 / speed);   
+           wait(1/ (float) speed);   
         } else {
             mtr_ctrl = STOP;   
             Thread::yield();
         }
     }   
+
 }
 
 // Private Step Class functions
@@ -84,6 +146,7 @@
             cur_step = ONE; 
             break;      
     }
+    //pc.printf("  CUR_STEP = %x\n",cur_step);
     return cur_step;
 }
 
@@ -104,4 +167,9 @@
             break;      
     }
     return cur_step;
+}
+
+void LinStepMtr::Step::operator=(LinStepMtr::Step_Num s)
+{
+    cur_step = s;   
 }
\ No newline at end of file