CreaMot.cpp

00001 #include "CreaMot.h"
00002 
00003   // -------------------- CreaMot Class ---------------------------
00004 
00005 CreaMot::CreaMot(PinName _MPh[4]) {
00006     initialization( _MPh , MOTOR_STEP_TIME_DEFAULT_US);
00007 }
00008 
00009 CreaMot::CreaMot(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3) {
00010     PinName _MPh[4] = {_MPh0, _MPh1, _MPh2, _MPh3};
00011     initialization( _MPh , MOTOR_STEP_TIME_DEFAULT_US);
00012 }
00013 
00014 CreaMot::CreaMot(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3, uint32_t AStepTime_us) {
00015     PinName _MPh[4] = {_MPh0, _MPh1, _MPh2, _MPh3};
00016     initialization( _MPh, AStepTime_us);
00017 }
00018 
00019 void CreaMot::initialization(PinName _MPh[4], uint32_t AStepTime_us) {
00020 
00021     for (int ph=0; ph<4; ph++) { MPh[ph] = new DigitalOut(_MPh[ph]); } 
00022     
00023     MotorOFF();    // Default state is all phases are OFF
00024     StepPhase = 0; // initial phase is Zero
00025     
00026     setCommand(MOTOR_nop, CLOCKWISE, 0);// Default command is NOP, clockwise direction, 0 steps
00027     
00028     TickIsAttached = false;
00029     StepTime_us = AStepTime_us;// duration in micro second for one step
00030 
00031     Steps_FullTurn = MOTOR_STEPS_FOR_A_TURN;  // Default Calibration value
00032     _callback = NULL; // No default Callback
00033     
00034     // All geometric information defaults to 0:
00035     diam_cm          = 0; // wheel diameter in centimeter
00036     perim_cm         = 0; // wheel perimeter in centimeter
00037     degree_per_cm    = 0; // rotation angle in degrees per cm circumference
00038     defaultDirection = CLOCKWISE;
00039 }
00040 
00041 // *****************************************************************
00042 //   all following functions are based on centimeter information
00043 // *****************************************************************
00044 
00045 /* High Level: Run CreaMot for a given number of Dist_cm, Dist_cm<0 are run in opposite direction. */
00046 void CreaMot::RunDist_cm(bool AClockWise, float Dist_cm)
00047 {  RunDegrees(AClockWise,Dist_cm * degree_per_cm); }
00048 
00049 /* High Level: Run CreaMot for a given number of Dist_cm in default direction. */
00050 void CreaMot::RunDist_cm(float Dist_cm)
00051 {  RunDegrees(defaultDirection, Dist_cm * degree_per_cm); }
00052 
00053 /** Additional geometric information: set the wheel diameter, also sets perimeter and degrees per cm.*/
00054 void CreaMot::setDiamCM( float Adiam_cm) 
00055 {   diam_cm = Adiam_cm;
00056     perim_cm = PI*diam_cm;
00057     degree_per_cm=360.0f/perim_cm;
00058 } 
00059 
00060 /** Calculate the needed wheel angle from a turn angle and a turn_radius_cm  */
00061 void CreaMot::RunTurnAngle(float turn_angle_deg, float turn_radius_cm) 
00062 {   // a turn radius smaller than 0 make no sense is not executed
00063     if( turn_radius_cm>= 0 ) 
00064         RunDegrees(  turn_angle_deg * PI_OVER_180 * turn_radius_cm * degree_per_cm);
00065 } 
00066 
00067 void CreaMot::setSpeed_cm_sec(float speed_cm_sec)
00068 {   if (speed_cm_sec < MIN_SPEED_CM_SEC) // catch too small or negative speeds
00069           setRotationPeriodSec( perim_cm / MIN_SPEED_CM_SEC); 
00070     else  setRotationPeriodSec( perim_cm / speed_cm_sec );
00071 }
00072 
00073 // *****************************************************************
00074 //  all following functions are agnostic of centimeter-information
00075 // *****************************************************************
00076 
00077 void CreaMot::RunInfinite(bool AClockWise) 
00078 {   setCommand(MOTOR_run, AClockWise, -1);
00079     StartTick();
00080 }
00081 
00082 /* High Level: Run CreaMot for a given angle, angles<0 are run in opposite direction. */
00083 void CreaMot::RunDegrees(bool AClockWise, float angle_deg) {
00084  RunSteps( AClockWise, (int32_t)(angle_deg * (float)Steps_FullTurn / 360.0f) );   
00085 }
00086 
00087 /* High Level: Run CreaMot for a given angle in default direction, angles<0 are run in opposite direction. */
00088 void CreaMot::RunDegrees(float angle_deg) {
00089  RunSteps( defaultDirection, (int32_t)(angle_deg * (float)Steps_FullTurn / 360.0f) );   
00090 }
00091 
00092 /* High Level: Run CreaMot for a given number of steps, steps<0 are run in opposite direction. */
00093 void CreaMot::RunSteps(bool AClockWise, int32_t steps) 
00094 {   if (steps<0) { AClockWise = !AClockWise; steps=-steps; }
00095     if (steps>0) 
00096         { setCommand( MOTOR_run, AClockWise, steps );
00097           StartTick(); }
00098 }
00099 
00100 void CreaMot::PauseRun() 
00101 {   if (CurrCmd==MOTOR_run) CurrCmd = MOTOR_pause;  }
00102 
00103 void CreaMot::RestartRun() 
00104 {   if (CurrCmd==MOTOR_pause) CurrCmd = MOTOR_run;  }
00105 
00106 // not only halt the state machine but also set NSteps to 0
00107 void CreaMot::StopRun() 
00108 {   setCommand(MOTOR_stop,ClockWise,0);  } 
00109 
00110 void CreaMot::setCommand(motCmands aCmd, bool aClockWise, int32_t  aNSteps) {
00111     CurrCmd    = aCmd;
00112     ClockWise  = aClockWise;
00113     NStepsToDo = aNSteps;
00114 };
00115 
00116 
00117 /*******************************************************
00118  **   Ticker / Timing procedures
00119  *******************************************************/
00120 //Get, set the scaling
00121 uint32_t CreaMot::getStepsFullTurn()
00122 {    return Steps_FullTurn;  }
00123 
00124 void CreaMot::setStepsFullTurn(uint32_t StepsFullTurn) 
00125 {   Steps_FullTurn = StepsFullTurn; }
00126 
00127 void CreaMot::setRotationPeriodSec(float Seconds_Per_Turn) {
00128     // rescale to usec and pass on to the next handler. 
00129     setStepTime_us( uint32_t(Seconds_Per_Turn / Steps_FullTurn * 1000000) ) ;
00130 }
00131 void CreaMot::setStepTime_us(uint32_t AStepTime_us) {
00132     if(StepTime_us == AStepTime_us) return; // avoid futile activity
00133     if (StepTime_us < MOTOR_STEP_TIME_MIN_US) // filter too small values
00134        StepTime_us = MOTOR_STEP_TIME_MIN_US;
00135        else StepTime_us = AStepTime_us; // or if OK then assign value
00136     // if ticker already running recreate a new ticker;
00137     if(TickIsAttached) { StopTick(); StartTick(); }
00138  }
00139 
00140 void CreaMot::StartTick() {
00141     if(!TickIsAttached)   {
00142         // Connect Interrupt routine in which the CreaMot and all the state machine is performed
00143         MotorSysTick.attach_us(callback(this, &CreaMot::ProcessMotorStateMachine), StepTime_us);
00144         // last=tuneTimings.read_us();
00145         TickIsAttached=true;
00146         }
00147 }
00148 
00149 void CreaMot::ProcessMotorStateMachine()
00150 {
00151     switch(CurrCmd) {
00152         case MOTOR_run: {
00153             switch(CurrState) {
00154                 case Motor_OFF:
00155                     MotorON(); // First only turn on the CreaMot ..
00156                     break;
00157                 case Motor_ZERO:
00158                 case Motor_ON:
00159                         CurrState = Motor_RUN;
00160                 case Motor_RUN:
00161                     if (NStepsToDo==0)  // No more steps to go
00162                         { CurrCmd = MOTOR_stop; 
00163                           if (_callback) _callback.call(); } 
00164                       else // More steps to go
00165                         { StepOnce(); 
00166                         NStepsToDo--;}
00167                     break;
00168                 } // switch(CurrState)
00169             } // case MOTOR_run
00170         case MOTOR_stop:
00171             StopTick();
00172             CurrCmd = MOTOR_nop; 
00173             MotorOFF(); 
00174             break;       
00175         case MOTOR_nop:
00176         case MOTOR_pause:
00177         default: break;
00178     } // switch(CurrCmd)
00179 }
00180 
00181 void CreaMot::StopTick() {
00182     if(TickIsAttached)   
00183       { MotorSysTick.detach(); TickIsAttached=false; }
00184 }
00185 
00186 /*******************************************************
00187  **   all the low level direct CreaMot HW access
00188  *******************************************************/
00189  
00190 
00191  void CreaMot::MotorTest()    // Just to check that it make a full turn back and forth
00192 {
00193     int i;
00194     MotorON();
00195     for (i=0; i<Steps_FullTurn; i++) {
00196         wait(0.005);
00197         StepClkW();
00198         }   
00199     wait(0.5);
00200     for (i=0; i<Steps_FullTurn; i++) {
00201         wait(0.005);
00202         StepCCW();
00203         }   
00204     MotorOFF();
00205 }
00206 
00207  /** Turn off all CreaMot Phases, no more current flowing */
00208 void CreaMot::MotorOFF() 
00209 {   for (int ph=0; ph<4; ph++) *MPh[ph] = 0;  
00210     CurrState=Motor_OFF;
00211 }
00212 
00213 /** Turn on the CreaMot Phase, In the last used phase, memorized in StepPhases 
00214 *  Equivalent to what previously the function "void Start();" did */
00215 void CreaMot::MotorON()
00216 {   SetPhases(); // attention, does not change StepPhase!
00217     if (StepPhase==0)  CurrState=Motor_ZERO;  
00218         else CurrState=Motor_ON;
00219 } 
00220 
00221 /** CreaMot phases turned on and put to Zero Position*/    
00222 void CreaMot::MotorZero() {
00223     StepPhase = 0;  // sets the phases to 0
00224     SetPhases(); 
00225     CurrState=Motor_ZERO;
00226 }
00227 
00228 void    CreaMot::StepOnce()     // Move the CreaMot in the requested 'direction'
00229 {   if (ClockWise) StepClkW(); else StepCCW();
00230 }
00231 
00232 void    CreaMot::StepClkW()    // Move the CreaMot one step Clockwise
00233 {   if (StepPhase<7) StepPhase++;  else  StepPhase = 0;
00234     SetPhases();
00235 }
00236 void    CreaMot::StepCCW()    // Move the CreaMot one step Clockwise
00237 {   if (StepPhase>0) StepPhase--;  else  StepPhase = 7;
00238     SetPhases();
00239 }
00240 
00241 static const int MotPh[4][8] = 
00242     { {1, 1, 0, 0, 0, 0, 0, 1}, 
00243       {0, 1, 1, 1, 0, 0, 0, 0}, 
00244       {0, 0, 0, 1, 1, 1, 0, 0}, 
00245       {0, 0, 0, 0, 0, 1, 1, 1}};
00246       
00247 void    CreaMot::SetPhases()    // Engage CreaMot Phases according to StepPhase
00248 { for (int ph=0; ph<4; ph++) { *MPh[ph] = MotPh[ph][StepPhase]; }   }
00249