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@16:d818c1a4dafb, 2018-11-28 (annotated)

Committer:
sepp_nepp
Date:
Wed Nov 28 09:44:34 2018 +0000
Revision:
16:d818c1a4dafb
Parent:
15:88fecbdd191c
Child:
17:86e5af6f7628
Release with major documentation added

Who changed what in which revision?

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