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@12:fd881ff72048, 2018-10-15 (annotated)

Committer:
julienPhuong
Date:
Mon Oct 15 12:06:50 2018 +0000
Revision:
12:fd881ff72048
Parent:
11:25d26c72a2f7
Child:
13:4563244c4071
Fixed the motor command issue. Thanks Francois.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
garphil 0:bd05fd602a6e 1 #include "motor.h"
garphil 0:bd05fd602a6e 2
garphil 9:5983c10d5f8e 3 void Motor::initialization(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3, uint32_t tickTime) {
garphil 10:1df5a7a265e8 4 //pc_uart.printf("MOTOR INIT\n");
garphil 2:c91c5ef00d25 5 MPh0 = new DigitalOut(_MPh0);
garphil 2:c91c5ef00d25 6 MPh1 = new DigitalOut(_MPh1);
garphil 2:c91c5ef00d25 7 MPh2 = new DigitalOut(_MPh2);
garphil 2:c91c5ef00d25 8 MPh3 = new DigitalOut(_MPh3);
garphil 3:01b4c058454d 9 init = true;
garphil 0:bd05fd602a6e 10 MotorIndex = 0;
garphil 1:9519ac966b79 11 //
garphil 1:9519ac966b79 12 // Connect Interrupt routine in which the motor and all the state machine is performed
garphil 1:9519ac966b79 13 //
garphil 4:c009bcd5518c 14 direction = CLOCKWISE; // Default direction is clockwise
garphil 2:c91c5ef00d25 15 state = Motor_IDLE; // Default state is IDLE
garphil 2:c91c5ef00d25 16 command = MOTOR_nop; // Default command is NOP
garphil 4:c009bcd5518c 17 MotorStepTime = tickTime; // value in micro second for one step
garphil 9:5983c10d5f8e 18 MotorFullTurn = MOTOR_TICKS_FOR_A_TURN; // Initial Calibration
garphil 9:5983c10d5f8e 19 NumSteps = MotorFullTurn; // Default 1 turn
garphil 9:5983c10d5f8e 20 itOnStop = true;
garphil 10:1df5a7a265e8 21 tuneTimings.reset();
garphil 10:1df5a7a265e8 22 tuneTimings.start();
garphil 9:5983c10d5f8e 23 }
garphil 9:5983c10d5f8e 24
garphil 9:5983c10d5f8e 25 Motor::Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3) {
garphil 9:5983c10d5f8e 26 initialization( _MPh0, _MPh1, _MPh2, _MPh3, MOTOR_STEP_TIME_DEFAULT);
garphil 9:5983c10d5f8e 27
garphil 9:5983c10d5f8e 28 }
garphil 9:5983c10d5f8e 29
garphil 9:5983c10d5f8e 30
garphil 9:5983c10d5f8e 31 Motor::Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3, uint32_t tickTime) {
garphil 9:5983c10d5f8e 32
garphil 9:5983c10d5f8e 33 initialization( _MPh0, _MPh1, _MPh2, _MPh3, tickTime);
garphil 3:01b4c058454d 34
garphil 0:bd05fd602a6e 35 }
garphil 1:9519ac966b79 36
garphil 9:5983c10d5f8e 37
garphil 9:5983c10d5f8e 38 void Motor::removeMotorCallback() {
garphil 11:25d26c72a2f7 39
garphil 11:25d26c72a2f7 40 _callback = NULL;
garphil 11:25d26c72a2f7 41
garphil 9:5983c10d5f8e 42 }
garphil 10:1df5a7a265e8 43
garphil 10:1df5a7a265e8 44 void Motor::setMotorCallback(void (*function)(void))
garphil 10:1df5a7a265e8 45 {
garphil 10:1df5a7a265e8 46 setMotorCallback(function, true);
garphil 10:1df5a7a265e8 47 }
garphil 10:1df5a7a265e8 48 void Motor::setMotorCallback(void (*function)(void), bool onStop)
garphil 10:1df5a7a265e8 49 {
garphil 10:1df5a7a265e8 50 _callback = function;
garphil 10:1df5a7a265e8 51 itOnStop = onStop;
garphil 10:1df5a7a265e8 52 }
garphil 10:1df5a7a265e8 53
garphil 10:1df5a7a265e8 54
garphil 10:1df5a7a265e8 55 uint32_t Motor::getCalibration()
garphil 10:1df5a7a265e8 56 {
garphil 10:1df5a7a265e8 57 return MotorFullTurn;
garphil 10:1df5a7a265e8 58 }
garphil 9:5983c10d5f8e 59
garphil 9:5983c10d5f8e 60 void Motor::setCalibration(uint32_t nbTicksforFullTurn) {
garphil 9:5983c10d5f8e 61 MotorFullTurn = nbTicksforFullTurn;
garphil 9:5983c10d5f8e 62 }
garphil 9:5983c10d5f8e 63
garphil 9:5983c10d5f8e 64 void Motor::setDelayBtwTicks(uint32_t tickTime) {
garphil 11:25d26c72a2f7 65 if(MotorStepTime == tickTime) return;
garphil 9:5983c10d5f8e 66 MotorStepTime = tickTime;
garphil 9:5983c10d5f8e 67 if(MotorStepTime < MOTOR_STEP_TIME_MIN) MotorStepTime = MOTOR_STEP_TIME_MIN;
garphil 11:25d26c72a2f7 68 // pc_uart.printf("Change ticktime to %d %d\n\r",tickTime, MotorStepTime);
garphil 11:25d26c72a2f7 69 if(!init) {
garphil 10:1df5a7a265e8 70 MotorSystemTick.detach();
garphil 10:1df5a7a265e8 71 MotorSystemTick.attach_us(callback(this, &Motor::ProcessMotorStateMachine), MotorStepTime);
garphil 10:1df5a7a265e8 72 }
garphil 9:5983c10d5f8e 73 }
garphil 9:5983c10d5f8e 74
garphil 9:5983c10d5f8e 75 void Motor::setSpeed(float sForOneTurn) {
garphil 9:5983c10d5f8e 76 MotorStepTime = 1000*sForOneTurn / MotorFullTurn;
garphil 9:5983c10d5f8e 77 if(MotorStepTime < MOTOR_STEP_TIME_MIN) MotorStepTime = MOTOR_STEP_TIME_MIN;
garphil 11:25d26c72a2f7 78 if(!init) {
garphil 10:1df5a7a265e8 79
garphil 10:1df5a7a265e8 80 MotorSystemTick.detach();
garphil 11:25d26c72a2f7 81 MotorSystemTick.attach_us(callback(this, &Motor::ProcessMotorStateMachine), MotorStepTime);
garphil 10:1df5a7a265e8 82 }
garphil 9:5983c10d5f8e 83 }
garphil 9:5983c10d5f8e 84
garphil 4:c009bcd5518c 85 void Motor::Start() {
garphil 4:c009bcd5518c 86 SetCommand(MOTOR_start);
garphil 4:c009bcd5518c 87 };
garphil 4:c009bcd5518c 88
garphil 4:c009bcd5518c 89 void Motor::Stop() {
garphil 4:c009bcd5518c 90 SetCommand(MOTOR_stop);
garphil 4:c009bcd5518c 91 }
garphil 4:c009bcd5518c 92
garphil 4:c009bcd5518c 93
garphil 4:c009bcd5518c 94 void Motor::Pause() {
garphil 4:c009bcd5518c 95 SetCommand(MOTOR_pause);
garphil 4:c009bcd5518c 96 }
garphil 4:c009bcd5518c 97
garphil 4:c009bcd5518c 98
garphil 4:c009bcd5518c 99 void Motor::Restart() {
garphil 4:c009bcd5518c 100 SetCommand(MOTOR_restart);
garphil 4:c009bcd5518c 101 }
garphil 4:c009bcd5518c 102
garphil 4:c009bcd5518c 103 void Motor::SetZero() {
garphil 4:c009bcd5518c 104 SetCommand(MOTOR_zero);
garphil 4:c009bcd5518c 105 }
garphil 4:c009bcd5518c 106
garphil 6:aec892eb1b49 107 void Motor::RunInfinite(MotorDir dir) {
garphil 6:aec892eb1b49 108 SetDirection( dir);
garphil 6:aec892eb1b49 109 NumSteps = -1;
garphil 6:aec892eb1b49 110 SetCommand(MOTOR_start);
garphil 6:aec892eb1b49 111 }
garphil 6:aec892eb1b49 112
garphil 3:01b4c058454d 113 void Motor::RunSteps(MotorDir dir, uint32_t steps) {
garphil 3:01b4c058454d 114 SetDirection( dir);
garphil 3:01b4c058454d 115 NumSteps = steps;
garphil 3:01b4c058454d 116 SetCommand(MOTOR_start);
garphil 3:01b4c058454d 117 }
garphil 3:01b4c058454d 118
garphil 3:01b4c058454d 119 void Motor::RunDegrees(MotorDir dir, float degree) {
garphil 3:01b4c058454d 120 SetDirection( dir);
garphil 8:4dd59b6f4e92 121 NumSteps = (int)(degree * (float)MotorFullTurn / (float)360.0);
garphil 3:01b4c058454d 122 SetCommand(MOTOR_start);
garphil 3:01b4c058454d 123 }
garphil 2:c91c5ef00d25 124 void Motor::SetDirection(MotorDir dir) {
garphil 2:c91c5ef00d25 125 direction=dir;
garphil 2:c91c5ef00d25 126 }
garphil 2:c91c5ef00d25 127
garphil 2:c91c5ef00d25 128 void Motor::SetCommand(MotorCommand cmd) {
garphil 11:25d26c72a2f7 129 if(init) {
garphil 11:25d26c72a2f7 130 MotorSystemTick.attach_us(callback(this, &Motor::ProcessMotorStateMachine), MotorStepTime);
garphil 11:25d26c72a2f7 131 last=tuneTimings.read_us();
garphil 11:25d26c72a2f7 132 init=false;
garphil 11:25d26c72a2f7 133 }
garphil 2:c91c5ef00d25 134 command=cmd;
garphil 2:c91c5ef00d25 135 }
garphil 3:01b4c058454d 136
garphil 0:bd05fd602a6e 137 void Motor::StopMotor() // --- Stop Motor
garphil 0:bd05fd602a6e 138 {
garphil 1:9519ac966b79 139 *MPh0 = 0; *MPh1 = 0; *MPh2 = 0; *MPh3 = 0;
garphil 1:9519ac966b79 140 MotorIndex = 0;
garphil 4:c009bcd5518c 141
garphil 1:9519ac966b79 142 }
garphil 1:9519ac966b79 143
garphil 1:9519ac966b79 144
garphil 1:9519ac966b79 145 void Motor::StartMotor()
garphil 1:9519ac966b79 146 {
garphil 0:bd05fd602a6e 147 MotorIndex = 0;
garphil 1:9519ac966b79 148 *MPh0 = 1; *MPh1 = 0; *MPh2 = 0; *MPh3 = 0;
garphil 1:9519ac966b79 149 }
garphil 1:9519ac966b79 150
garphil 1:9519ac966b79 151 void Motor::RightMotor() // Move the Motor one step Right
garphil 1:9519ac966b79 152 {
garphil 10:1df5a7a265e8 153 static const int RPh0[8] = {1, 1, 0, 0, 0, 0, 0, 1};
garphil 4:c009bcd5518c 154 static const int RPh1[8] = {0, 1, 1, 1, 0, 0, 0, 0};
garphil 4:c009bcd5518c 155 static const int RPh2[8] = {0, 0, 0, 1, 1, 1, 0, 0};
garphil 4:c009bcd5518c 156 static const int RPh3[8] = {0, 0, 0, 0, 0, 1, 1, 1};
garphil 1:9519ac966b79 157 *MPh0 = RPh0[MotorIndex]; *MPh1 = RPh1[MotorIndex]; *MPh2 = RPh2[MotorIndex]; *MPh3 = RPh3[MotorIndex];
garphil 4:c009bcd5518c 158 if (MotorIndex<7) MotorIndex++;
garphil 1:9519ac966b79 159 else MotorIndex = 0;
garphil 1:9519ac966b79 160 }
garphil 1:9519ac966b79 161 void Motor::LeftMotor() // Move the Motor one step Right
garphil 1:9519ac966b79 162 {
garphil 10:1df5a7a265e8 163 static const int LPh0[8] = {1, 1, 0, 0, 0, 0, 0, 1};
garphil 10:1df5a7a265e8 164 static const int LPh1[8] = {0, 1, 1, 1, 0, 0, 0, 0};
garphil 10:1df5a7a265e8 165 static const int LPh2[8] = {0, 0, 0, 1, 1, 1, 0, 0};
garphil 10:1df5a7a265e8 166 static const int LPh3[8] = {0, 0, 0, 0, 0, 1, 1, 1};
garphil 4:c009bcd5518c 167 *MPh0 = LPh0[MotorIndex]; *MPh1 = LPh1[MotorIndex]; *MPh2 = LPh2[MotorIndex]; *MPh3 = LPh3[MotorIndex];
garphil 10:1df5a7a265e8 168 if (MotorIndex>0) MotorIndex--;
garphil 10:1df5a7a265e8 169 else MotorIndex = 7;
garphil 1:9519ac966b79 170 }
garphil 1:9519ac966b79 171
garphil 1:9519ac966b79 172 void Motor::RunMotor() // Move the Motor in the user direction
garphil 1:9519ac966b79 173 {
garphil 4:c009bcd5518c 174 if (direction==CLOCKWISE) RightMotor();
garphil 1:9519ac966b79 175 else LeftMotor();
garphil 1:9519ac966b79 176 }
garphil 1:9519ac966b79 177
garphil 1:9519ac966b79 178
garphil 1:9519ac966b79 179 void Motor::ProcessMotorStateMachine()
garphil 11:25d26c72a2f7 180 {
garphil 11:25d26c72a2f7 181
garphil 2:c91c5ef00d25 182 if (state==Motor_IDLE) {
garphil 2:c91c5ef00d25 183 if (command == MOTOR_start) {
garphil 3:01b4c058454d 184 // Start the Motor
garphil 1:9519ac966b79 185 StartMotor();
garphil 2:c91c5ef00d25 186 state = Motor_RUN;
garphil 1:9519ac966b79 187 }
garphil 2:c91c5ef00d25 188 else if (command == MOTOR_zero) {
garphil 1:9519ac966b79 189 // Start zeroing the Motor
garphil 1:9519ac966b79 190 StartMotor();
garphil 2:c91c5ef00d25 191 state = Motor_ZERO;
garphil 1:9519ac966b79 192 }
garphil 2:c91c5ef00d25 193 command = MOTOR_nop;
garphil 1:9519ac966b79 194 }
garphil 2:c91c5ef00d25 195 else if (state==Motor_RUN) {
garphil 1:9519ac966b79 196 // Action always performed in that state
garphil 6:aec892eb1b49 197 if (NumSteps>0 || NumSteps <0) {
garphil 1:9519ac966b79 198 RunMotor();
garphil 1:9519ac966b79 199 NumSteps--;
garphil 1:9519ac966b79 200 }
garphil 1:9519ac966b79 201 // Check next state
garphil 2:c91c5ef00d25 202 if (command == MOTOR_pause) {
garphil 2:c91c5ef00d25 203 state = Motor_PAUSE;
garphil 1:9519ac966b79 204 }
garphil 8:4dd59b6f4e92 205 else if ((command == MOTOR_stop)||(NumSteps==0)) {
garphil 1:9519ac966b79 206 StopMotor();
julienPhuong 12:fd881ff72048 207 // if(state != Motor_IDLE ){//|| itOnStop == true) {
julienPhuong 12:fd881ff72048 208 // _callback.call();
julienPhuong 12:fd881ff72048 209 // }
garphil 10:1df5a7a265e8 210 state = Motor_IDLE;
garphil 10:1df5a7a265e8 211 }
garphil 2:c91c5ef00d25 212 command = MOTOR_nop;
garphil 1:9519ac966b79 213 }
garphil 2:c91c5ef00d25 214 else if (state==Motor_PAUSE) {
garphil 2:c91c5ef00d25 215 if (command == MOTOR_stop) {
garphil 1:9519ac966b79 216 StopMotor();
garphil 1:9519ac966b79 217 NumSteps=0;
garphil 2:c91c5ef00d25 218 state = Motor_IDLE;
garphil 1:9519ac966b79 219 }
garphil 2:c91c5ef00d25 220 else if (command == MOTOR_restart) {
garphil 2:c91c5ef00d25 221 state = Motor_RUN;
garphil 1:9519ac966b79 222 }
garphil 2:c91c5ef00d25 223 command = MOTOR_nop;
garphil 1:9519ac966b79 224 }
garphil 2:c91c5ef00d25 225 else if (state==Motor_ZERO) {
garphil 2:c91c5ef00d25 226 command = MOTOR_nop;
garphil 1:9519ac966b79 227 }
garphil 1:9519ac966b79 228 }
garphil 1:9519ac966b79 229
arnophilippe 7:439458133bba 230 MotorStateList Motor::getState() {
arnophilippe 7:439458133bba 231 return state;
arnophilippe 7:439458133bba 232 }
garphil 1:9519ac966b79 233
garphil 1:9519ac966b79 234 void Motor::TestMotor() // Just to check that it make a full taurn back and forth
garphil 1:9519ac966b79 235 {
garphil 1:9519ac966b79 236 int i;
garphil 1:9519ac966b79 237 StartMotor();
garphil 1:9519ac966b79 238 for (i=0; i<MotorFullTurn; i++) {
garphil 1:9519ac966b79 239 wait(0.005);
garphil 1:9519ac966b79 240 RightMotor();
garphil 1:9519ac966b79 241 }
garphil 1:9519ac966b79 242 wait(0.5);
garphil 1:9519ac966b79 243 for (i=0; i<MotorFullTurn; i++) {
garphil 1:9519ac966b79 244 wait(0.005);
garphil 1:9519ac966b79 245 LeftMotor();
garphil 1:9519ac966b79 246 }
garphil 1:9519ac966b79 247 StopMotor();
garphil 0:bd05fd602a6e 248 }