Diff: CreaBot.cpp

Revision:

6:4d8938b686a6

Parent:

5:efe80c5db389

Child:

7:3a793ddc3490

--- a/CreaBot.cpp	Wed Oct 31 14:36:07 2018 +0000
+++ b/CreaBot.cpp	Wed Nov 28 09:46:41 2018 +0000
@@ -1,43 +1,56 @@
 #include "CreaBot.h"
 
 // *****************************************************************
-//    Motor Command and Command-FIFO classes
+//    BotCommand Structure Helper
 // *****************************************************************
 
-void MotCommand::assign(cmdbot_t cmd, float p1, float p2) 
-{   command = cmd; 
-    switch (command) {
-        case FORWARD : ; 
-        case BACKWARD: cm=p1; angle=0; break; 
-        case ROTATE  : angle = p1; break; 
-        case LEFT    : 
-        case RIGHT   : angle = p1; cm=p2; break; 
-        case IDLE    : break; 
-        default      : break;
-    };
+// Struct Helper; set structure fields to values  */
+void BotCommand::set(BotCmdVerb Acommand, float Adist_cm, float Aangle_deg) 
+{   command  = Acommand; 
+    dist_cm  = Adist_cm; 
+    angle_deg= Aangle_deg;
+}; 
+
+// Struct Helper; set structure fields to values  */
+void BotCommand::set(BotCmdVerb Acommand, float Adist_cm) 
+{   command  = Acommand; 
+    dist_cm  = Adist_cm; 
+    angle_deg= 0;
 }; 
 
-MotCommand *CommandFIFO::put() {
-    if(count<DEPTH_FIFO) {
+// *****************************************************************
+//    CommandFIFO Class
+// *****************************************************************
+
+//  Class Creator: initializes an empties FIFO
+CommandFIFO::CommandFIFO();
+{ readIdx = writeIdx = count = 0; 
+  cmd_idle.set(IDLE, 0, 0); 
+}
+
+// Reserve a new element at the head of the FIFO
+BotCommand *CommandFIFO::put()
+{
+    if( !isFull() ) {
         int old_writeIdx = writeIdx;
         writeIdx++;
-        if (writeIdx==DEPTH_FIFO) writeIdx=0; 
+        if (writeIdx==DEPTH_FIFO) writeIdx=0;
         count++;
         return &cmd[old_writeIdx];
-        } 
-      else 
+    } else
         return NULL;
 }; // put()
 
-MotCommand *CommandFIFO::get() {
+// Get and remove the oldest element at the tail of the FIFO
+BotCommand *CommandFIFO::get()
+{
     if ( !isEmpty() ) {
-            int old_readIdx = readIdx; 
+        int old_readIdx = readIdx;
         readIdx++;
-        if (readIdx==DEPTH_FIFO) readIdx=0; 
-        count--; 
+        if (readIdx==DEPTH_FIFO) readIdx=0;
+        count--;
         return &cmd[old_readIdx];
-        } 
-      else 
+    } else
         return NULL;
 }; // get()
 
@@ -48,42 +61,47 @@
 Creabot::Creabot(Motor *left, Motor *right, float diameter_wheel_cm, float distance_wheel_cm):
       motor_left(left), motor_right(right), diameter_wheel(diameter_wheel_cm), distance_wheel(distance_wheel_cm)
 {
+    // Initialize dimensions
     perimeter_wheel = PI*diameter_wheel;
     perimeter_bot = PI*distance_wheel;
     degree_wheel_per_cm=360.0f/perimeter_wheel;
     degree_bot_per_cm=360.0f/perimeter_bot;;
     ratio_wheel_bot=perimeter_bot/perimeter_wheel;
-    status=0;
+    setSpeed(DEFAULT_SPEED);
 
-    motor_left->setMotorCallback(this, &Creabot::callBackLeft);
-    motor_right->setMotorCallback(this, &Creabot::callBackRight);
+    // Attach the Crabot functions as callbacks to the motors
+    motor_left->callbackSet(this, &Creabot::cbLeftMotStopped);
+    motor_right->callbackSet(this, &Creabot::cbRightMotStopped);
+
+    // Initialize own callback as empty
     extCallBack = NULL;
-    setSpeed(DEFAULT_SPEED);
-    endMoveLeft = true;
+
+    // Initialize the own status fields
+    endMoveLeft  = true;
     endMoveRight = true;
-    endMove=true;
-    executingFifo=false;
+    endMove      = true;
+    executingFifo= false;
 }
 
-void Creabot::moveForward(float cm)
+void Creabot::moveForward(float dist_cm)
 {
-    if (cm<0) moveBackward(cm);
+    if (dist_cm<0) moveBackward(dist_cm);
     else {
-        float angle = cm*degree_wheel_per_cm;
+        float angle_deg = dist_cm*degree_wheel_per_cm;
         setSpeed(last_speed);
-        moveMotorLeft(COUNTERCLOCKWISE, angle);
-        moveMotorRight(CLOCKWISE, angle);
+        moveMotorLeft(COUNTERCLOCKWISE, angle_deg);
+        moveMotorRight(CLOCKWISE, angle_deg);
         }     
 }
 
-void Creabot::moveBackward(float cm)
+void Creabot::moveBackward(float dist_cm)
 {
-    if(cm<0) moveForward(cm);
+    if(dist_cm<0) moveForward(dist_cm);
     else {
-        float angle = cm*degree_wheel_per_cm;
+        float angle_deg = dist_cm*degree_wheel_per_cm;
         setSpeed(last_speed);
-        moveMotorLeft(CLOCKWISE, angle);
-        moveMotorRight(COUNTERCLOCKWISE, angle);
+        moveMotorLeft(CLOCKWISE, angle_deg);
+        moveMotorRight(COUNTERCLOCKWISE, angle_deg);
         }
 }
 void Creabot::rotate(float angleBot)
@@ -99,16 +117,6 @@
     }
 }
 
-void Creabot::stopMove()
-{
-    motor_left->MotorOFF();
-    motor_right->MotorOFF();
-    endMoveLeft = true;
-    endMoveRight = true;
-    endMove = true;
-    flushFifo();
-    //  callBack(); // ? To be called or set as parameter?
-}
 
 void Creabot::waitEndMove()
 {
@@ -127,38 +135,38 @@
     }
 } // watchdog
 
-void Creabot::setCallBack(void (*callback)(int status))
+void Creabot::setCallBack(void (*Acallback)(int status))
 {
-    extCallBack = callback;
+    extCallBack = Acallback;
 }
 
-void Creabot::fifo(cmdbot_t moveType, float angle_or_cm)
+void Creabot::fifo(BotCmdVerb moveType, float angle_or_cm)
 {
-    MotCommand *cmd = fifoBot.put();
+    BotCommand *cmd = fifoBot.put();
     if(cmd != NULL) {
-        cmd->assign(moveType, angle_or_cm, 0);
+        cmd->set(moveType, angle_or_cm, 0);
         }
     executeFifo();
 }
 
-void Creabot::fifo(cmdbot_t moveType, float angle, float cm)
+void Creabot::fifo(BotCmdVerb moveType, float angle_deg, float dist_cm)
 {
-    MotCommand *cmd = fifoBot.put();
+    BotCommand *cmd = fifoBot.put();
     if(cmd!=NULL) {
-        cmd->assign(moveType, angle, cm);
+        cmd->set(moveType, angle_deg, dist_cm);
         }
     executeFifo();
 }
 
-void Creabot::moveAndWait(cmdbot_t moveType, float angle_or_cm)
+void Creabot::moveAndWait(BotCmdVerb moveType, float angle_or_cm)
 {
     move(moveType,angle_or_cm);
     waitEndMove();
 }
 
-void Creabot::moveAndWait(cmdbot_t moveType, float angle, float cm)
+void Creabot::moveAndWait(BotCmdVerb moveType, float angle_deg, float dist_cm)
 {
-    move(moveType,angle,cm);
+    move(moveType,angle_deg,dist_cm);
     waitEndMove();
 }
 
@@ -175,35 +183,16 @@
 }
 
 void Creabot::spirale(float b, float turns) {
-    float r=0.0;
-    float angle = 0.0f;
-    while(angle < float( 360.0 * turns) ) {
+    float r=0.0f;
+    float angle_deg = 0.0f;
+    while(angle_deg < float( 360.0f * turns) ) {
         this->moveAndWait(RIGHT, 10, r);
         r=r+b;
     }
 }
 
-void Creabot::callBackLeft()
-{
-    endMoveLeft=true;
-    if(endMoveRight)callBack();
-}
-
-void Creabot::callBackRight()
-{
-    endMoveRight=true;
-    if(endMoveLeft)callBack();
-}
-
-void Creabot::callBack()
-{
-    endMove=true;
-    if(extCallBack!=NULL) extCallBack(status);
-}
-
-
 //******************************************
-//    Executing the FIFO of Motor Commands 
+//    Using the FIFO to queue up Motor Commands 
 //******************************************
 
 // This is the own callback function of the ticker, that works throught the FIFO stack
@@ -211,7 +200,7 @@
 {   if (fifoBot.isEmpty()) flushFifo();
     else {
         if(endMove) {
-            MotCommand *cmd = fifoBot.get();
+            BotCommand *cmd = fifoBot.get();
             executeCommand(cmd);
             if (!executingFifo) botTicker.attach_us( callback( this, &Creabot::executeFifo), 3150);
             executingFifo=true;
@@ -219,35 +208,35 @@
     }
 }
 
-void Creabot::move(cmdbot_t moveType, float angle_or_cm)
+void Creabot::move(BotCmdVerb moveType, float angle_or_cm)
 {
-    current_cmd.assign(moveType, angle_or_cm, 0);
+    current_cmd.set(moveType, angle_or_cm, 0);
     executeCommand(&current_cmd);
 }
 
-void Creabot::move(cmdbot_t moveType, float angle, float cm)
+void Creabot::move(BotCmdVerb moveType, float angle_deg, float dist_cm)
 {
-    current_cmd.assign(moveType, angle, cm);
+    current_cmd.set(moveType, angle_deg, dist_cm);
     executeCommand(&current_cmd);
 }
 
 // Direct Command Execution, called by FIFO-Handler. 
-void Creabot::executeCommand(MotCommand *cmd) {
+void Creabot::executeCommand(BotCommand *cmd) {
           switch (cmd->command) {
             case FORWARD:
-                moveForward(cmd->cm);
+                moveForward(cmd->dist_cm);
                 break;
             case BACKWARD:
-                moveBackward(cmd->cm);
+                moveBackward(cmd->dist_cm);
                 break;
             case ROTATE:
-                rotate(cmd->angle);
+                rotate(cmd->angle_deg);
                 break;
             case LEFT:
-                moveLeft(cmd->angle, cmd->cm);
+                moveLeft(cmd->angle_deg, cmd->dist_cm);
                 break;
             case RIGHT:
-                moveRight(cmd->angle, cmd->cm);
+                moveRight(cmd->angle_deg, cmd->dist_cm);
                 break;
             case IDLE:
               //  wait(10); // Not to be done in an interrupt. could start a timer with a call back with 'end move'? Would be stopped by stopMove... 
@@ -257,42 +246,47 @@
         };
 }; // executeCommand
 
+void Creabot::stopAll()
+{
+    flushFifo();
+    stopMotors();
+}    
+
 
 //***********************************************
 //    Execute high level motor functions directly, no FIFO
 //***********************************************
 
-void Creabot::moveRight(float angle)
+void Creabot::moveRight(float angle_deg)
 {
-    moveRight(angle,distance_wheel);
+    moveRight(angle_deg,distance_wheel);
 }
 
-void Creabot::moveRight(float angle, float center_cm)
+void Creabot::moveRight(float angle_deg, float center_cm)
 {
     if(center_cm<0) center_cm=0; /* TO remove? */
-    float perimeter_outer = 2.0f*angle*(center_cm+distance_wheel)*PI/360.0f;
-    float perimeter_inner = 2.0f*angle*(center_cm)*PI/360.0f;
-    float  angleLeft = perimeter_outer*degree_wheel_per_cm;
-    float  angleRight = perimeter_inner*degree_wheel_per_cm;
+    float perimeter_outer = 2.0f*angle_deg*(center_cm+distance_wheel)*PI/360.0f;
+    float perimeter_inner = 2.0f*angle_deg*(center_cm)*PI/360.0f;
+    float angleLeft = perimeter_outer*degree_wheel_per_cm;
+    float angleRight = perimeter_inner*degree_wheel_per_cm;
     float ratio = angleRight/angleLeft;
     float speedRight = last_speed * ratio;
-   // pc_uart.printf("Set Perimeter angle =%f center_cm=%f outer = %f inner = %f angle_left %f angle right %f ratio %f cs %f\n\r",angle, center_cm, perimeter_outer,perimeter_inner,angleLeft,angleRight,ratio, last_speed);
     setSpeedMot(motor_left,last_speed);
     setSpeedMot(motor_right,speedRight);
     moveMotorLeft(COUNTERCLOCKWISE, angleLeft);
     moveMotorRight(CLOCKWISE, angleRight);
 }
 
-void Creabot::moveLeft(float angle)
+void Creabot::moveLeft(float angle_deg)
 {
-    moveLeft(angle,0);
+    moveLeft(angle_deg,0);
 }
 
-void Creabot::moveLeft(float angle, float center_cm)
+void Creabot::moveLeft(float angle_deg, float center_cm)
 {
     if(center_cm<0) center_cm=0; /* TO remove? */
-    float perimeter_outer = 2.0f*angle*(center_cm+distance_wheel)*PI/360.0f;
-    float perimeter_inner = 2.0f*angle*(center_cm)*PI/360.0f;
+    float perimeter_outer = 2.0f*angle_deg*(center_cm+distance_wheel)*PI/360.0f;
+    float perimeter_inner = 2.0f*angle_deg*(center_cm)*PI/360.0f;
     float angleLeft = perimeter_inner*degree_wheel_per_cm;
     float angleRight = perimeter_outer*degree_wheel_per_cm;
     float ratio = angleLeft/angleRight;
@@ -304,42 +298,69 @@
 }
 
 //******************************************************************************
-// low level motor functions, filters for angle>0, remembers that a motor moves
+// low level motor functions, filters for angle_deg>0, remembers that a motor moves
 //******************************************************************************
 
-void Creabot::moveMotorLeft(motorDir dir, float angle)
+void Creabot::moveMotorLeft(motorDir dir, float angle_deg)
 {
-   if(angle>0) {
-        motor_left->RunDegrees(dir, angle);
-        endMove = endMoveLeft = false;
+   if(angle_deg>0) {
+        motor_left->RunDegrees(dir, angle_deg);
+        if (MotState==RMOT_RUNS) MotState=MOTORS_RUN;
+            else  MotState=LMOT_RUNS;
    }
 }
 
-void Creabot::moveMotorRight(motorDir dir, float angle)
-{    if(angle>0) {
-        motor_right->RunDegrees(dir, angle);
-        endMove = endMoveRight = false;}
-}
-
-int Creabot::getStatus()
-{   if(endMove     ) return 0; // No Movement
-    if(endMoveRight) return 1; // Returns 1, assuming that only Left Motor is still running
-    if(endMoveLeft ) return 3; // Returns 2, Right Motor is running aloone
-    return 2; // Both Motors are still running!!!
+void Creabot::moveMotorRight(motorDir dir, float angle_deg)
+{
+    if(angle_deg>0) {
+        motor_right->RunDegrees(dir, angle_deg);
+        if (MotState==LMOT_RUNS) MotState=MOTORS_RUN;
+          else  MotState=RMOT_RUNS;
+    }
 }
 
-void Creabot::setSpeed(float cm_per_second)
+void Creabot::cbLeftMotStopped()
 {
-    setSpeedMot(motor_left,cm_per_second);
-    setSpeedMot(motor_right,cm_per_second);
-    last_speed = cm_per_second;
+    if (MotState==MOTORS_RUN) MotState = RMOT_RUNS;
+      else AllMotorsStopped();
 }
 
-void Creabot::setSpeedMot(Motor *motor, float cm_per_second)
-{   if (cm_per_second <0.0001f) 
-           motor->setStepTime_us( 1000000 ); 
-    else { uint32_t tick = motor->getStepsFullTurn();
-           motor->setStepTime_us( perimeter_wheel * 1000000 / cm_per_second / tick ) ;
+void Creabot::cbRightMotStopped()
+{
+    if (MotState==MOTORS_RUN) MotState = LMOT_RUNS;
+      else AllMotorsStopped();
+}
+
+void Creabot::AllMotorsStopped()
+{
+    if (MotState!=ALLSTOP)
+        { // Only do all this here if motor state is not already in ALLSTOP!
+          MotState = AlLSTOP;
+          if(extCallBack!=NULL) extCallBack( MotState );
         }
 }
 
+
+void Creabot::stopMotors()
+{
+    motor_left->MotorOFF();
+    motor_right->MotorOFF();
+    AllMotorsStopped()    
+}
+
+void Creabot::setSpeed(float speed_cm_sec)
+{
+    if (MAX_SPEED_CM_P_SEC)
+    setSpeedMot(motor_left,speed_cm_sec);
+    setSpeedMot(motor_right,speed_cm_sec);
+    last_speed = speed_cm_sec;
+}
+
+void Creabot::setSpeedMot(Motor *motor, float speed_cm_sec)
+{   if (speed_cm_sec <0.0001f) 
+           motor->setStepTime_us( 1000000 ); 
+    else { uint32_t tick = motor->getStepsFullTurn();
+           motor->setStepTime_us( perimeter_wheel * 1000000 / speed_cm_sec / tick ) ;
+        }
+}
+