Revision 0:ebda1df8a43e, committed 2020-06-13

Comitter:

missa_sami

Date:

Sat Jun 13 14:16:45 2020 +0000

Commit message:

kiosque 1

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorLib/motor.cpp	Sat Jun 13 14:16:45 2020 +0000
@@ -0,0 +1,248 @@
+#include "motor.h"
+
+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);
+    MPh3 = new DigitalOut(_MPh3);
+        init = true;
+    MotorIndex = 0;
+    //
+    // Connect Interrupt routine in which the motor and all the state machine is performed
+    //
+    direction = CLOCKWISE;         // Default direction is clockwise
+    state = Motor_IDLE;    // Default state is IDLE
+    command = MOTOR_nop;       // Default command is NOP
+    MotorStepTime = tickTime;      // value in micro second for one step
+    MotorFullTurn = MOTOR_TICKS_FOR_A_TURN;       // Initial Calibration
+    NumSteps = MotorFullTurn;               // Default 1 turn
+    itOnStop = true;
+            tuneTimings.reset();
+         tuneTimings.start();
+}
+
+Motor::Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3) {
+    initialization( _MPh0,  _MPh1,  _MPh2,  _MPh3, MOTOR_STEP_TIME_DEFAULT);
+ 
+}
+
+
+Motor::Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3, uint32_t tickTime) {
+ 
+    initialization( _MPh0,  _MPh1,  _MPh2,  _MPh3, tickTime);
+ 
+}
+
+
+void Motor::removeMotorCallback() {
+    
+    _callback = NULL;
+    
+}
+
+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;
+    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();
+        MotorSystemTick.attach_us(callback(this, &Motor::ProcessMotorStateMachine), 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);
+    }
+}
+
+void Motor::Start() {
+        SetCommand(MOTOR_start);   
+};
+
+void Motor::Stop() {
+        SetCommand(MOTOR_stop);
+}
+
+
+void Motor::Pause() {
+        SetCommand(MOTOR_pause);
+}
+
+
+void Motor::Restart() {
+        SetCommand(MOTOR_restart);
+}
+
+void Motor::SetZero() {
+        SetCommand(MOTOR_zero);
+}
+
+void Motor::RunInfinite(MotorDir dir) {
+    SetDirection( dir);
+    NumSteps = -1;
+    SetCommand(MOTOR_start);
+}
+
+void Motor::RunSteps(MotorDir dir, uint32_t steps) {
+    SetDirection( dir);
+    NumSteps = steps;
+    SetCommand(MOTOR_start);
+}
+
+void Motor::RunDegrees(MotorDir dir, float degree) {
+    SetDirection( dir);
+    NumSteps = (int)(degree * (float)MotorFullTurn / (float)360.0);
+    SetCommand(MOTOR_start);
+}
+void Motor::SetDirection(MotorDir dir) {
+    direction=dir;
+}
+
+void Motor::SetCommand(MotorCommand cmd) {
+    if(init)   {
+        MotorSystemTick.attach_us(callback(this, &Motor::ProcessMotorStateMachine), MotorStepTime);
+        last=tuneTimings.read_us();
+        init=false;
+        }
+    command=cmd;
+}
+
+void Motor::StopMotor() // --- Stop Motor
+{
+        *MPh0 = 0;  *MPh1 = 0;  *MPh2 = 0;  *MPh3 = 0;
+        MotorIndex = 0;
+   
+}
+
+
+void    Motor::StartMotor()
+{
+        MotorIndex = 0;
+        *MPh0 = 1;  *MPh1 = 0;  *MPh2 = 0;   *MPh3 = 0;
+}
+
+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 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];
+    if (MotorIndex<7) MotorIndex++;
+    else    MotorIndex = 0;
+}
+void    Motor::LeftMotor()    // Move the Motor one step Right
+{
+    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};
+   *MPh0 = LPh0[MotorIndex];  *MPh1 = LPh1[MotorIndex];  *MPh2 = LPh2[MotorIndex];  *MPh3 = LPh3[MotorIndex];
+    if (MotorIndex>0) MotorIndex--;
+    else    MotorIndex = 7;
+}
+
+void    Motor::RunMotor()     // Move the Motor in the user direction
+{
+        if (direction==CLOCKWISE) RightMotor();
+        else LeftMotor();
+}
+
+
+void Motor::ProcessMotorStateMachine()
+{        
+
+        if (state==Motor_IDLE) {
+            if (command == MOTOR_start) {
+                // Start the Motor
+                StartMotor();
+                state = Motor_RUN;
+                }
+            else if (command == MOTOR_zero) {
+                // Start zeroing the Motor
+                StartMotor();
+                state = Motor_ZERO;
+                }
+            command = MOTOR_nop;
+            }
+        else if (state==Motor_RUN) {
+            // Action always performed in that state
+             if (NumSteps>0 || NumSteps <0) {
+                RunMotor();
+                NumSteps--;
+                }
+            // Check next state
+            if (command == MOTOR_pause) {
+                state = Motor_PAUSE;
+                }
+            else if ((command == MOTOR_stop)||(NumSteps==0)) {
+                StopMotor();
+                     if(state != Motor_IDLE ){//|| itOnStop == true) {
+                        _callback.call();
+                    }
+                state = Motor_IDLE;
+               }
+            command = MOTOR_nop;
+            }
+        else if (state==Motor_PAUSE) {
+            if (command == MOTOR_stop) {
+                StopMotor();
+                NumSteps=0;
+                state = Motor_IDLE;
+                }
+            else if (command == MOTOR_restart) {
+                state = Motor_RUN;
+                }
+            command = MOTOR_nop;
+            }
+        else if (state==Motor_ZERO) {
+            command = MOTOR_nop;
+            }
+}
+
+MotorStateList Motor::getState() {
+   return state;    
+}
+
+void Motor::TestMotor()    // Just to check that it make a full taurn back and forth
+{
+    int i;
+    StartMotor();
+    for (i=0; i<MotorFullTurn; i++) {
+        wait(0.005);
+        RightMotor();
+        }   
+    wait(0.5);
+    for (i=0; i<MotorFullTurn; i++) {
+        wait(0.005);
+        LeftMotor();
+        }   
+    StopMotor();
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorLib/motor.h	Sat Jun 13 14:16:45 2020 +0000
@@ -0,0 +1,102 @@
+// -------------------- Motor ---------------------------
+#ifdef MOTOR_H
+#else
+#define MOTOR_H
+
+#include "mbed.h"
+
+#define MOTOR_STEP_TIME_MIN 700
+#define MOTOR_STEP_TIME_DEFAULT 5000
+#define MOTOR_TICKS_FOR_A_TURN 4096
+
+typedef enum MotorStateList {   // Define Motor States for the State Machine
+    Motor_IDLE = 0,
+    Motor_RUN,
+    Motor_PAUSE,
+    Motor_ZERO,
+    Motor_CALIB
+    } MotorState;
+    
+typedef enum MotorCommandList { // Define Motor State Machine Commands
+    MOTOR_nop = 0,
+    MOTOR_start,
+    MOTOR_pause,
+    MOTOR_restart,
+    MOTOR_stop,
+    MOTOR_zero
+    } MotorCommand;
+
+typedef enum MotorDirectionList { // Define Motor Clockwise or Anticlockwise
+    CLOCKWISE = 0,
+    COUNTERCLOCKWISE
+    } MotorDir;
+    
+class Motor {
+    
+    MotorState state;
+    MotorCommand command;
+    MotorDir direction;
+
+    
+    public:
+ 
+    
+    Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3, uint32_t TickTime);
+    Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3);
+    void RunSteps(MotorDir direction, uint32_t steps);
+    void RunDegrees(MotorDir direction, float steps);
+    void RunInfinite(MotorDir direction);
+    void SetDirection(MotorDir dir);
+    void TestMotor();
+    void RunMotor();
+    void setMotorCallback(void (*mIT)());
+     void setMotorCallback(void (*mIT)(), bool itOnStop);
+ 
+template<typename T>
+void setMotorCallback(T *object, void (T::*member)(void))
+{
+    _callback = callback(object,member);
+    itOnStop = true;
+}
+ 
+    void removeMotorCallback();
+
+    uint32_t getCalibration();    
+    void setCalibration(uint32_t nbTicksforFullTurn);
+    void setDelayBtwTicks(uint32_t tickTime); // in ms
+    void setSpeed(float sForOneTurn) ;// nb of s to get rotation of 360° (if 20s, the motor will do 360° in 20 s). 
+    
+    void Start();
+    void Stop();
+    void Pause();
+    void Restart();
+    void SetZero();
+    
+    MotorStateList getState();
+    
+    private:
+    void initialization(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3, uint32_t TickTime);
+ 
+    void StopMotor();
+    void StartMotor();
+    void SetCommand(MotorCommand cmd);
+    void LeftMotor();
+    void RightMotor();
+    void ProcessMotorStateMachine();
+    Timer tuneTimings;
+uint32_t last;
+    DigitalOut *MPh0, *MPh1, *MPh2, *MPh3;
+    
+    int MotorIndex;    // --- Motor Variable
+   
+    bool init;
+    Ticker  MotorSystemTick;    // System Callback for Motor
+    timestamp_t MotorStepTime;  // Time in µs for one motor step
+    uint32_t    MotorFullTurn;  // Number of step for a complete turn
+    int32_t    NumSteps;       // Number of Steps = NumWire * MotorFullTurn
+    Callback<void()> _callback;
+
+    bool itOnStop;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Jun 13 14:16:45 2020 +0000
@@ -0,0 +1,8 @@
+#include "mbed.h"
+#include "motor.h"
+
+Motor mon_motor(A1,A2,A5,A6);
+
+int main() {
+    mon_motor.RunInfinite(CLOCKWISE);  
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sat Jun 13 14:16:45 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file