Tarek Lule / MotorLib

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of MotorLib by CreaLab

motor.cpp@13:4563244c4071, 2018-10-31 (annotated)

Committer:
sepp_nepp
Date:
Wed Oct 31 14:24:17 2018 +0000
Revision:
13:4563244c4071
Parent:
12:fd881ff72048
Child:
15:88fecbdd191c
First publishing of my version of Motor Lib

Who changed what in which revision?

UserRevisionLine numberNew contents of line
garphil 0:bd05fd602a6e 1 #include "motor.h"
garphil 0:bd05fd602a6e 2
sepp_nepp 13:4563244c4071 3 void MotStatus::set(motorCommands Acmd, motorDir Adir, int32_t ANSteps) {
sepp_nepp 13:4563244c4071 4 cmd = Acmd;
sepp_nepp 13:4563244c4071 5 dir = Adir;
sepp_nepp 13:4563244c4071 6 NSteps = NSteps;
sepp_nepp 13:4563244c4071 7 } MotStatus;
sepp_nepp 13:4563244c4071 8
sepp_nepp 13:4563244c4071 9 Motor::Motor(PinName _MPh[4]) {
sepp_nepp 13:4563244c4071 10 initialization( _MPh , MOTOR_STEP_TIME_DEFAULT_US);
garphil 9:5983c10d5f8e 11 }
garphil 9:5983c10d5f8e 12
garphil 9:5983c10d5f8e 13 Motor::Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3) {
sepp_nepp 13:4563244c4071 14 PinName _MPh[4] = {_MPh0, _MPh1, _MPh2, _MPh3};
sepp_nepp 13:4563244c4071 15 initialization( _MPh , MOTOR_STEP_TIME_DEFAULT_US);
garphil 9:5983c10d5f8e 16 }
garphil 9:5983c10d5f8e 17
sepp_nepp 13:4563244c4071 18 Motor::Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3, uint32_t aStepTime_us) {
sepp_nepp 13:4563244c4071 19 PinName _MPh[4] = {_MPh0, _MPh1, _MPh2, _MPh3};
sepp_nepp 13:4563244c4071 20 initialization( _MPh, aStepTime_us);
garphil 9:5983c10d5f8e 21 }
garphil 10:1df5a7a265e8 22
sepp_nepp 13:4563244c4071 23 //void Motor::initialization(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3, uint32_t aStepTime_us) {
sepp_nepp 13:4563244c4071 24 void Motor::initialization(PinName _MPh[4], uint32_t aStepTime_us) {
garphil 10:1df5a7a265e8 25
sepp_nepp 13:4563244c4071 26 for (int ph=0; ph<4; ph++) { MPh[ph] = new DigitalOut(_MPh[ph]); }
sepp_nepp 13:4563244c4071 27 /*MPh[0] = new DigitalOut(_MPh0);
sepp_nepp 13:4563244c4071 28 MPh[1] = new DigitalOut(_MPh1);
sepp_nepp 13:4563244c4071 29 MPh[2] = new DigitalOut(_MPh2);
sepp_nepp 13:4563244c4071 30 MPh[3] = new DigitalOut(_MPh3);*/
sepp_nepp 13:4563244c4071 31
sepp_nepp 13:4563244c4071 32 MotorOFF(); // Default state is all phases are OFF
sepp_nepp 13:4563244c4071 33 StepPhase = 0; // initial phase is Zero
sepp_nepp 13:4563244c4071 34
sepp_nepp 13:4563244c4071 35 Status.set(MOTOR_nop, CLOCKWISE, 0);// Default command is NOP, clockwise direction, 0 steps
sepp_nepp 13:4563244c4071 36
sepp_nepp 13:4563244c4071 37 TickIsAttached = false;
sepp_nepp 13:4563244c4071 38 StepTime_us = aStepTime_us;// duration in micro second for one step
garphil 9:5983c10d5f8e 39
sepp_nepp 13:4563244c4071 40 Steps_FullTurn = MOTOR_STEPS_FOR_A_TURN; // Default Calibration value
sepp_nepp 13:4563244c4071 41 _callback = NULL; // No default Callback
sepp_nepp 13:4563244c4071 42
sepp_nepp 13:4563244c4071 43 // itOnStop = true;
sepp_nepp 13:4563244c4071 44 // tuneTimings.reset();
sepp_nepp 13:4563244c4071 45 // tuneTimings.start();
garphil 9:5983c10d5f8e 46 }
garphil 9:5983c10d5f8e 47
sepp_nepp 13:4563244c4071 48 //Attaching and removing Callbacks
sepp_nepp 13:4563244c4071 49 void Motor::removeMotorCallback()
sepp_nepp 13:4563244c4071 50 { _callback = NULL; }
sepp_nepp 13:4563244c4071 51
sepp_nepp 13:4563244c4071 52 void Motor::setMotorCallback(void (*function)(void))
sepp_nepp 13:4563244c4071 53 { _callback = function; }
sepp_nepp 13:4563244c4071 54
sepp_nepp 13:4563244c4071 55 uint32_t Motor::getStepsFullTurn()
sepp_nepp 13:4563244c4071 56 { return Steps_FullTurn; }
sepp_nepp 13:4563244c4071 57
sepp_nepp 13:4563244c4071 58 void Motor::setStepsFullTurn(uint32_t StepsFullTurn)
sepp_nepp 13:4563244c4071 59 { Steps_FullTurn = StepsFullTurn; }
sepp_nepp 13:4563244c4071 60
sepp_nepp 13:4563244c4071 61 void Motor::RunInfinite(motorDir direction) {
sepp_nepp 13:4563244c4071 62 Status.set(MOTOR_run, direction, -1);
sepp_nepp 13:4563244c4071 63 StartTick();
garphil 9:5983c10d5f8e 64 }
garphil 9:5983c10d5f8e 65
sepp_nepp 13:4563244c4071 66 void Motor::RunSteps(motorDir direction, uint32_t steps) {
sepp_nepp 13:4563244c4071 67 if (steps>0)
sepp_nepp 13:4563244c4071 68 { Status.set(MOTOR_run, direction, steps);
sepp_nepp 13:4563244c4071 69 StartTick(); }
garphil 4:c009bcd5518c 70 }
garphil 4:c009bcd5518c 71
sepp_nepp 13:4563244c4071 72 void Motor::Pause()
sepp_nepp 13:4563244c4071 73 { if (Status.cmd==MOTOR_run) Status.cmd = MOTOR_pause; }
garphil 4:c009bcd5518c 74
sepp_nepp 13:4563244c4071 75 void Motor::Restart()
sepp_nepp 13:4563244c4071 76 { if (Status.cmd==MOTOR_pause) Status.cmd = MOTOR_run; }
garphil 6:aec892eb1b49 77
sepp_nepp 13:4563244c4071 78 void Motor::Stop()
sepp_nepp 13:4563244c4071 79 { Status.cmd = MOTOR_stop; }
sepp_nepp 13:4563244c4071 80
sepp_nepp 13:4563244c4071 81 MotStatus Motor::getStatus() { return Status; }
garphil 3:01b4c058454d 82
sepp_nepp 13:4563244c4071 83 void Motor::TestMotor() // Just to check that it make a full turn back and forth
sepp_nepp 13:4563244c4071 84 {
sepp_nepp 13:4563244c4071 85 int i;
sepp_nepp 13:4563244c4071 86 MotorON();
sepp_nepp 13:4563244c4071 87 for (i=0; i<Steps_FullTurn; i++) {
sepp_nepp 13:4563244c4071 88 wait(0.005);
sepp_nepp 13:4563244c4071 89 StepRight();
sepp_nepp 13:4563244c4071 90 }
sepp_nepp 13:4563244c4071 91 wait(0.5);
sepp_nepp 13:4563244c4071 92 for (i=0; i<Steps_FullTurn; i++) {
sepp_nepp 13:4563244c4071 93 wait(0.005);
sepp_nepp 13:4563244c4071 94 StepLeft();
sepp_nepp 13:4563244c4071 95 }
sepp_nepp 13:4563244c4071 96 MotorOFF();
garphil 2:c91c5ef00d25 97 }
garphil 3:01b4c058454d 98
sepp_nepp 13:4563244c4071 99 /*******************************************************
sepp_nepp 13:4563244c4071 100 ** Ticker / Timing procedures
sepp_nepp 13:4563244c4071 101 *******************************************************/
garphil 1:9519ac966b79 102
sepp_nepp 13:4563244c4071 103 void Motor::setRotationPeriodSec(float Seconds_Per_Turn) {
sepp_nepp 13:4563244c4071 104 // rescale to usec and pass on to the next handler.
sepp_nepp 13:4563244c4071 105 setStepTime_us(1000 * Seconds_Per_Turn / Steps_FullTurn) ;
garphil 1:9519ac966b79 106 }
sepp_nepp 13:4563244c4071 107 void Motor::setStepTime_us(uint32_t aStepTime_us) {
sepp_nepp 13:4563244c4071 108 if(StepTime_us == aStepTime_us) return; // avoid futile activity
sepp_nepp 13:4563244c4071 109 if (StepTime_us < MOTOR_STEP_TIME_MIN_US) // filter too small values
sepp_nepp 13:4563244c4071 110 StepTime_us = MOTOR_STEP_TIME_MIN_US;
sepp_nepp 13:4563244c4071 111 else StepTime_us = aStepTime_us; // or if OK then assign value
sepp_nepp 13:4563244c4071 112 // if ticker already running recreate a new ticker;
sepp_nepp 13:4563244c4071 113 if(TickIsAttached) { StopTick(); StartTick(); }
sepp_nepp 13:4563244c4071 114 }
sepp_nepp 13:4563244c4071 115
sepp_nepp 13:4563244c4071 116 void Motor::StartTick() {
sepp_nepp 13:4563244c4071 117 if(!TickIsAttached) {
sepp_nepp 13:4563244c4071 118 // Connect Interrupt routine in which the motor and all the state machine is performed
sepp_nepp 13:4563244c4071 119 MotorSysTick.attach_us(callback(this, &Motor::ProcessMotorStateMachine), StepTime_us);
sepp_nepp 13:4563244c4071 120 // last=tuneTimings.read_us();
sepp_nepp 13:4563244c4071 121 TickIsAttached=true;
sepp_nepp 13:4563244c4071 122 }
garphil 1:9519ac966b79 123 }
garphil 1:9519ac966b79 124
garphil 1:9519ac966b79 125 void Motor::ProcessMotorStateMachine()
sepp_nepp 13:4563244c4071 126 {
sepp_nepp 13:4563244c4071 127 switch(Status.cmd) {
sepp_nepp 13:4563244c4071 128 case MOTOR_run: {
sepp_nepp 13:4563244c4071 129 switch(state) {
sepp_nepp 13:4563244c4071 130 case Motor_OFF:
sepp_nepp 13:4563244c4071 131 MotorON(); // First only turn on the motor ..
sepp_nepp 13:4563244c4071 132 break;
sepp_nepp 13:4563244c4071 133 case Motor_ZERO:
sepp_nepp 13:4563244c4071 134 case Motor_ON:
sepp_nepp 13:4563244c4071 135 state = MOTOR_RUN;
sepp_nepp 13:4563244c4071 136 case MOTOR_RUN:
sepp_nepp 13:4563244c4071 137 if (Status.NSteps==0) // No more steps to go
sepp_nepp 13:4563244c4071 138 { Status.cmd = MOTOR_stop;
sepp_nepp 13:4563244c4071 139 if (_callback) _callback.call(); }
sepp_nepp 13:4563244c4071 140 else // More steps to go
sepp_nepp 13:4563244c4071 141 { StepOnce();
sepp_nepp 13:4563244c4071 142 Status.NSteps--;}
sepp_nepp 13:4563244c4071 143 break;
sepp_nepp 13:4563244c4071 144 } // switch(state)
sepp_nepp 13:4563244c4071 145 } // case MOTOR_run
sepp_nepp 13:4563244c4071 146 case MOTOR_stop:
sepp_nepp 13:4563244c4071 147 StopTick();
sepp_nepp 13:4563244c4071 148 Status.cmd = MOTOR_nop;
sepp_nepp 13:4563244c4071 149 MotorOFF();
sepp_nepp 13:4563244c4071 150 break;
sepp_nepp 13:4563244c4071 151 case MOTOR_nop:
sepp_nepp 13:4563244c4071 152 case MOTOR_pause:
sepp_nepp 13:4563244c4071 153 default: break;
sepp_nepp 13:4563244c4071 154 } // switch(Status.cmd)
sepp_nepp 13:4563244c4071 155 }
garphil 11:25d26c72a2f7 156
sepp_nepp 13:4563244c4071 157 void Motor::StopTick() {
sepp_nepp 13:4563244c4071 158 if(TickIsAttached)
sepp_nepp 13:4563244c4071 159 { MotorSysTick.detach(); TickIsAttached=false; }
garphil 1:9519ac966b79 160 }
garphil 1:9519ac966b79 161
sepp_nepp 13:4563244c4071 162 /*******************************************************
sepp_nepp 13:4563244c4071 163 ** all the low level direct motor HW access
sepp_nepp 13:4563244c4071 164 *******************************************************/
sepp_nepp 13:4563244c4071 165
sepp_nepp 13:4563244c4071 166 /** Turn off all motor Phases, no more current flowing */
sepp_nepp 13:4563244c4071 167 void Motor::MotorOFF()
sepp_nepp 13:4563244c4071 168 { for (int ph=0; ph<4; ph++) *MPh[ph] = 0;
sepp_nepp 13:4563244c4071 169 state=Motor_OFF;
arnophilippe 7:439458133bba 170 }
garphil 1:9519ac966b79 171
sepp_nepp 13:4563244c4071 172 /** Turn on the motor Phase, In the last used phase, memorized in StepPhases
sepp_nepp 13:4563244c4071 173 * Equivalent to what previously the function "void Start();" did */
sepp_nepp 13:4563244c4071 174 void Motor::MotorON()
sepp_nepp 13:4563244c4071 175 { SetPhases(); // attention, does not change StepPhase!
sepp_nepp 13:4563244c4071 176 if (StepPhase==0) state=Motor_ZERO;
sepp_nepp 13:4563244c4071 177 else state=Motor_ON;
sepp_nepp 13:4563244c4071 178 }
sepp_nepp 13:4563244c4071 179
sepp_nepp 13:4563244c4071 180 /** Motor phases turned on and put to Zero Position*/
sepp_nepp 13:4563244c4071 181 void Motor::MotorZero() {
sepp_nepp 13:4563244c4071 182 StepPhase = 0; // sets the phases to 0
sepp_nepp 13:4563244c4071 183 SetPhases();
sepp_nepp 13:4563244c4071 184 state=Motor_ZERO;
sepp_nepp 13:4563244c4071 185 }
sepp_nepp 13:4563244c4071 186
sepp_nepp 13:4563244c4071 187 void Motor::StepOnce() // Move the Motor in the used 'direction'
sepp_nepp 13:4563244c4071 188 { if (Status.dir == CLOCKWISE) StepRight(); else StepLeft();
sepp_nepp 13:4563244c4071 189 }
sepp_nepp 13:4563244c4071 190
sepp_nepp 13:4563244c4071 191 void Motor::StepRight() // Move the Motor one step Right
sepp_nepp 13:4563244c4071 192 { if (StepPhase<7) StepPhase++; else StepPhase = 0;
sepp_nepp 13:4563244c4071 193 SetPhases();
sepp_nepp 13:4563244c4071 194 }
sepp_nepp 13:4563244c4071 195 void Motor::StepLeft() // Move the Motor one step Right
sepp_nepp 13:4563244c4071 196 { if (StepPhase>0) StepPhase--; else StepPhase = 7;
sepp_nepp 13:4563244c4071 197 SetPhases();
sepp_nepp 13:4563244c4071 198 }
sepp_nepp 13:4563244c4071 199
sepp_nepp 13:4563244c4071 200 static const int MotPh[4][8] =
sepp_nepp 13:4563244c4071 201 { {1, 1, 0, 0, 0, 0, 0, 1},
sepp_nepp 13:4563244c4071 202 {0, 1, 1, 1, 0, 0, 0, 0},
sepp_nepp 13:4563244c4071 203 {0, 0, 0, 1, 1, 1, 0, 0},
sepp_nepp 13:4563244c4071 204 {0, 0, 0, 0, 0, 1, 1, 1}};
sepp_nepp 13:4563244c4071 205
sepp_nepp 13:4563244c4071 206 void Motor::SetPhases() // Engage Motor Phases according to StepPhase
sepp_nepp 13:4563244c4071 207 { for (int ph=0; ph<4; ph++) { *MPh[ph] = MotPh[ph][StepPhase]; } }
sepp_nepp 13:4563244c4071 208