motor.cpp

00001 #include "motor.h"
00002 
00003 void Motor::initialization(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3, uint32_t tickTime) {
00004 //pc_uart.printf("MOTOR INIT\n");
00005     MPh0 = new DigitalOut(_MPh0);
00006     MPh1 = new DigitalOut(_MPh1);
00007     MPh2 = new DigitalOut(_MPh2);
00008     MPh3 = new DigitalOut(_MPh3);
00009         init = true;
00010     MotorIndex = 0;
00011     //
00012     // Connect Interrupt routine in which the motor and all the state machine is performed
00013     //
00014     direction = CLOCKWISE;         // Default direction is clockwise
00015     state = Motor_IDLE;    // Default state is IDLE
00016     command = MOTOR_nop;       // Default command is NOP
00017     MotorStepTime = tickTime;      // value in micro second for one step
00018     MotorFullTurn = MOTOR_TICKS_FOR_A_TURN;       // Initial Calibration
00019     NumSteps = MotorFullTurn;               // Default 1 turn
00020     itOnStop = true;
00021             tuneTimings.reset();
00022          tuneTimings.start();
00023 }
00024 
00025 Motor::Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3) {
00026     initialization( _MPh0,  _MPh1,  _MPh2,  _MPh3, MOTOR_STEP_TIME_DEFAULT);
00027  
00028 }
00029 
00030 
00031 Motor::Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3, uint32_t tickTime) {
00032  
00033     initialization( _MPh0,  _MPh1,  _MPh2,  _MPh3, tickTime);
00034  
00035 }
00036 
00037 
00038 void Motor::removeMotorCallback() {
00039     
00040     _callback = NULL;
00041     
00042 }
00043 
00044 void Motor::setMotorCallback(void (*function)(void))
00045 {
00046     setMotorCallback(function, true);
00047 }
00048 void Motor::setMotorCallback(void (*function)(void), bool onStop)
00049 {
00050     _callback = function;
00051     itOnStop = onStop;  
00052 }
00053 
00054 
00055 uint32_t Motor::getCalibration()
00056 {
00057     return MotorFullTurn;
00058 }
00059 
00060 void Motor::setCalibration(uint32_t nbTicksforFullTurn) {
00061     MotorFullTurn = nbTicksforFullTurn;
00062 }
00063 
00064 void Motor::setDelayBtwTicks(uint32_t tickTime) {
00065    if(MotorStepTime == tickTime) return;
00066     MotorStepTime = tickTime;
00067     if(MotorStepTime < MOTOR_STEP_TIME_MIN)  MotorStepTime = MOTOR_STEP_TIME_MIN;
00068     // pc_uart.printf("Change ticktime to %d %d\n\r",tickTime, MotorStepTime);
00069    if(!init) {
00070         MotorSystemTick.detach();
00071         MotorSystemTick.attach_us(callback(this, &Motor::ProcessMotorStateMachine), MotorStepTime);
00072    }
00073 }
00074 
00075 void Motor::setSpeed(float sForOneTurn) {
00076     MotorStepTime = 1000*sForOneTurn / MotorFullTurn;
00077     if(MotorStepTime < MOTOR_STEP_TIME_MIN)  MotorStepTime = MOTOR_STEP_TIME_MIN;
00078     if(!init) {
00079         
00080         MotorSystemTick.detach();
00081        MotorSystemTick.attach_us(callback(this, &Motor::ProcessMotorStateMachine), MotorStepTime);
00082     }
00083 }
00084 
00085 void Motor::Start() {
00086         SetCommand(MOTOR_start);   
00087 };
00088 
00089 void Motor::Stop() {
00090         SetCommand(MOTOR_stop);
00091 }
00092 
00093 
00094 void Motor::Pause() {
00095         SetCommand(MOTOR_pause);
00096 }
00097 
00098 
00099 void Motor::Restart() {
00100         SetCommand(MOTOR_restart);
00101 }
00102 
00103 void Motor::SetZero() {
00104         SetCommand(MOTOR_zero);
00105 }
00106 
00107 void Motor::RunInfinite(MotorDir dir) {
00108     SetDirection( dir);
00109     NumSteps = -1;
00110     SetCommand(MOTOR_start);
00111 }
00112 
00113 void Motor::RunSteps(MotorDir dir, uint32_t steps) {
00114     SetDirection( dir);
00115     NumSteps = steps;
00116     SetCommand(MOTOR_start);
00117 }
00118 
00119 void Motor::RunDegrees(MotorDir dir, float degree) {
00120     SetDirection( dir);
00121     NumSteps = (int)(degree * (float)MotorFullTurn / (float)360.0);
00122     SetCommand(MOTOR_start);
00123 }
00124 void Motor::SetDirection(MotorDir dir) {
00125     direction=dir;
00126 }
00127 
00128 void Motor::SetCommand(MotorCommand cmd) {
00129     if(init)   {
00130         MotorSystemTick.attach_us(callback(this, &Motor::ProcessMotorStateMachine), MotorStepTime);
00131         last=tuneTimings.read_us();
00132         init=false;
00133         }
00134     command=cmd;
00135 }
00136 
00137 void Motor::StopMotor() // --- Stop Motor
00138 {
00139         *MPh0 = 0;  *MPh1 = 0;  *MPh2 = 0;  *MPh3 = 0;
00140         MotorIndex = 0;
00141    
00142 }
00143 
00144 
00145 void    Motor::StartMotor()
00146 {
00147         MotorIndex = 0;
00148         *MPh0 = 1;  *MPh1 = 0;  *MPh2 = 0;   *MPh3 = 0;
00149 }
00150 
00151 void    Motor::RightMotor()    // Move the Motor one step Right
00152 {
00153     static const   int RPh0[8] =  {1, 1, 0, 0, 0, 0, 0, 1};
00154     static const   int RPh1[8] = {0, 1, 1, 1, 0, 0, 0, 0};
00155     static const   int RPh2[8] = {0, 0, 0, 1, 1, 1, 0, 0};
00156     static const   int RPh3[8] = {0, 0, 0, 0, 0, 1, 1, 1};
00157     *MPh0 = RPh0[MotorIndex];  *MPh1 = RPh1[MotorIndex];  *MPh2 = RPh2[MotorIndex];  *MPh3 = RPh3[MotorIndex];
00158     if (MotorIndex<7) MotorIndex++;
00159     else    MotorIndex = 0;
00160 }
00161 void    Motor::LeftMotor()    // Move the Motor one step Right
00162 {
00163     static const   int LPh0[8] =  {1, 1, 0, 0, 0, 0, 0, 1};
00164     static const   int LPh1[8] = {0, 1, 1, 1, 0, 0, 0, 0};
00165     static const   int LPh2[8] = {0, 0, 0, 1, 1, 1, 0, 0};
00166     static const   int LPh3[8] = {0, 0, 0, 0, 0, 1, 1, 1};
00167    *MPh0 = LPh0[MotorIndex];  *MPh1 = LPh1[MotorIndex];  *MPh2 = LPh2[MotorIndex];  *MPh3 = LPh3[MotorIndex];
00168     if (MotorIndex>0) MotorIndex--;
00169     else    MotorIndex = 7;
00170 }
00171 
00172 void    Motor::RunMotor()     // Move the Motor in the user direction
00173 {
00174         if (direction==CLOCKWISE) RightMotor();
00175         else LeftMotor();
00176 }
00177 
00178 
00179 void Motor::ProcessMotorStateMachine()
00180 {        
00181 
00182         if (state==Motor_IDLE) {
00183             if (command == MOTOR_start) {
00184                 // Start the Motor
00185                 StartMotor();
00186                 state = Motor_RUN;
00187                 }
00188             else if (command == MOTOR_zero) {
00189                 // Start zeroing the Motor
00190                 StartMotor();
00191                 state = Motor_ZERO;
00192                 }
00193             command = MOTOR_nop;
00194             }
00195         else if (state==Motor_RUN) {
00196             // Action always performed in that state
00197              if (NumSteps>0 || NumSteps <0) {
00198                 RunMotor();
00199                 NumSteps--;
00200                 }
00201             // Check next state
00202             if (command == MOTOR_pause) {
00203                 state = Motor_PAUSE;
00204                 }
00205             else if ((command == MOTOR_stop)||(NumSteps==0)) {
00206                 StopMotor();
00207                      if(state != Motor_IDLE ){//|| itOnStop == true) {
00208                         _callback.call();
00209                     }
00210                 state = Motor_IDLE;
00211                }
00212             command = MOTOR_nop;
00213             }
00214         else if (state==Motor_PAUSE) {
00215             if (command == MOTOR_stop) {
00216                 StopMotor();
00217                 NumSteps=0;
00218                 state = Motor_IDLE;
00219                 }
00220             else if (command == MOTOR_restart) {
00221                 state = Motor_RUN;
00222                 }
00223             command = MOTOR_nop;
00224             }
00225         else if (state==Motor_ZERO) {
00226             command = MOTOR_nop;
00227             }
00228 }
00229 
00230 MotorStateList Motor::getState() {
00231    return state;    
00232 }
00233 
00234 void Motor::TestMotor()    // Just to check that it make a full taurn back and forth
00235 {
00236     int i;
00237     StartMotor();
00238     for (i=0; i<MotorFullTurn; i++) {
00239         wait(0.005);
00240         RightMotor();
00241         }   
00242     wait(0.5);
00243     for (i=0; i<MotorFullTurn; i++) {
00244         wait(0.005);
00245         LeftMotor();
00246         }   
00247     StopMotor();
00248 }