Diff: motor.cpp

Revision:

22:d2c742bdae16

Parent:

17:86e5af6f7628

Child:

23:6060422cd6eb

--- a/motor.cpp	Tue Jan 01 21:44:20 2019 +0000
+++ b/motor.cpp	Thu Jan 03 23:15:42 2019 +0000
@@ -3,9 +3,9 @@
 
  // -------------------- MotStatus Helper ---------------------------
 
-void MotStatus::set(motorCommands aCmd, motorDir aDir, int32_t  aNSteps) {
+void MotStatus::set(motCmands aCmd, bool AClockWise, int32_t  aNSteps) {
     cmd = aCmd;
-    dir = aDir;
+    AClockWise = AClockWise;
     NSteps  = aNSteps;
 };
 
@@ -20,19 +20,14 @@
     initialization( _MPh , MOTOR_STEP_TIME_DEFAULT_US);
 }
 
-Motor::Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3, uint32_t aStepTime_us) {
+Motor::Motor(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3, uint32_t AStepTime_us) {
     PinName _MPh[4] = {_MPh0, _MPh1, _MPh2, _MPh3};
-    initialization( _MPh, aStepTime_us);
+    initialization( _MPh, AStepTime_us);
 }
 
-//void Motor::initialization(PinName _MPh0, PinName _MPh1, PinName _MPh2, PinName _MPh3, uint32_t aStepTime_us) {
-void Motor::initialization(PinName _MPh[4], uint32_t aStepTime_us) {
+void Motor::initialization(PinName _MPh[4], uint32_t AStepTime_us) {
 
     for (int ph=0; ph<4; ph++) { MPh[ph] = new DigitalOut(_MPh[ph]); } 
-    /*MPh[0] = new DigitalOut(_MPh0);
-    MPh[1] = new DigitalOut(_MPh1);
-    MPh[2] = new DigitalOut(_MPh2);
-    MPh[3] = new DigitalOut(_MPh3);*/
     
     MotorOFF();    // Default state is all phases are OFF
     StepPhase = 0; // initial phase is Zero
@@ -40,35 +35,74 @@
     Status.set(MOTOR_nop, CLOCKWISE, 0);// Default command is NOP, clockwise direction, 0 steps
     
     Status.TickIsAttached = false;
-    StepTime_us = aStepTime_us;// duration in micro second for one step
+    StepTime_us = AStepTime_us;// duration in micro second for one step
 
     Steps_FullTurn = MOTOR_STEPS_FOR_A_TURN;  // Default Calibration value
     _callback = NULL; // No default Callback
-
-    // itOnStop = true;    
-    // tuneTimings.reset();
-    // tuneTimings.start();
+    
+    // All geometric information defaults to 0:
+    rotate_angle_deg = 0; // calculated wheel rotation angle 
+    diam_cm          = 0; // wheel diameter in centimeter
+    perim_cm         = 0; // wheel perimeter in centimeter
+    degree_per_cm    = 0; // rotation angle in degrees per cm circumference
+    defaultDirection = CLOCKWISE;
 }
 
-//Attaching and removing Callbacks
-void Motor::callbackSet(void (*CBfunction)(void))
-{  _callback = CBfunction;  }
+// *****************************************************************
+//   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 Motor for a given number of centimeters in default direction. */
+void Motor::RunCentimeters  (float centimeters);
+{  RunDegrees(defaultDirection, centimeters * 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  */
+{   diam_cm = Adiam_cm;
+    perim_cm = PI*diam_cm;
+    degree_per_cm=360.0f/perim_cm;
+} 
 
-void Motor::callbackRemove() 
-{ _callback = NULL; }
+/** 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) 
+{   // 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;
+        else rotate_angle_deg = 0;
+    return rotate_angle_deg;
+} 
 
-void Motor::RunInfinite(motorDir direction) {
-    Status.set(MOTOR_run, direction, -1);
+void Motor::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 );
+}
+
+// *****************************************************************
+//  all following functions are agnostic of centimeter-information
+// *****************************************************************
+
+void Motor::RunInfinite(bool AClockWise) {
+    Status.set(MOTOR_run, AClockWise, -1);
     StartTick();
 }
 
-void Motor::RunSteps(motorDir direction, uint32_t steps) {
-    if (steps>0) 
-        { Status.set(MOTOR_run, direction, steps);
-        StartTick(); }
+/* High Level: Run Motor for a given angle, angles<0 are run in opposite direction. */
+void Motor::RunDegrees(bool AClockWise, float angle_deg) {
+ RunSteps( AClockWise, (int32_t)(angle_deg * (float)Steps_FullTurn / 360.0f) );   
 }
-void Motor::RunDegrees(motorDir direction, float angle_deg) {
-    RunSteps(direction, (int)(angle_deg * (float)Steps_FullTurn / (float)360.0) );   
+
+/* 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) {
+    if (steps!=0) 
+        { Status.set(MOTOR_run, AClockWise ^^ (steps<0), abs(steps));
+          StartTick(); }
 }
 
 void Motor::PauseRun() 
@@ -94,13 +128,13 @@
 
 void Motor::setRotationPeriodSec(float Seconds_Per_Turn) {
     // rescale to usec and pass on to the next handler. 
-    setStepTime_us(1000 * Seconds_Per_Turn / Steps_FullTurn) ;
+    setStepTime_us( uint32_t(Seconds_Per_Turn / Steps_FullTurn * 1000000) ) ;
 }
-void Motor::setStepTime_us(uint32_t aStepTime_us) {
-    if(StepTime_us == aStepTime_us) return; // avoid futile activity
+void Motor::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
+       else StepTime_us = AStepTime_us; // or if OK then assign value
     // if ticker already running recreate a new ticker;
     if(Status.TickIsAttached) { StopTick(); StartTick(); }
  }
@@ -192,7 +226,7 @@
 }
 
 void    Motor::StepOnce()     // Move the Motor in the used 'direction'
-{   if (Status.dir == CLOCKWISE) StepClkW(); else StepCCW();
+{   if (Status.AClockWise) StepClkW(); else StepCCW();
 }
 
 void    Motor::StepClkW()    // Move the Motor one step Clockwise