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@22:d2c742bdae16, 2019-01-03 (annotated)

Committer:
sepp_nepp
Date:
Thu Jan 03 23:15:42 2019 +0000
Revision:
22:d2c742bdae16
Parent:
17:86e5af6f7628
Child:
23:6060422cd6eb
Updates that makes the motor aware of the wheel diameter.

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 22:d2c742bdae16 6 void MotStatus::set(motCmands aCmd, bool AClockWise, int32_t aNSteps) {
sepp_nepp 16:d818c1a4dafb 7 cmd = aCmd;
sepp_nepp 22:d2c742bdae16 8 AClockWise = AClockWise;
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 22:d2c742bdae16 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 22:d2c742bdae16 25 initialization( _MPh, AStepTime_us);
garphil 9:5983c10d5f8e 26 }
garphil 10:1df5a7a265e8 27
sepp_nepp 22:d2c742bdae16 28 void Motor::initialization(PinName _MPh[4], uint32_t AStepTime_us) {
garphil 10:1df5a7a265e8 29
sepp_nepp 13:4563244c4071 30 for (int ph=0; ph<4; ph++) { MPh[ph] = new DigitalOut(_MPh[ph]); }
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 22:d2c742bdae16 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 22:d2c742bdae16 42
sepp_nepp 22:d2c742bdae16 43 // All geometric information defaults to 0:
sepp_nepp 22:d2c742bdae16 44 rotate_angle_deg = 0; // calculated wheel rotation angle
sepp_nepp 22:d2c742bdae16 45 diam_cm = 0; // wheel diameter in centimeter
sepp_nepp 22:d2c742bdae16 46 perim_cm = 0; // wheel perimeter in centimeter
sepp_nepp 22:d2c742bdae16 47 degree_per_cm = 0; // rotation angle in degrees per cm circumference
sepp_nepp 22:d2c742bdae16 48 defaultDirection = CLOCKWISE;
garphil 9:5983c10d5f8e 49 }
garphil 9:5983c10d5f8e 50
sepp_nepp 22:d2c742bdae16 51 // *****************************************************************
sepp_nepp 22:d2c742bdae16 52 // all following functions are based on centimeter information
sepp_nepp 22:d2c742bdae16 53 // *****************************************************************
sepp_nepp 22:d2c742bdae16 54 #define PI 3.141592f /** PI needed to calcuate from angle to distances */
sepp_nepp 22:d2c742bdae16 55 #define PI_OVER_180 (PI/180.0f) /** needed to translate from angle to circumference */
sepp_nepp 22:d2c742bdae16 56
sepp_nepp 22:d2c742bdae16 57 /* High Level: Run Motor for a given number of centimeters, centimeters<0 are run in opposite direction. */
sepp_nepp 22:d2c742bdae16 58 void Motor::RunCentimeters (bool AClockWise, float centimeters);
sepp_nepp 22:d2c742bdae16 59 { RunDegrees(AClockWise, centimeters * degree_per_cm; ) }
sepp_nepp 22:d2c742bdae16 60
sepp_nepp 22:d2c742bdae16 61 /* High Level: Run Motor for a given number of centimeters in default direction. */
sepp_nepp 22:d2c742bdae16 62 void Motor::RunCentimeters (float centimeters);
sepp_nepp 22:d2c742bdae16 63 { RunDegrees(defaultDirection, centimeters * degree_per_cm; ) }
sepp_nepp 22:d2c742bdae16 64
sepp_nepp 22:d2c742bdae16 65 /** Additional geometric information: set the wheel diameter, also sets perimeter and degrees per cm.*/
sepp_nepp 22:d2c742bdae16 66 void Motor:setDiamCM( float Adiam_cm) /**< Helper; set diameter and perim to values */
sepp_nepp 22:d2c742bdae16 67 { diam_cm = Adiam_cm;
sepp_nepp 22:d2c742bdae16 68 perim_cm = PI*diam_cm;
sepp_nepp 22:d2c742bdae16 69 degree_per_cm=360.0f/perim_cm;
sepp_nepp 22:d2c742bdae16 70 }
sepp_nepp 13:4563244c4071 71
sepp_nepp 22:d2c742bdae16 72 /** Helper: calculate the needed wheel angle from a turn angle and a turn_radius_cm */
sepp_nepp 22:d2c742bdae16 73 float Motor:calcAngle(float turn_angle_deg, float turn_radius_cm)
sepp_nepp 22:d2c742bdae16 74 { // a turn radius smaller than 0 make no sense
sepp_nepp 22:d2c742bdae16 75 if( turn_radius_cm>= 0 )
sepp_nepp 22:d2c742bdae16 76 rotate_angle_deg = turn_angle_deg * PI_OVER_180 * turn_radius_cm * degree_per_cm;
sepp_nepp 22:d2c742bdae16 77 else rotate_angle_deg = 0;
sepp_nepp 22:d2c742bdae16 78 return rotate_angle_deg;
sepp_nepp 22:d2c742bdae16 79 }
sepp_nepp 13:4563244c4071 80
sepp_nepp 22:d2c742bdae16 81 void Motor::setSpeed_cm_sec(float speed_cm_sec)
sepp_nepp 22:d2c742bdae16 82 { if (speed_cm_sec < MIN_SPEED_CM_SEC) // catch too small or negative speeds
sepp_nepp 22:d2c742bdae16 83 setRotationPeriodSec( perim_cm / MIN_SPEED_CM_SEC);
sepp_nepp 22:d2c742bdae16 84 else setRotationPeriodSec( perim_cm / speed_cm_sec );
sepp_nepp 22:d2c742bdae16 85 }
sepp_nepp 22:d2c742bdae16 86
sepp_nepp 22:d2c742bdae16 87 // *****************************************************************
sepp_nepp 22:d2c742bdae16 88 // all following functions are agnostic of centimeter-information
sepp_nepp 22:d2c742bdae16 89 // *****************************************************************
sepp_nepp 22:d2c742bdae16 90
sepp_nepp 22:d2c742bdae16 91 void Motor::RunInfinite(bool AClockWise) {
sepp_nepp 22:d2c742bdae16 92 Status.set(MOTOR_run, AClockWise, -1);
sepp_nepp 13:4563244c4071 93 StartTick();
garphil 9:5983c10d5f8e 94 }
garphil 9:5983c10d5f8e 95
sepp_nepp 22:d2c742bdae16 96 /* High Level: Run Motor for a given angle, angles<0 are run in opposite direction. */
sepp_nepp 22:d2c742bdae16 97 void Motor::RunDegrees(bool AClockWise, float angle_deg) {
sepp_nepp 22:d2c742bdae16 98 RunSteps( AClockWise, (int32_t)(angle_deg * (float)Steps_FullTurn / 360.0f) );
garphil 4:c009bcd5518c 99 }
sepp_nepp 22:d2c742bdae16 100
sepp_nepp 22:d2c742bdae16 101 /* High Level: Run Motor for a given number of steps, steps<0 are run in opposite direction. */
sepp_nepp 22:d2c742bdae16 102 void Motor::RunSteps(bool AClockWise, int32_t steps) {
sepp_nepp 22:d2c742bdae16 103 if (steps!=0)
sepp_nepp 22:d2c742bdae16 104 { Status.set(MOTOR_run, AClockWise ^^ (steps<0), abs(steps));
sepp_nepp 22:d2c742bdae16 105 StartTick(); }
sepp_nepp 16:d818c1a4dafb 106 }
garphil 4:c009bcd5518c 107
sepp_nepp 16:d818c1a4dafb 108 void Motor::PauseRun()
sepp_nepp 13:4563244c4071 109 { if (Status.cmd==MOTOR_run) Status.cmd = MOTOR_pause; }
garphil 4:c009bcd5518c 110
sepp_nepp 16:d818c1a4dafb 111 void Motor::RestartRun()
sepp_nepp 13:4563244c4071 112 { if (Status.cmd==MOTOR_pause) Status.cmd = MOTOR_run; }
garphil 6:aec892eb1b49 113
sepp_nepp 16:d818c1a4dafb 114 void Motor::StopRun()
sepp_nepp 13:4563244c4071 115 { Status.cmd = MOTOR_stop; }
sepp_nepp 13:4563244c4071 116
sepp_nepp 13:4563244c4071 117 MotStatus Motor::getStatus() { return Status; }
garphil 3:01b4c058454d 118
sepp_nepp 13:4563244c4071 119 /*******************************************************
sepp_nepp 13:4563244c4071 120 ** Ticker / Timing procedures
sepp_nepp 13:4563244c4071 121 *******************************************************/
sepp_nepp 16:d818c1a4dafb 122 //Get, set the scaling
sepp_nepp 16:d818c1a4dafb 123 uint32_t Motor::getStepsFullTurn()
sepp_nepp 16:d818c1a4dafb 124 { return Steps_FullTurn; }
sepp_nepp 16:d818c1a4dafb 125
sepp_nepp 16:d818c1a4dafb 126 void Motor::setStepsFullTurn(uint32_t StepsFullTurn)
sepp_nepp 16:d818c1a4dafb 127 { Steps_FullTurn = StepsFullTurn; }
garphil 1:9519ac966b79 128
sepp_nepp 13:4563244c4071 129 void Motor::setRotationPeriodSec(float Seconds_Per_Turn) {
sepp_nepp 13:4563244c4071 130 // rescale to usec and pass on to the next handler.
sepp_nepp 22:d2c742bdae16 131 setStepTime_us( uint32_t(Seconds_Per_Turn / Steps_FullTurn * 1000000) ) ;
garphil 1:9519ac966b79 132 }
sepp_nepp 22:d2c742bdae16 133 void Motor::setStepTime_us(uint32_t AStepTime_us) {
sepp_nepp 22:d2c742bdae16 134 if(StepTime_us == AStepTime_us) return; // avoid futile activity
sepp_nepp 13:4563244c4071 135 if (StepTime_us < MOTOR_STEP_TIME_MIN_US) // filter too small values
sepp_nepp 13:4563244c4071 136 StepTime_us = MOTOR_STEP_TIME_MIN_US;
sepp_nepp 22:d2c742bdae16 137 else StepTime_us = AStepTime_us; // or if OK then assign value
sepp_nepp 13:4563244c4071 138 // if ticker already running recreate a new ticker;
sepp_nepp 15:88fecbdd191c 139 if(Status.TickIsAttached) { StopTick(); StartTick(); }
sepp_nepp 13:4563244c4071 140 }
sepp_nepp 13:4563244c4071 141
sepp_nepp 13:4563244c4071 142 void Motor::StartTick() {
sepp_nepp 15:88fecbdd191c 143 if(!Status.TickIsAttached) {
sepp_nepp 16:d818c1a4dafb 144 // Connect Interrupt routine in which the Motor and all the state machine is performed
sepp_nepp 13:4563244c4071 145 MotorSysTick.attach_us(callback(this, &Motor::ProcessMotorStateMachine), StepTime_us);
sepp_nepp 13:4563244c4071 146 // last=tuneTimings.read_us();
sepp_nepp 15:88fecbdd191c 147 Status.TickIsAttached=true;
sepp_nepp 13:4563244c4071 148 }
garphil 1:9519ac966b79 149 }
garphil 1:9519ac966b79 150
garphil 1:9519ac966b79 151 void Motor::ProcessMotorStateMachine()
sepp_nepp 13:4563244c4071 152 {
sepp_nepp 13:4563244c4071 153 switch(Status.cmd) {
sepp_nepp 13:4563244c4071 154 case MOTOR_run: {
sepp_nepp 15:88fecbdd191c 155 switch(Status.state) {
sepp_nepp 13:4563244c4071 156 case Motor_OFF:
sepp_nepp 16:d818c1a4dafb 157 MotorON(); // First only turn on the Motor ..
sepp_nepp 13:4563244c4071 158 break;
sepp_nepp 13:4563244c4071 159 case Motor_ZERO:
sepp_nepp 13:4563244c4071 160 case Motor_ON:
sepp_nepp 15:88fecbdd191c 161 Status.state = Motor_RUN;
sepp_nepp 15:88fecbdd191c 162 case Motor_RUN:
sepp_nepp 13:4563244c4071 163 if (Status.NSteps==0) // No more steps to go
sepp_nepp 13:4563244c4071 164 { Status.cmd = MOTOR_stop;
sepp_nepp 13:4563244c4071 165 if (_callback) _callback.call(); }
sepp_nepp 13:4563244c4071 166 else // More steps to go
sepp_nepp 13:4563244c4071 167 { StepOnce();
sepp_nepp 13:4563244c4071 168 Status.NSteps--;}
sepp_nepp 13:4563244c4071 169 break;
sepp_nepp 15:88fecbdd191c 170 } // switch(Status.state)
sepp_nepp 13:4563244c4071 171 } // case MOTOR_run
sepp_nepp 13:4563244c4071 172 case MOTOR_stop:
sepp_nepp 13:4563244c4071 173 StopTick();
sepp_nepp 13:4563244c4071 174 Status.cmd = MOTOR_nop;
sepp_nepp 13:4563244c4071 175 MotorOFF();
sepp_nepp 13:4563244c4071 176 break;
sepp_nepp 13:4563244c4071 177 case MOTOR_nop:
sepp_nepp 13:4563244c4071 178 case MOTOR_pause:
sepp_nepp 13:4563244c4071 179 default: break;
sepp_nepp 13:4563244c4071 180 } // switch(Status.cmd)
sepp_nepp 13:4563244c4071 181 }
garphil 11:25d26c72a2f7 182
sepp_nepp 13:4563244c4071 183 void Motor::StopTick() {
sepp_nepp 15:88fecbdd191c 184 if(Status.TickIsAttached)
sepp_nepp 15:88fecbdd191c 185 { MotorSysTick.detach(); Status.TickIsAttached=false; }
garphil 1:9519ac966b79 186 }
garphil 1:9519ac966b79 187
sepp_nepp 13:4563244c4071 188 /*******************************************************
sepp_nepp 16:d818c1a4dafb 189 ** all the low level direct Motor HW access
sepp_nepp 13:4563244c4071 190 *******************************************************/
sepp_nepp 16:d818c1a4dafb 191 void Motor::MotorTest() // Just to check that it make a full turn back and forth
sepp_nepp 16:d818c1a4dafb 192 {
sepp_nepp 16:d818c1a4dafb 193 int i;
sepp_nepp 16:d818c1a4dafb 194 MotorON();
sepp_nepp 16:d818c1a4dafb 195 for (i=0; i<Steps_FullTurn; i++) {
sepp_nepp 16:d818c1a4dafb 196 wait(0.005);
sepp_nepp 16:d818c1a4dafb 197 StepClkW();
sepp_nepp 16:d818c1a4dafb 198 }
sepp_nepp 16:d818c1a4dafb 199 wait(0.5);
sepp_nepp 16:d818c1a4dafb 200 for (i=0; i<Steps_FullTurn; i++) {
sepp_nepp 16:d818c1a4dafb 201 wait(0.005);
sepp_nepp 16:d818c1a4dafb 202 StepCCW();
sepp_nepp 16:d818c1a4dafb 203 }
sepp_nepp 16:d818c1a4dafb 204 MotorOFF();
sepp_nepp 16:d818c1a4dafb 205 }
sepp_nepp 16:d818c1a4dafb 206
sepp_nepp 16:d818c1a4dafb 207 /** Turn off all Motor Phases, no more current flowing */
sepp_nepp 13:4563244c4071 208 void Motor::MotorOFF()
sepp_nepp 13:4563244c4071 209 { for (int ph=0; ph<4; ph++) *MPh[ph] = 0;
sepp_nepp 15:88fecbdd191c 210 Status.state=Motor_OFF;
arnophilippe 7:439458133bba 211 }
garphil 1:9519ac966b79 212
sepp_nepp 16:d818c1a4dafb 213 /** Turn on the Motor Phase, In the last used phase, memorized in StepPhases
sepp_nepp 13:4563244c4071 214 * Equivalent to what previously the function "void Start();" did */
sepp_nepp 13:4563244c4071 215 void Motor::MotorON()
sepp_nepp 13:4563244c4071 216 { SetPhases(); // attention, does not change StepPhase!
sepp_nepp 15:88fecbdd191c 217 if (StepPhase==0) Status.state=Motor_ZERO;
sepp_nepp 15:88fecbdd191c 218 else Status.state=Motor_ON;
sepp_nepp 13:4563244c4071 219 }
sepp_nepp 13:4563244c4071 220
sepp_nepp 13:4563244c4071 221 /** Motor phases turned on and put to Zero Position*/
sepp_nepp 13:4563244c4071 222 void Motor::MotorZero() {
sepp_nepp 13:4563244c4071 223 StepPhase = 0; // sets the phases to 0
sepp_nepp 13:4563244c4071 224 SetPhases();
sepp_nepp 15:88fecbdd191c 225 Status.state=Motor_ZERO;
sepp_nepp 13:4563244c4071 226 }
sepp_nepp 13:4563244c4071 227
sepp_nepp 13:4563244c4071 228 void Motor::StepOnce() // Move the Motor in the used 'direction'
sepp_nepp 22:d2c742bdae16 229 { if (Status.AClockWise) StepClkW(); else StepCCW();
sepp_nepp 13:4563244c4071 230 }
sepp_nepp 13:4563244c4071 231
sepp_nepp 16:d818c1a4dafb 232 void Motor::StepClkW() // Move the Motor one step Clockwise
sepp_nepp 13:4563244c4071 233 { if (StepPhase<7) StepPhase++; else StepPhase = 0;
sepp_nepp 13:4563244c4071 234 SetPhases();
sepp_nepp 13:4563244c4071 235 }
sepp_nepp 16:d818c1a4dafb 236 void Motor::StepCCW() // Move the Motor one step Clockwise
sepp_nepp 13:4563244c4071 237 { if (StepPhase>0) StepPhase--; else StepPhase = 7;
sepp_nepp 13:4563244c4071 238 SetPhases();
sepp_nepp 13:4563244c4071 239 }
sepp_nepp 13:4563244c4071 240
sepp_nepp 13:4563244c4071 241 static const int MotPh[4][8] =
sepp_nepp 13:4563244c4071 242 { {1, 1, 0, 0, 0, 0, 0, 1},
sepp_nepp 13:4563244c4071 243 {0, 1, 1, 1, 0, 0, 0, 0},
sepp_nepp 13:4563244c4071 244 {0, 0, 0, 1, 1, 1, 0, 0},
sepp_nepp 13:4563244c4071 245 {0, 0, 0, 0, 0, 1, 1, 1}};
sepp_nepp 13:4563244c4071 246
sepp_nepp 13:4563244c4071 247 void Motor::SetPhases() // Engage Motor Phases according to StepPhase
sepp_nepp 13:4563244c4071 248 { for (int ph=0; ph<4; ph++) { *MPh[ph] = MotPh[ph][StepPhase]; } }
sepp_nepp 13:4563244c4071 249