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.h@18:00e3d8c71a9c, 2018-12-30 (annotated)

Committer:
sepp_nepp
Date:
Sun Dec 30 22:48:24 2018 +0000
Revision:
18:00e3d8c71a9c
Parent:
17:86e5af6f7628
Child:
19:2ac158fe414e
Updated Doxygen

Who changed what in which revision?

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