Diff: motor.cpp

Revision:

10:1df5a7a265e8

Parent:

9:5983c10d5f8e

Child:

11:25d26c72a2f7

--- a/motor.cpp	Mon Jul 17 14:02:22 2017 +0000
+++ b/motor.cpp	Thu Jul 27 07:25:47 2017 +0000
@@ -1,7 +1,8 @@
 #include "motor.h"
+//extern Serial pc_uart;
 
 void Motor::initialization(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3, uint32_t tickTime) {
-
+//pc_uart.printf("MOTOR INIT\n");
     MPh0 = new DigitalOut(_MPh0);
     MPh1 = new DigitalOut(_MPh1);
     MPh2 = new DigitalOut(_MPh2);
@@ -19,6 +20,9 @@
     NumSteps = MotorFullTurn;               // Default 1 turn
     motorCallback = NULL;
     itOnStop = true;
+            tuneTimings.reset();
+         tuneTimings.start();
+ toremove=false;
 }
 
 Motor::Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3) {
@@ -37,7 +41,7 @@
 void Motor::removeMotorCallback() {
     motorCallback = NULL;
 }
-
+/*
 void Motor::setMotorCallback(void (*mIT)()) {
     motorCallback = mIT;
     itOnStop = true;
@@ -45,20 +49,51 @@
 void Motor::setMotorCallback(void (*mIT)(), bool onStop) {
     motorCallback = mIT;
 }
+*/
+
+void Motor::setMotorCallback(void (*function)(void))
+{
+    setMotorCallback(function, true);
+}
+void Motor::setMotorCallback(void (*function)(void), bool onStop)
+{
+    _callback = function;
+    itOnStop = onStop;  
+}
+
+
+uint32_t Motor::getCalibration()
+{
+    return MotorFullTurn;
+}
 
 void Motor::setCalibration(uint32_t nbTicksforFullTurn) {
     MotorFullTurn = nbTicksforFullTurn;
 }
 
 void Motor::setDelayBtwTicks(uint32_t tickTime) {
+    if(MotorStepTime == tickTime) return;
+    toremove=true;
     MotorStepTime = tickTime;
     if(MotorStepTime < MOTOR_STEP_TIME_MIN)  MotorStepTime = MOTOR_STEP_TIME_MIN;
- 
+   //  pc_uart.printf("Change ticktime to %d %d\n\r",tickTime, MotorStepTime);
+   if(init) {
+        MotorSystemTick.detach();
+     //   init=false;
+        MotorSystemTick.attach_us(callback(this, &Motor::ProcessMotorStateMachine), MotorStepTime);
+ //     pc_uart.printf("Changed icktime to %d %d\n\r",tickTime, MotorStepTime);
+   }
 }
 
 void Motor::setSpeed(float sForOneTurn) {
     MotorStepTime = 1000*sForOneTurn / MotorFullTurn;
     if(MotorStepTime < MOTOR_STEP_TIME_MIN)  MotorStepTime = MOTOR_STEP_TIME_MIN;
+    if(init) {
+        
+        MotorSystemTick.detach();
+        MotorSystemTick.attach_us(callback(this, &Motor::ProcessMotorStateMachine), MotorStepTime);
+    }
+    //init=false;
 }
 
 void Motor::Start() {
@@ -105,7 +140,7 @@
 }
 
 void Motor::SetCommand(MotorCommand cmd) {
-    if(init)   MotorSystemTick.attach_us(this, &Motor::ProcessMotorStateMachine, MotorStepTime);
+    if(init)   MotorSystemTick.attach_us(callback(this, &Motor::ProcessMotorStateMachine), MotorStepTime);
     init = false;
     command=cmd;
 }
@@ -126,23 +161,28 @@
 
 void    Motor::RightMotor()    // Move the Motor one step Right
 {
-   static const   int RPh0[8] = {1, 1, 0, 0, 0, 0, 0, 1};
+    static const   int RPh0[8] =  {1, 1, 0, 0, 0, 0, 0, 1};
     static const   int RPh1[8] = {0, 1, 1, 1, 0, 0, 0, 0};
     static const   int RPh2[8] = {0, 0, 0, 1, 1, 1, 0, 0};
     static const   int RPh3[8] = {0, 0, 0, 0, 0, 1, 1, 1};
     *MPh0 = RPh0[MotorIndex];  *MPh1 = RPh1[MotorIndex];  *MPh2 = RPh2[MotorIndex];  *MPh3 = RPh3[MotorIndex];
+ //   pc_uart.printf("   STEP: %d, %d.%d.%d.%d\n\r",MotorIndex,RPh0[MotorIndex], RPh1[MotorIndex],RPh2[MotorIndex], RPh3[MotorIndex]);
     if (MotorIndex<7) MotorIndex++;
     else    MotorIndex = 0;
 }
 void    Motor::LeftMotor()    // Move the Motor one step Right
 {
-    static const   int LPh0[8] = { 1, 0, 0, 0, 0, 0, 1, 1};
-    static const   int LPh1[8] = { 0, 0, 0, 0, 1, 1, 1, 0};
-    static const   int LPh2[8] = { 0, 0, 1, 1, 1, 0, 0, 0};
-    static const   int LPh3[8] = { 1, 1, 1, 0, 0, 0, 0, 0};
+    static const   int LPh0[8] =  {1, 1, 0, 0, 0, 0, 0, 1};
+    static const   int LPh1[8] = {0, 1, 1, 1, 0, 0, 0, 0};
+    static const   int LPh2[8] = {0, 0, 0, 1, 1, 1, 0, 0};
+    static const   int LPh3[8] = {0, 0, 0, 0, 0, 1, 1, 1};
+//   static const   int LPh0[8] = { 1, 0, 0, 0, 0, 0, 1, 1};
+ //   static const   int LPh1[8] = { 0, 0, 0, 0, 1, 1, 1, 0};
+  //  static const   int LPh2[8] = { 0, 0, 1, 1, 1, 0, 0, 0};
+   // static const   int LPh3[8] = { 1, 1, 1, 0, 0, 0, 0, 0};
    *MPh0 = LPh0[MotorIndex];  *MPh1 = LPh1[MotorIndex];  *MPh2 = LPh2[MotorIndex];  *MPh3 = LPh3[MotorIndex];
-    if (MotorIndex<7) MotorIndex++;
-    else    MotorIndex = 0;
+    if (MotorIndex>0) MotorIndex--;
+    else    MotorIndex = 7;
 }
 
 void    Motor::RunMotor()     // Move the Motor in the user direction
@@ -153,7 +193,20 @@
 
 
 void Motor::ProcessMotorStateMachine()
-{        
+{        /*
+static int count = 0;
+static uint32_t vmax=0;
+if(state!=Motor_IDLE && toremove) {
+   uint32_t v = tuneTimings.read_us();
+   count = count+1;
+   vmax = vmax+v-last;
+last=v;
+if(count==500) {
+    printf("MOTOR TICK : %d - %d - => id %d\n\r",v,vmax/500,MotorIndex);
+    count = 0;
+    vmax=0;
+}
+}*/
         if (state==Motor_IDLE) {
             if (command == MOTOR_start) {
                 // Start the Motor
@@ -179,11 +232,12 @@
                 }
             else if ((command == MOTOR_stop)||(NumSteps==0)) {
                 StopMotor();
-                state = Motor_IDLE;
-                    if(command != MOTOR_stop || itOnStop == true) {
+                     if(state != Motor_IDLE ){//|| itOnStop == true) {
                         if(motorCallback != NULL) motorCallback();
+                        _callback.call();
                     }
-                }
+                state = Motor_IDLE;
+               }
             command = MOTOR_nop;
             }
         else if (state==Motor_PAUSE) {