Tarek Lule / MotorLib

My Version of the Crealab MotorLib.

Fork of MotorLib by CreaLab

motor.cpp@4:c009bcd5518c, 2016-08-23 (annotated)

Committer:
garphil
Date:
Tue Aug 23 11:33:46 2016 +0000
Revision:
4:c009bcd5518c
Parent:
3:01b4c058454d
Child:
6:aec892eb1b49
Added Start Stop function directly... instead of setcommand(Motor_Start)...;

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 3:01b4c058454d 46 void Motor::RunSteps(MotorDir dir, uint32_t steps) {
garphil 3:01b4c058454d 47 SetDirection( dir);
garphil 3:01b4c058454d 48 NumSteps = steps;
garphil 3:01b4c058454d 49 SetCommand(MOTOR_start);
garphil 3:01b4c058454d 50 }
garphil 3:01b4c058454d 51
garphil 3:01b4c058454d 52 void Motor::RunDegrees(MotorDir dir, float degree) {
garphil 3:01b4c058454d 53 SetDirection( dir);
garphil 4:c009bcd5518c 54 NumSteps = (int)(degree * MotorFullTurn / 360.0);
garphil 3:01b4c058454d 55 SetCommand(MOTOR_start);
garphil 3:01b4c058454d 56 }
garphil 2:c91c5ef00d25 57 void Motor::SetDirection(MotorDir dir) {
garphil 2:c91c5ef00d25 58 direction=dir;
garphil 2:c91c5ef00d25 59 }
garphil 2:c91c5ef00d25 60
garphil 2:c91c5ef00d25 61 void Motor::SetCommand(MotorCommand cmd) {
garphil 3:01b4c058454d 62 if(init) MotorSystemTick.attach_us(this, &Motor::ProcessMotorStateMachine, MotorStepTime);
garphil 3:01b4c058454d 63 init = false;
garphil 2:c91c5ef00d25 64 command=cmd;
garphil 2:c91c5ef00d25 65 }
garphil 3:01b4c058454d 66
garphil 0:bd05fd602a6e 67 void Motor::StopMotor() // --- Stop Motor
garphil 0:bd05fd602a6e 68 {
garphil 1:9519ac966b79 69 *MPh0 = 0; *MPh1 = 0; *MPh2 = 0; *MPh3 = 0;
garphil 1:9519ac966b79 70 MotorIndex = 0;
garphil 4:c009bcd5518c 71
garphil 1:9519ac966b79 72 }
garphil 1:9519ac966b79 73
garphil 1:9519ac966b79 74
garphil 1:9519ac966b79 75 void Motor::StartMotor()
garphil 1:9519ac966b79 76 {
garphil 0:bd05fd602a6e 77 MotorIndex = 0;
garphil 1:9519ac966b79 78 *MPh0 = 1; *MPh1 = 0; *MPh2 = 0; *MPh3 = 0;
garphil 1:9519ac966b79 79 }
garphil 1:9519ac966b79 80
garphil 1:9519ac966b79 81 void Motor::RightMotor() // Move the Motor one step Right
garphil 1:9519ac966b79 82 {
garphil 4:c009bcd5518c 83 static const int RPh0[8] = {1, 1, 0, 0, 0, 0, 0, 1};
garphil 4:c009bcd5518c 84 static const int RPh1[8] = {0, 1, 1, 1, 0, 0, 0, 0};
garphil 4:c009bcd5518c 85 static const int RPh2[8] = {0, 0, 0, 1, 1, 1, 0, 0};
garphil 4:c009bcd5518c 86 static const int RPh3[8] = {0, 0, 0, 0, 0, 1, 1, 1};
garphil 1:9519ac966b79 87 *MPh0 = RPh0[MotorIndex]; *MPh1 = RPh1[MotorIndex]; *MPh2 = RPh2[MotorIndex]; *MPh3 = RPh3[MotorIndex];
garphil 4:c009bcd5518c 88 if (MotorIndex<7) MotorIndex++;
garphil 1:9519ac966b79 89 else MotorIndex = 0;
garphil 1:9519ac966b79 90 }
garphil 1:9519ac966b79 91 void Motor::LeftMotor() // Move the Motor one step Right
garphil 1:9519ac966b79 92 {
garphil 4:c009bcd5518c 93 static const int LPh0[8] = { 1, 0, 0, 0, 0, 0, 1, 1};
garphil 4:c009bcd5518c 94 static const int LPh1[8] = { 0, 0, 0, 0, 1, 1, 1, 0};
garphil 4:c009bcd5518c 95 static const int LPh2[8] = { 0, 0, 1, 1, 1, 0, 0, 0};
garphil 4:c009bcd5518c 96 static const int LPh3[8] = { 1, 1, 1, 0, 0, 0, 0, 0};
garphil 4:c009bcd5518c 97 *MPh0 = LPh0[MotorIndex]; *MPh1 = LPh1[MotorIndex]; *MPh2 = LPh2[MotorIndex]; *MPh3 = LPh3[MotorIndex];
garphil 4:c009bcd5518c 98 if (MotorIndex<7) MotorIndex++;
garphil 1:9519ac966b79 99 else MotorIndex = 0;
garphil 1:9519ac966b79 100 }
garphil 1:9519ac966b79 101
garphil 1:9519ac966b79 102 void Motor::RunMotor() // Move the Motor in the user direction
garphil 1:9519ac966b79 103 {
garphil 4:c009bcd5518c 104 if (direction==CLOCKWISE) RightMotor();
garphil 1:9519ac966b79 105 else LeftMotor();
garphil 1:9519ac966b79 106 }
garphil 1:9519ac966b79 107
garphil 1:9519ac966b79 108
garphil 1:9519ac966b79 109 void Motor::ProcessMotorStateMachine()
garphil 3:01b4c058454d 110 {
garphil 2:c91c5ef00d25 111 if (state==Motor_IDLE) {
garphil 2:c91c5ef00d25 112 if (command == MOTOR_start) {
garphil 3:01b4c058454d 113 // Start the Motor
garphil 1:9519ac966b79 114 StartMotor();
garphil 2:c91c5ef00d25 115 state = Motor_RUN;
garphil 1:9519ac966b79 116 }
garphil 2:c91c5ef00d25 117 else if (command == MOTOR_zero) {
garphil 1:9519ac966b79 118 // Start zeroing the Motor
garphil 1:9519ac966b79 119 StartMotor();
garphil 2:c91c5ef00d25 120 state = Motor_ZERO;
garphil 1:9519ac966b79 121 }
garphil 2:c91c5ef00d25 122 command = MOTOR_nop;
garphil 1:9519ac966b79 123 }
garphil 2:c91c5ef00d25 124 else if (state==Motor_RUN) {
garphil 1:9519ac966b79 125 // Action always performed in that state
garphil 1:9519ac966b79 126 if (NumSteps>0) {
garphil 1:9519ac966b79 127 RunMotor();
garphil 1:9519ac966b79 128 NumSteps--;
garphil 1:9519ac966b79 129 }
garphil 1:9519ac966b79 130 // Check next state
garphil 2:c91c5ef00d25 131 if (command == MOTOR_pause) {
garphil 2:c91c5ef00d25 132 state = Motor_PAUSE;
garphil 1:9519ac966b79 133 }
garphil 2:c91c5ef00d25 134 else if ((command == MOTOR_stop)||(NumSteps<=0)) {
garphil 1:9519ac966b79 135 StopMotor();
garphil 2:c91c5ef00d25 136 state = Motor_IDLE;
garphil 1:9519ac966b79 137 }
garphil 2:c91c5ef00d25 138 command = MOTOR_nop;
garphil 1:9519ac966b79 139 }
garphil 2:c91c5ef00d25 140 else if (state==Motor_PAUSE) {
garphil 2:c91c5ef00d25 141 if (command == MOTOR_stop) {
garphil 1:9519ac966b79 142 StopMotor();
garphil 1:9519ac966b79 143 NumSteps=0;
garphil 2:c91c5ef00d25 144 state = Motor_IDLE;
garphil 1:9519ac966b79 145 }
garphil 2:c91c5ef00d25 146 else if (command == MOTOR_restart) {
garphil 2:c91c5ef00d25 147 state = Motor_RUN;
garphil 1:9519ac966b79 148 }
garphil 2:c91c5ef00d25 149 command = MOTOR_nop;
garphil 1:9519ac966b79 150 }
garphil 2:c91c5ef00d25 151 else if (state==Motor_ZERO) {
garphil 2:c91c5ef00d25 152 command = MOTOR_nop;
garphil 1:9519ac966b79 153 }
garphil 1:9519ac966b79 154 }
garphil 1:9519ac966b79 155
garphil 1:9519ac966b79 156
garphil 1:9519ac966b79 157 void Motor::TestMotor() // Just to check that it make a full taurn back and forth
garphil 1:9519ac966b79 158 {
garphil 1:9519ac966b79 159 int i;
garphil 1:9519ac966b79 160 StartMotor();
garphil 1:9519ac966b79 161 for (i=0; i<MotorFullTurn; i++) {
garphil 1:9519ac966b79 162 wait(0.005);
garphil 1:9519ac966b79 163 RightMotor();
garphil 1:9519ac966b79 164 }
garphil 1:9519ac966b79 165 wait(0.5);
garphil 1:9519ac966b79 166 for (i=0; i<MotorFullTurn; i++) {
garphil 1:9519ac966b79 167 wait(0.005);
garphil 1:9519ac966b79 168 LeftMotor();
garphil 1:9519ac966b79 169 }
garphil 1:9519ac966b79 170 StopMotor();
garphil 0:bd05fd602a6e 171 }