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@7:439458133bba, 2017-03-09 (annotated)

Committer:
arnophilippe
Date:
Thu Mar 09 11:58:10 2017 +0000
Revision:
7:439458133bba
Parent:
6:aec892eb1b49
Child:
8:4dd59b6f4e92
Add function getting the Motor State

Who changed what in which revision?

UserRevisionLine numberNew contents of line
garphil 0:bd05fd602a6e 1 #include "motor.h"
garphil 0:bd05fd602a6e 2
garphil 0:bd05fd602a6e 3
garphil 4:c009bcd5518c 4 Motor::Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3, uint32_t tickTime) {
garphil 1:9519ac966b79 5
garphil 2:c91c5ef00d25 6 MPh0 = new DigitalOut(_MPh0);
garphil 2:c91c5ef00d25 7 MPh1 = new DigitalOut(_MPh1);
garphil 2:c91c5ef00d25 8 MPh2 = new DigitalOut(_MPh2);
garphil 2:c91c5ef00d25 9 MPh3 = new DigitalOut(_MPh3);
garphil 3:01b4c058454d 10 init = true;
garphil 0:bd05fd602a6e 11 MotorIndex = 0;
garphil 1:9519ac966b79 12 //
garphil 1:9519ac966b79 13 // Connect Interrupt routine in which the motor and all the state machine is performed
garphil 1:9519ac966b79 14 //
garphil 4:c009bcd5518c 15 direction = CLOCKWISE; // Default direction is clockwise
garphil 2:c91c5ef00d25 16 state = Motor_IDLE; // Default state is IDLE
garphil 2:c91c5ef00d25 17 command = MOTOR_nop; // Default command is NOP
garphil 4:c009bcd5518c 18 MotorStepTime = tickTime; // value in micro second for one step
garphil 1:9519ac966b79 19 MotorFullTurn = 2140; // Initial Calibration
garphil 3:01b4c058454d 20 NumSteps = 2000; // Default
garphil 3:01b4c058454d 21
garphil 0:bd05fd602a6e 22 }
garphil 1:9519ac966b79 23
garphil 4:c009bcd5518c 24 void Motor::Start() {
garphil 4:c009bcd5518c 25 SetCommand(MOTOR_start);
garphil 4:c009bcd5518c 26 };
garphil 4:c009bcd5518c 27
garphil 4:c009bcd5518c 28 void Motor::Stop() {
garphil 4:c009bcd5518c 29 SetCommand(MOTOR_stop);
garphil 4:c009bcd5518c 30 }
garphil 4:c009bcd5518c 31
garphil 4:c009bcd5518c 32
garphil 4:c009bcd5518c 33 void Motor::Pause() {
garphil 4:c009bcd5518c 34 SetCommand(MOTOR_pause);
garphil 4:c009bcd5518c 35 }
garphil 4:c009bcd5518c 36
garphil 4:c009bcd5518c 37
garphil 4:c009bcd5518c 38 void Motor::Restart() {
garphil 4:c009bcd5518c 39 SetCommand(MOTOR_restart);
garphil 4:c009bcd5518c 40 }
garphil 4:c009bcd5518c 41
garphil 4:c009bcd5518c 42 void Motor::SetZero() {
garphil 4:c009bcd5518c 43 SetCommand(MOTOR_zero);
garphil 4:c009bcd5518c 44 }
garphil 4:c009bcd5518c 45
garphil 6:aec892eb1b49 46 void Motor::RunInfinite(MotorDir dir) {
garphil 6:aec892eb1b49 47 SetDirection( dir);
garphil 6:aec892eb1b49 48 NumSteps = -1;
garphil 6:aec892eb1b49 49 SetCommand(MOTOR_start);
garphil 6:aec892eb1b49 50 }
garphil 6:aec892eb1b49 51
garphil 3:01b4c058454d 52 void Motor::RunSteps(MotorDir dir, uint32_t steps) {
garphil 3:01b4c058454d 53 SetDirection( dir);
garphil 3:01b4c058454d 54 NumSteps = steps;
garphil 3:01b4c058454d 55 SetCommand(MOTOR_start);
garphil 3:01b4c058454d 56 }
garphil 3:01b4c058454d 57
garphil 3:01b4c058454d 58 void Motor::RunDegrees(MotorDir dir, float degree) {
garphil 3:01b4c058454d 59 SetDirection( dir);
garphil 4:c009bcd5518c 60 NumSteps = (int)(degree * MotorFullTurn / 360.0);
garphil 3:01b4c058454d 61 SetCommand(MOTOR_start);
garphil 3:01b4c058454d 62 }
garphil 2:c91c5ef00d25 63 void Motor::SetDirection(MotorDir dir) {
garphil 2:c91c5ef00d25 64 direction=dir;
garphil 2:c91c5ef00d25 65 }
garphil 2:c91c5ef00d25 66
garphil 2:c91c5ef00d25 67 void Motor::SetCommand(MotorCommand cmd) {
garphil 3:01b4c058454d 68 if(init) MotorSystemTick.attach_us(this, &Motor::ProcessMotorStateMachine, MotorStepTime);
garphil 3:01b4c058454d 69 init = false;
garphil 2:c91c5ef00d25 70 command=cmd;
garphil 2:c91c5ef00d25 71 }
garphil 3:01b4c058454d 72
garphil 0:bd05fd602a6e 73 void Motor::StopMotor() // --- Stop Motor
garphil 0:bd05fd602a6e 74 {
garphil 1:9519ac966b79 75 *MPh0 = 0; *MPh1 = 0; *MPh2 = 0; *MPh3 = 0;
garphil 1:9519ac966b79 76 MotorIndex = 0;
garphil 4:c009bcd5518c 77
garphil 1:9519ac966b79 78 }
garphil 1:9519ac966b79 79
garphil 1:9519ac966b79 80
garphil 1:9519ac966b79 81 void Motor::StartMotor()
garphil 1:9519ac966b79 82 {
garphil 0:bd05fd602a6e 83 MotorIndex = 0;
garphil 1:9519ac966b79 84 *MPh0 = 1; *MPh1 = 0; *MPh2 = 0; *MPh3 = 0;
garphil 1:9519ac966b79 85 }
garphil 1:9519ac966b79 86
garphil 1:9519ac966b79 87 void Motor::RightMotor() // Move the Motor one step Right
garphil 1:9519ac966b79 88 {
garphil 4:c009bcd5518c 89 static const int RPh0[8] = {1, 1, 0, 0, 0, 0, 0, 1};
garphil 4:c009bcd5518c 90 static const int RPh1[8] = {0, 1, 1, 1, 0, 0, 0, 0};
garphil 4:c009bcd5518c 91 static const int RPh2[8] = {0, 0, 0, 1, 1, 1, 0, 0};
garphil 4:c009bcd5518c 92 static const int RPh3[8] = {0, 0, 0, 0, 0, 1, 1, 1};
garphil 1:9519ac966b79 93 *MPh0 = RPh0[MotorIndex]; *MPh1 = RPh1[MotorIndex]; *MPh2 = RPh2[MotorIndex]; *MPh3 = RPh3[MotorIndex];
garphil 4:c009bcd5518c 94 if (MotorIndex<7) MotorIndex++;
garphil 1:9519ac966b79 95 else MotorIndex = 0;
garphil 1:9519ac966b79 96 }
garphil 1:9519ac966b79 97 void Motor::LeftMotor() // Move the Motor one step Right
garphil 1:9519ac966b79 98 {
garphil 4:c009bcd5518c 99 static const int LPh0[8] = { 1, 0, 0, 0, 0, 0, 1, 1};
garphil 4:c009bcd5518c 100 static const int LPh1[8] = { 0, 0, 0, 0, 1, 1, 1, 0};
garphil 4:c009bcd5518c 101 static const int LPh2[8] = { 0, 0, 1, 1, 1, 0, 0, 0};
garphil 4:c009bcd5518c 102 static const int LPh3[8] = { 1, 1, 1, 0, 0, 0, 0, 0};
garphil 4:c009bcd5518c 103 *MPh0 = LPh0[MotorIndex]; *MPh1 = LPh1[MotorIndex]; *MPh2 = LPh2[MotorIndex]; *MPh3 = LPh3[MotorIndex];
garphil 4:c009bcd5518c 104 if (MotorIndex<7) MotorIndex++;
garphil 1:9519ac966b79 105 else MotorIndex = 0;
garphil 1:9519ac966b79 106 }
garphil 1:9519ac966b79 107
garphil 1:9519ac966b79 108 void Motor::RunMotor() // Move the Motor in the user direction
garphil 1:9519ac966b79 109 {
garphil 4:c009bcd5518c 110 if (direction==CLOCKWISE) RightMotor();
garphil 1:9519ac966b79 111 else LeftMotor();
garphil 1:9519ac966b79 112 }
garphil 1:9519ac966b79 113
garphil 1:9519ac966b79 114
garphil 1:9519ac966b79 115 void Motor::ProcessMotorStateMachine()
garphil 3:01b4c058454d 116 {
garphil 2:c91c5ef00d25 117 if (state==Motor_IDLE) {
garphil 2:c91c5ef00d25 118 if (command == MOTOR_start) {
garphil 3:01b4c058454d 119 // Start the Motor
garphil 1:9519ac966b79 120 StartMotor();
garphil 2:c91c5ef00d25 121 state = Motor_RUN;
garphil 1:9519ac966b79 122 }
garphil 2:c91c5ef00d25 123 else if (command == MOTOR_zero) {
garphil 1:9519ac966b79 124 // Start zeroing the Motor
garphil 1:9519ac966b79 125 StartMotor();
garphil 2:c91c5ef00d25 126 state = Motor_ZERO;
garphil 1:9519ac966b79 127 }
garphil 2:c91c5ef00d25 128 command = MOTOR_nop;
garphil 1:9519ac966b79 129 }
garphil 2:c91c5ef00d25 130 else if (state==Motor_RUN) {
garphil 1:9519ac966b79 131 // Action always performed in that state
garphil 6:aec892eb1b49 132 if (NumSteps>0 || NumSteps <0) {
garphil 1:9519ac966b79 133 RunMotor();
garphil 1:9519ac966b79 134 NumSteps--;
garphil 1:9519ac966b79 135 }
garphil 1:9519ac966b79 136 // Check next state
garphil 2:c91c5ef00d25 137 if (command == MOTOR_pause) {
garphil 2:c91c5ef00d25 138 state = Motor_PAUSE;
garphil 1:9519ac966b79 139 }
garphil 2:c91c5ef00d25 140 else if ((command == MOTOR_stop)||(NumSteps<=0)) {
garphil 1:9519ac966b79 141 StopMotor();
garphil 2:c91c5ef00d25 142 state = Motor_IDLE;
garphil 1:9519ac966b79 143 }
garphil 2:c91c5ef00d25 144 command = MOTOR_nop;
garphil 1:9519ac966b79 145 }
garphil 2:c91c5ef00d25 146 else if (state==Motor_PAUSE) {
garphil 2:c91c5ef00d25 147 if (command == MOTOR_stop) {
garphil 1:9519ac966b79 148 StopMotor();
garphil 1:9519ac966b79 149 NumSteps=0;
garphil 2:c91c5ef00d25 150 state = Motor_IDLE;
garphil 1:9519ac966b79 151 }
garphil 2:c91c5ef00d25 152 else if (command == MOTOR_restart) {
garphil 2:c91c5ef00d25 153 state = Motor_RUN;
garphil 1:9519ac966b79 154 }
garphil 2:c91c5ef00d25 155 command = MOTOR_nop;
garphil 1:9519ac966b79 156 }
garphil 2:c91c5ef00d25 157 else if (state==Motor_ZERO) {
garphil 2:c91c5ef00d25 158 command = MOTOR_nop;
garphil 1:9519ac966b79 159 }
garphil 1:9519ac966b79 160 }
garphil 1:9519ac966b79 161
arnophilippe 7:439458133bba 162 MotorStateList Motor::getState() {
arnophilippe 7:439458133bba 163 return state;
arnophilippe 7:439458133bba 164 }
garphil 1:9519ac966b79 165
garphil 1:9519ac966b79 166 void Motor::TestMotor() // Just to check that it make a full taurn back and forth
garphil 1:9519ac966b79 167 {
garphil 1:9519ac966b79 168 int i;
garphil 1:9519ac966b79 169 StartMotor();
garphil 1:9519ac966b79 170 for (i=0; i<MotorFullTurn; i++) {
garphil 1:9519ac966b79 171 wait(0.005);
garphil 1:9519ac966b79 172 RightMotor();
garphil 1:9519ac966b79 173 }
garphil 1:9519ac966b79 174 wait(0.5);
garphil 1:9519ac966b79 175 for (i=0; i<MotorFullTurn; i++) {
garphil 1:9519ac966b79 176 wait(0.005);
garphil 1:9519ac966b79 177 LeftMotor();
garphil 1:9519ac966b79 178 }
garphil 1:9519ac966b79 179 StopMotor();
garphil 0:bd05fd602a6e 180 }