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@17:86e5af6f7628, 2018-11-28 (annotated)

Committer:
sepp_nepp
Date:
Wed Nov 28 21:04:24 2018 +0000
Revision:
17:86e5af6f7628
Parent:
16:d818c1a4dafb
Child:
22:d2c742bdae16
syntax fixed of some doxygen documentation

Who changed what in which revision?

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