eLab Team / Mbed 2 deprecated myRobot

Working version without LEDs

Dependencies:   mbed WS2812

Voici le dernier schéma de cablage (version du 08/02/2020)

https://os.mbed.com/media/uploads/max_ence/schemarobot_fev2020.pdf

MotorLib/motor.cpp@1:69b5d8f0ba9c, 2020-05-30 (annotated)

Committer:
elab
Date:
Sat May 30 09:31:57 2020 +0000
Revision:
1:69b5d8f0ba9c
Parent:
0:0e577ce96b2f 
pour eLab;

Who changed what in which revision?

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