Diff: motor.cpp

Revision:

23:6060422cd6eb

Parent:

22:d2c742bdae16

--- a/motor.cpp	Thu Jan 03 23:15:42 2019 +0000
+++ b/motor.cpp	Wed Apr 17 21:48:14 2019 +0000
@@ -1,40 +1,32 @@
 #include "motor.h"
 
 
- // -------------------- MotStatus Helper ---------------------------
+  // -------------------- CreaMot Class ---------------------------
 
-void MotStatus::set(motCmands aCmd, bool AClockWise, int32_t  aNSteps) {
-    cmd = aCmd;
-    AClockWise = AClockWise;
-    NSteps  = aNSteps;
-};
-
- // -------------------- Motor Class ---------------------------
-
-Motor::Motor(PinName _MPh[4]) {
+CreaMot::CreaMot(PinName _MPh[4]) {
     initialization( _MPh , MOTOR_STEP_TIME_DEFAULT_US);
 }
 
-Motor::Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3) {
+CreaMot::CreaMot(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3) {
     PinName _MPh[4] = {_MPh0, _MPh1, _MPh2, _MPh3};
     initialization( _MPh , MOTOR_STEP_TIME_DEFAULT_US);
 }
 
-Motor::Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3, uint32_t AStepTime_us) {
+CreaMot::CreaMot(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3, uint32_t AStepTime_us) {
     PinName _MPh[4] = {_MPh0, _MPh1, _MPh2, _MPh3};
     initialization( _MPh, AStepTime_us);
 }
 
-void Motor::initialization(PinName _MPh[4], uint32_t AStepTime_us) {
+void CreaMot::initialization(PinName _MPh[4], uint32_t AStepTime_us) {
 
     for (int ph=0; ph<4; ph++) { MPh[ph] = new DigitalOut(_MPh[ph]); } 
     
     MotorOFF();    // Default state is all phases are OFF
     StepPhase = 0; // initial phase is Zero
     
-    Status.set(MOTOR_nop, CLOCKWISE, 0);// Default command is NOP, clockwise direction, 0 steps
+    setStatus(MOTOR_nop, CLOCKWISE, 0);// Default command is NOP, clockwise direction, 0 steps
     
-    Status.TickIsAttached = false;
+    TickIsAttached = false;
     StepTime_us = AStepTime_us;// duration in micro second for one step
 
     Steps_FullTurn = MOTOR_STEPS_FOR_A_TURN;  // Default Calibration value
@@ -51,26 +43,24 @@
 // *****************************************************************
 //   all following functions are based on centimeter information
 // *****************************************************************
-#define PI 3.141592f    /** PI needed to calcuate from angle to distances */
-#define PI_OVER_180 (PI/180.0f)  /** needed to translate from angle to circumference */
 
-/* High Level: Run Motor for a given number of centimeters, centimeters<0 are run in opposite direction. */
-void Motor::RunCentimeters  (bool AClockWise, float centimeters);
-{  RunDegrees(AClockWise, centimeters * degree_per_cm; ) }
+/* High Level: Run CreaMot for a given number of Dist_cm, Dist_cm<0 are run in opposite direction. */
+void CreaMot::RunDist_cm  (bool AClockWise, float Dist_cm);
+{  RunDegrees(AClockWise, Dist_cm * degree_per_cm; ) }
 
-/* High Level: Run Motor for a given number of centimeters in default direction. */
-void Motor::RunCentimeters  (float centimeters);
-{  RunDegrees(defaultDirection, centimeters * degree_per_cm; ) }
+/* High Level: Run CreaMot for a given number of Dist_cm in default direction. */
+void CreaMot::RunDist_cm  (float Dist_cm);
+{  RunDegrees(defaultDirection, Dist_cm * degree_per_cm; ) }
 
 /** Additional geometric information: set the wheel diameter, also sets perimeter and degrees per cm.*/
-void Motor:setDiamCM( float Adiam_cm) /**< Helper; set diameter and perim to values  */
+void CreaMot:setDiamCM( float Adiam_cm) /**< Helper; set diameter and perim to values  */
 {   diam_cm = Adiam_cm;
     perim_cm = PI*diam_cm;
     degree_per_cm=360.0f/perim_cm;
 } 
 
 /** Helper: calculate the needed wheel angle from a turn angle and a turn_radius_cm  */
-float Motor:calcAngle(float turn_angle_deg, float turn_radius_cm) 
+float CreaMot:RunTurnAngle(float turn_angle_deg, float turn_radius_cm) 
 {   // a turn radius smaller than 0 make no sense
     if( turn_radius_cm>= 0 ) 
         rotate_angle_deg = turn_angle_deg * PI_OVER_180 * turn_radius_cm * degree_per_cm;
@@ -78,7 +68,7 @@
     return rotate_angle_deg;
 } 
 
-void Motor::setSpeed_cm_sec(float speed_cm_sec)
+void CreaMot::setSpeed_cm_sec(float speed_cm_sec)
 {   if (speed_cm_sec < MIN_SPEED_CM_SEC) // catch too small or negative speeds
           setRotationPeriodSec( perim_cm / MIN_SPEED_CM_SEC); 
     else  setRotationPeriodSec( perim_cm / speed_cm_sec );
@@ -88,107 +78,120 @@
 //  all following functions are agnostic of centimeter-information
 // *****************************************************************
 
-void Motor::RunInfinite(bool AClockWise) {
-    Status.set(MOTOR_run, AClockWise, -1);
+void CreaMot::RunInfinite(bool AClockWise) {
+    setStatus(MOTOR_run, AClockWise, -1);
     StartTick();
 }
 
-/* High Level: Run Motor for a given angle, angles<0 are run in opposite direction. */
-void Motor::RunDegrees(bool AClockWise, float angle_deg) {
+/* High Level: Run CreaMot for a given angle, angles<0 are run in opposite direction. */
+void CreaMot::RunDegrees(bool AClockWise, float angle_deg) {
  RunSteps( AClockWise, (int32_t)(angle_deg * (float)Steps_FullTurn / 360.0f) );   
 }
 
-/* High Level: Run Motor for a given number of steps, steps<0 are run in opposite direction. */
-void Motor::RunSteps(bool AClockWise, int32_t steps) {
+/* High Level: Run CreaMot for a given angle in default direction, angles<0 are run in opposite direction. */
+void CreaMot::RunDegrees(float angle_deg) {
+ RunSteps( defaultDirection, (int32_t)(angle_deg * (float)Steps_FullTurn / 360.0f) );   
+}
+
+/* High Level: Run CreaMot for a given number of steps, steps<0 are run in opposite direction. */
+void CreaMot::RunSteps(bool AClockWise, int32_t steps) {
     if (steps!=0) 
-        { Status.set(MOTOR_run, AClockWise ^^ (steps<0), abs(steps));
+        { setStatus(MOTOR_run, AClockWise ^^ (steps<0), abs(steps));
           StartTick(); }
 }
 
-void Motor::PauseRun() 
-{   if (Status.cmd==MOTOR_run) Status.cmd = MOTOR_pause;  }
+void CreaMot::PauseRun() 
+{   if (CurrCmd==MOTOR_run) CurrCmd = MOTOR_pause;  }
 
-void Motor::RestartRun() 
-{   if (Status.cmd==MOTOR_pause) Status.cmd = MOTOR_run;  }
+void CreaMot::RestartRun() 
+{   if (CurrCmd==MOTOR_pause) CurrCmd = MOTOR_run;  }
 
-void Motor::StopRun() 
-{   Status.cmd = MOTOR_stop;  } 
+void CreaMot::StopRun() 
+{   CurrCmd = MOTOR_stop;  } 
 
-MotStatus Motor::getStatus() { return Status; }
+MotStatus CreaMot::getStatus() { return Status; }
 
 /*******************************************************
  **   Ticker / Timing procedures
  *******************************************************/
 //Get, set the scaling
-uint32_t Motor::getStepsFullTurn()
+uint32_t CreaMot::getStepsFullTurn()
 {    return Steps_FullTurn;  }
 
-void Motor::setStepsFullTurn(uint32_t StepsFullTurn) 
+void CreaMot::setStepsFullTurn(uint32_t StepsFullTurn) 
 {   Steps_FullTurn = StepsFullTurn; }
 
-void Motor::setRotationPeriodSec(float Seconds_Per_Turn) {
+void CreaMot::setRotationPeriodSec(float Seconds_Per_Turn) {
     // rescale to usec and pass on to the next handler. 
     setStepTime_us( uint32_t(Seconds_Per_Turn / Steps_FullTurn * 1000000) ) ;
 }
-void Motor::setStepTime_us(uint32_t AStepTime_us) {
+void CreaMot::setStepTime_us(uint32_t AStepTime_us) {
     if(StepTime_us == AStepTime_us) return; // avoid futile activity
     if (StepTime_us < MOTOR_STEP_TIME_MIN_US) // filter too small values
        StepTime_us = MOTOR_STEP_TIME_MIN_US;
        else StepTime_us = AStepTime_us; // or if OK then assign value
     // if ticker already running recreate a new ticker;
-    if(Status.TickIsAttached) { StopTick(); StartTick(); }
+    if(TickIsAttached) { StopTick(); StartTick(); }
  }
 
-void Motor::StartTick() {
-    if(!Status.TickIsAttached)   {
-        // Connect Interrupt routine in which the Motor and all the state machine is performed
-        MotorSysTick.attach_us(callback(this, &Motor::ProcessMotorStateMachine), StepTime_us);
+void CreaMot::StartTick() {
+    if(!TickIsAttached)   {
+        // Connect Interrupt routine in which the CreaMot and all the state machine is performed
+        MotorSysTick.attach_us(callback(this, &CreaMot::ProcessMotorStateMachine), StepTime_us);
         // last=tuneTimings.read_us();
-        Status.TickIsAttached=true;
+        TickIsAttached=true;
         }
 }
 
-void Motor::ProcessMotorStateMachine()
+void CreaMot::ProcessMotorStateMachine()
 {
-    switch(Status.cmd) {
+    switch(CurrCmd) {
         case MOTOR_run: {
-            switch(Status.state) {
+            switch(CurrState) {
                 case Motor_OFF:
-                    MotorON(); // First only turn on the Motor ..
+                    MotorON(); // First only turn on the CreaMot ..
                     break;
                 case Motor_ZERO:
                 case Motor_ON:
-                        Status.state = Motor_RUN;
+                        CurrState = Motor_RUN;
                 case Motor_RUN:
-                    if (Status.NSteps==0)  // No more steps to go
-                        { Status.cmd = MOTOR_stop; 
+                    if (NStepsToDo==0)  // No more steps to go
+                        { CurrCmd = MOTOR_stop; 
                           if (_callback) _callback.call(); } 
                       else // More steps to go
                         { StepOnce(); 
-                        Status.NSteps--;}
+                        NStepsToDo--;}
                     break;
-                } // switch(Status.state)
+                } // switch(CurrState)
             } // case MOTOR_run
         case MOTOR_stop:
             StopTick();
-            Status.cmd = MOTOR_nop; 
+            CurrCmd = MOTOR_nop; 
             MotorOFF(); 
             break;       
         case MOTOR_nop:
         case MOTOR_pause:
         default: break;
-    } // switch(Status.cmd)
+    } // switch(CurrCmd)
 }
 
-void Motor::StopTick() {
-    if(Status.TickIsAttached)   
-      { MotorSysTick.detach(); Status.TickIsAttached=false; }
+void CreaMot::StopTick() {
+    if(TickIsAttached)   
+      { MotorSysTick.detach(); TickIsAttached=false; }
 }
 
 /*******************************************************
- **   all the low level direct Motor HW access
+ **   all the low level direct CreaMot HW access
  *******************************************************/
- void Motor::MotorTest()    // Just to check that it make a full turn back and forth
+ 
+void CreaMot::setStatus(motCmands aCmd, bool AClockWise, int32_t  aNSteps) {
+    cmd = aCmd;
+    AClockWise = AClockWise;
+    NSteps  = aNSteps;
+};
+
+
+ void CreaMot::MotorTest()    // Just to check that it make a full turn back and forth
 {
     int i;
     MotorON();
@@ -204,36 +207,36 @@
     MotorOFF();
 }
 
- /** Turn off all Motor Phases, no more current flowing */
-void Motor::MotorOFF() 
+ /** Turn off all CreaMot Phases, no more current flowing */
+void CreaMot::MotorOFF() 
 {   for (int ph=0; ph<4; ph++) *MPh[ph] = 0;  
-    Status.state=Motor_OFF;
+    CurrState=Motor_OFF;
 }
 
-/** Turn on the Motor Phase, In the last used phase, memorized in StepPhases 
+/** Turn on the CreaMot Phase, In the last used phase, memorized in StepPhases 
 *  Equivalent to what previously the function "void Start();" did */
-void Motor::MotorON()
+void CreaMot::MotorON()
 {   SetPhases(); // attention, does not change StepPhase!
-    if (StepPhase==0)  Status.state=Motor_ZERO;  
-        else Status.state=Motor_ON;
+    if (StepPhase==0)  CurrState=Motor_ZERO;  
+        else CurrState=Motor_ON;
 } 
 
-/** Motor phases turned on and put to Zero Position*/    
-void Motor::MotorZero() {
+/** CreaMot phases turned on and put to Zero Position*/    
+void CreaMot::MotorZero() {
     StepPhase = 0;  // sets the phases to 0
     SetPhases(); 
-    Status.state=Motor_ZERO;
+    CurrState=Motor_ZERO;
 }
 
-void    Motor::StepOnce()     // Move the Motor in the used 'direction'
+void    CreaMot::StepOnce()     // Move the CreaMot in the Status 'direction'
 {   if (Status.AClockWise) StepClkW(); else StepCCW();
 }
 
-void    Motor::StepClkW()    // Move the Motor one step Clockwise
+void    CreaMot::StepClkW()    // Move the CreaMot one step Clockwise
 {   if (StepPhase<7) StepPhase++;  else  StepPhase = 0;
     SetPhases();
 }
-void    Motor::StepCCW()    // Move the Motor one step Clockwise
+void    CreaMot::StepCCW()    // Move the CreaMot one step Clockwise
 {   if (StepPhase>0) StepPhase--;  else  StepPhase = 7;
     SetPhases();
 }
@@ -244,6 +247,6 @@
       {0, 0, 0, 1, 1, 1, 0, 0}, 
       {0, 0, 0, 0, 0, 1, 1, 1}};
       
-void    Motor::SetPhases()    // Engage Motor Phases according to StepPhase
+void    CreaMot::SetPhases()    // Engage CreaMot Phases according to StepPhase
 { for (int ph=0; ph<4; ph++) { *MPh[ph] = MotPh[ph][StepPhase]; }   }