Diff: CreaBot.cpp

Revision:

10:79509113310a

Parent:

9:efe9f76d6f76

Child:

11:5a94af0afa12

--- a/CreaBot.cpp	Thu Jan 03 23:16:46 2019 +0000
+++ b/CreaBot.cpp	Wed Apr 17 21:48:00 2019 +0000
@@ -23,17 +23,17 @@
 //    Creabot Class
 // *****************************************************************
 
-Creabot::Creabot(motor *left, motor *right, float diameter_wheel_cm, float bot_diam_cm):
-      wheelLeft(left), wheelRight(right), 
+Creabot::Creabot(CreaMot *left, CreaMot *right, float diameter_wheel_cm, float bot_diam_cm):
+      wheelLeft(left), wheelRight(right) 
 {
     // TODO: To catch for illegal dimensions (<= 0)
     
     // Initialize wheel geometires
-    wheelLeft.setDiam(wheel_diam_cm);
-    wheelRight.setDiam(wheel_diam_cm);
+    wheelLeft ->diam_cm = diameter_wheel_cm;
+    wheelRight->diam_cm = diameter_wheel_cm;
     // setup the bot parameters:
-    botDiameter = bot_diam_cm;
-    ratio_wheel_bot=bot_diam_cm/wheel_diam_cm;
+    botDiameter     = bot_diam_cm;
+    ratio_wheel_bot = bot_diam_cm/diameter_wheel_cm;
     botSpeed_cm_sec = DEFAULT_SPEED_CM_SEC;
 
     // Attach the Crabot functions as callbacks to the wheels
@@ -92,7 +92,7 @@
         // in this case qCollide = false, and wheelsState!=LRWHEELS_STOP
         if ( qCollide  || (wheelsState==LRWHEELS_STOP) )
             // callback cannot happen here since the wheelState is already STOP, or Collide has happened before!
-            pExecuteNext();
+            qExecuteNext();
       };
 }
 
@@ -104,7 +104,7 @@
 void Creabot::qExecuteNext()
 {   if ( !qIsEmpty() ) //  see if something in the queue to execute
       { // Execute and then remove the oldest element at the tail of the Queue
-        iExeCommand( cmd[readIdx] );
+        iExeCommand( &cmd[readIdx] );
         readIdx++;
         if (readIdx==DEPTH_Queue) readIdx=0;
         Count--;
@@ -123,26 +123,26 @@
 //**************************************************************************
 
 void Creabot::iMoveAndWait(BotCmdVerb moveType, float Aparam)
-{   botMove(moveType,Aparam);
-    botWaitEnd();
+{   iMove(moveType,Aparam,Aparam);
+    iWaitEnd();
 }
 
 void Creabot::iMoveAndWait(BotCmdVerb moveType, float Aturn_angle_deg, float Adist_cm)
-{   botMove(moveType,Aturn_angle_deg,Adist_cm);
-    botWaitEnd();
+{   iMove(moveType,Aturn_angle_deg,Adist_cm);
+    iWaitEnd();
 }
 
 void Creabot::iMove(BotCmdVerb moveType, float Aparam)
-{   botMove(moveType, Aparam, Aparam);   }
+{   iMove(moveType, Aparam, Aparam);   }
 
 void Creabot::iMove(BotCmdVerb moveType,  float Aturn_angle_deg, float Adist_cm)
 {   BotCommand Anew_cmd;
     Anew_cmd.set(moveType, Aturn_angle_deg, Adist_cm);
-    executeCommand(&Anew_cmd);
+    iExeCommand(&Anew_cmd);
 }
 
 /** High level, immediate: move bot according to prefilled command structures.
-* Recommended to use botMove() methods to fill the command structure correctly. 
+* Recommended to use iMove() methods to fill the command structure correctly. 
 * Branches to the moveXXXX() methods. For details see docs for those methods. 
 * Warning: Collides with queued commands if called individually. 
 * @param[in] <*cmd> Pointer to type BotCommand, the prefilled command structure,*/
@@ -169,7 +169,6 @@
             case ROTATE:
                 moveRotate(cmd->turn_angle_deg);
                 break;
-            case IDLE:
             default:
                 break;
         };
@@ -193,8 +192,9 @@
     wheelRight->StopRun();
     // wheelsAllStoppedCB could handle the rest of the cleanup, but would also trigger external callback
     // Turn motor power off!
-    wheelLeft->WheelOFF();
-    wheelRight->WheelOFF();
+    wheelLeft->MotorOFF();
+    wheelRight->MotorOFF();
+    wheelsState = LRWHEELS_STOP;
 }
 
 // *****************************************************************
@@ -212,71 +212,92 @@
 
 // Mid level control function:  advance bot straight forward for a given distance
 void Creabot::moveForward(float dist_cm)
-{   if (dist_cm<0) moveBackward(-dist_cm);
-    else {
-        float rot_angle_deg = dist_cm*wheeldims.degree_per_cm;
-        wheelsSetSpeed(botSpeed_cm_sec);
-        wheelLeftRun (rot_angle_deg);
-        wheelRightRun(rot_angle_deg);
-        }     
+{   
+    wheelsSetSpeed(botSpeed_cm_sec);
+    wheelRight->RunDist_cm(dist_cm );
+    wheelLeft ->RunDist_cm(dist_cm ); // Direction = defaultDirection
+    // negative distances are run by RunDist_cm in opposite direction!
+    wheelsState = LRWHEELS_RUN;
 }
 
 // Mid level control function:  reverse bot straight backwards for a given distance
 void Creabot::moveBackward(float dist_cm)
 {
-    if(dist_cm<0) moveForward(-dist_cm);
-    else {
-        float rot_angle_deg = dist_cm*wheeldims.degree_per_cm;
-        wheelsSetSpeed(botSpeed_cm_sec);
-        wheelLeftRun (-rot_angle_deg);
-        wheelRightRun(-rot_angle_deg);
-        }
+    wheelsSetSpeed(botSpeed_cm_sec);
+    wheelRight->RunDist_cm(-dist_cm);
+    wheelLeft ->RunDist_cm(-dist_cm);// Direction = defaultDirection
+    // negative distance are run by RunDist_cm in opposite direction!
+    wheelsState = LRWHEELS_RUN;
 }
 
 /* Mid level control function: turn bot forward right, 
-   around a radius twice the bot size */
+   around a radius equl the bot size; i.e. turn around the right wheel */
 void Creabot::moveRight(float turn_angle_deg)
 {
     wheelLeft->setSpeed_cm_sec(botSpeed_cm_sec);
-    wheelLeftRun(COUNTERCLOCKWISE, wheelLeft.calcAngle(turn_angle_deg,botDiameter));
+    wheelLeft->RunTurnAngle(turn_angle_deg,botDiameter);
+    wheelsState = LWHEEL_RUNS;
 }
 
 /* Mid level control function: turn bot forward right, 
-around a radius that is center_cm away from the right wheel*/
+around a radius that is turn_radius_cm away from the right wheel*/
 void Creabot::moveRight(float turn_angle_deg, float turn_radius_cm)
-{   float wheelAngLeft  = wheelLeft.calcAngle(turn_angle_deg,turn_radius_cm+botDiameter);
-    float wheelAngRight = wheelRight.calcAngle(turn_angle_deg,turn_radius_cm);
-    wheelLeft->setSpeed_cm_sec(botSpeed_cm_sec);
-    wheelRight->setSpeed_cm_sec(botSpeed_cm_sec * wheelAngRight/wheelAngLeft);
-    wheelLeftRun(COUNTERCLOCKWISE, angleLeft);
-    wheelRightRun(CLOCKWISE, angleRight);
+{   // If Center_cm is too small the right wheel has practically nothing to do ;)
+    if (abs(turn_radius_cm)<0.1) { moveRight(turn_angle_deg); } 
+    else {
+        wheelLeft ->setSpeed_cm_sec(botSpeed_cm_sec);
+        wheelRight->setSpeed_cm_sec(botSpeed_cm_sec * turn_radius_cm/turn_radius_cm+botDiameter);
+        wheelRight->RunTurnAngle(turn_angle_deg,turn_radius_cm            );
+        wheelLeft ->RunTurnAngle(turn_angle_deg,turn_radius_cm+botDiameter);// Direction = defaultDirection
+        // negative distance are run by RunTurnAngle in opposite direction!
+        wheelsState = LRWHEELS_RUN;
+      } // else
 }
 
+/* Mid level control function: turn bot backwards Right, 
+   around a radius that is turn_radius_cm away from the Right wheel */
+void Creabot::moveBackRight(float turn_angle_deg, float turn_radius_cm)
+{ moveRight( -turn_angle_deg , turn_radius_cm); }
+
+/* Mid level control function: turn bot backwards Right, 
+   around a radius equal to the bot size; = the Right wheel stands still */
+void Creabot::moveBackRight(float turn_angle_deg)
+{ moveRight( - turn_angle_deg); }
 
 /* Mid level control function: turn bot forward left, 
-  around a radius twice the bot size */
+   around a radius equal to the bot size; = the left wheel stands still */
 void Creabot::moveLeft(float turn_angle_deg)
 {
-    moveLeft(turn_angle_deg,0);
+    wheelRight>setSpeed_cm_sec(botSpeed_cm_sec);
+    wheelRight->RunTurnAngle(turn_angle_deg,botDiameter);
+    wheelsState = RWHEEL_RUNS;
 }
 
 /* Mid level control function: turn bot forward left, 
-around a radius that is center_cm away from the left wheel*/
-void Creabot::moveLeft(float turn_angle_deg, float center_cm)
-{
-    if (center_cm<0) center_cm=0; /* TO remove? */
-    float perimeter_outer = 2.0f*turn_angle_deg*(center_cm+botDiameter)*PI/360.0f;
-    float perimeter_inner = 2.0f*turn_angle_deg*(center_cm)*PI/360.0f;
-    float angleLeft = perimeter_inner*wheeldims.degree_per_cm;
-    float angleRight = perimeter_outer*wheeldims.degree_per_cm;
-    float ratio = angleLeft/angleRight;
-    float speedRLeft = botSpeed_cm_sec * ratio;
-    wheelLeft->setSpeed_cm_sec(speedRLeft);
-    wheelRight->setSpeed_cm_sec(botSpeed_cm_sec);
-    wheelLeftRun(COUNTERCLOCKWISE, angleLeft);
-    wheelRightRun(CLOCKWISE, angleRight);
+   around a radius that is turn_radius_cm away from the left wheel */
+void Creabot::moveLeft(float turn_angle_deg, float turn_radius_cm)
+{   // If Center_cm is too small the left wheel has practically nothing to do ;)
+    if (abs(turn_radius_cm)<0.1) { moveLeft(turn_angle_deg); } 
+    else {
+        wheelLeft -> setSpeed_cm_sec(botSpeed_cm_sec * turn_radius_cm/(turn_radius_cm+botDiameter));
+        wheelRight-> setSpeed_cm_sec(botSpeed_cm_sec);
+        wheelLeft -> RunTurnAngle(turn_angle_deg,turn_radius_cm);
+        wheelRight-> RunTurnAngle(turn_angle_deg,turn_radius_cm+botDiameter);// Direction = defaultDirection
+        // negative distance are run by RunTurnAngle in opposite direction!
+        wheelsState = LRWHEELS_RUN;
+        }
 }
 
+/* Mid level control function: turn bot backwards left, 
+   around a radius that is turn_radius_cm away from the left wheel */
+void Creabot::moveBackLeft(float turn_angle_deg, float turn_radius_cm)
+{ moveLeft( - turn_angle_deg, turn_radius_cm); }
+
+/* Mid level control function: turn bot backwards left, 
+   around a radius equal to the bot size; = the left wheel stands still */
+void Creabot::moveBackLeft(float turn_angle_deg)
+{ moveLeft( - turn_angle_deg); }
+
 /* Mid level control function: turn bot on its place for a given angle.
 positive angles turn right, negative angles turn left*/
 void Creabot::moveRotate(float angleBot)
@@ -284,12 +305,15 @@
     float angleWheel = angleBot*ratio_wheel_bot;
     setSpeed(botSpeed_cm_sec);
     if(angleBot<0) {
-        wheelLeftRun(CLOCKWISE, -angleWheel);
-        wheelRightRun(CLOCKWISE, -angleWheel);
+        wheelRight->RunDegrees( angleWheel);
+        wheelLeft ->RunDegrees(-angleWheel);// Direction = defaultDirection
+        // negative distance are run by RunDegrees in opposite direction!
     } else {
-        wheelLeftRun(COUNTERCLOCKWISE, angleWheel);
-        wheelRightRun(COUNTERCLOCKWISE, angleWheel);
+        wheelLeft ->RunDegrees( angleWheel);
+        wheelRight->RunDegrees(-angleWheel);// Direction = defaultDirection
+        // negative distance are run by RunDegrees in opposite direction!
     }
+    wheelsState = LRWHEELS_RUN;
 }
 
 //******************************************************************************
@@ -300,33 +324,10 @@
 /* Low level access functions: sets both wheel speeds immediately  */ 
 void Creabot::wheelsSetSpeed(float mot_speed_cm_sec)
 {
-    wheelLeft->setSpeed_cm_sec(mot_speed_cm_sec);
+    wheelLeft ->setSpeed_cm_sec(mot_speed_cm_sec);
     wheelRight->setSpeed_cm_sec(mot_speed_cm_sec);
 }
 
-
-void Creabot::wheelLeftRun(float rot_angle_deg)
-{   if (rot_angle_deg>=0) wheelLeft->RunDegrees(wheelLeft->defaultDirection, rot_angle_deg);
-        else wheelLeft->RunDegrees(! wheelLeft->defaultDirection, -rot_angle_deg);
-        
-    if ( (wheelsState==RWHEEL_RUNS) || (wheelsState==LRWHEELS_RUN) ) wheelsState=LRWHEELS_RUN;
-            else  wheelsState=LWHEEL_RUNS;
-    }
-}
-
-void Creabot::wheelRightRun(float rot_angle_deg)
-{   if(rot_angle_deg>0) {
-        wheelRight->RunDegrees(dir, rot_angle_deg);
-        if ( (wheelsState==LWHEEL_RUNS) || (wheelsState==LRWHEELS_RUN) ) wheelsState=LRWHEELS_RUN;
-          else  wheelsState=RWHEEL_RUNS;
-    }
-}
-
-void Creabot::wheelRunCentimeters(AWheel *Wheel, float dist_cm)
-{   AWheel->RunCentimeters(dist_cm); // Direction used is defaultDirection
-    wheelsState = wheelsState | AWheel->StateValue; // bitwise or of state value, can be either LWHEEL_RUNS or RWHEEL_RUNS
-}
-
 //******************************************************************************
 // low level wheel functions, handles callbacks from wheels
 // *****************************************************************************