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@23:6060422cd6eb, 2019-04-17 (annotated)

Committer:
sepp_nepp
Date:
Wed Apr 17 21:48:14 2019 +0000
Revision:
23:6060422cd6eb
Parent:
22:d2c742bdae16 
Refactor

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