Tarek Lule / MotorLib

My Version of the Crealab MotorLib.

Fork of MotorLib by CreaLab

CreaMot.cpp@24:932ea7bdc850, 2019-04-18 (annotated)

Committer:
sepp_nepp
Date:
Thu Apr 18 12:12:48 2019 +0000
Revision:
24:932ea7bdc850
Compiles

Who changed what in which revision?

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