File content as of revision 12:530772639065:

#include "CreaBot.h"

// *****************************************************************
//    BotCommand Structure Helper
// *****************************************************************

// Struct Helper; set structure fields to values  */
void BotCommand::set(BotCmdVerb Acommand, float Aturn_angle_deg, float Adist_cm) 
{   command  = Acommand; 
    dist_cm  = Adist_cm; 
    turn_angle_deg= Aturn_angle_deg;
}; 

// Struct Helper; set structure fields to values  */
void BotCommand::set(BotCmdVerb Acommand, float Aparam) 
{   command  = Acommand; 
    dist_cm  = Aparam; 
    turn_angle_deg= Aparam;
}; 


// *****************************************************************
//    Creabot Class
// *****************************************************************

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 ->setDiamCM( diameter_wheel_cm );
    wheelRight->setDiamCM( diameter_wheel_cm );
    // setup the bot parameters:
    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
    wheelLeft ->callbackSet(this, &Creabot::wheelLeftStoppedCB);   
    wheelRight->callbackSet(this, &Creabot::wheelRightStoppedCB);
    // Initialize own callback as empty
    extCallBack = NULL;

    // setup the default Directions and State Values: 
    // used below as variable instead of always passing a direction
    wheelLeft ->defaultDirection = COUNTERCLOCKWISE;
    wheelLeft ->StateValue  = LWHEEL_RUNS;
    wheelRight->defaultDirection = CLOCKWISE;
    wheelRight->StateValue = RWHEEL_RUNS;

    // Initialize the own status fields
    wheelsState  = LRWHEELS_STOP;
    qReset();
}


// *****************************************************************
//    Queue Queue management
// *****************************************************************

void Creabot::qMove(BotCmdVerb Acommand, float Aparam)
{
    qMove(Acommand, Aparam, Aparam);
}

/* Add a new movement to the queue, if not full. 
*  ATTENTION: 
*   qExecuteNext() is called asynchronsously by the motor Callback wheelsAllStoppedCB()
*   qExecuteNext() tries to read access the next command in the queue.  
*   A simultaneous read and write access to the same resources can lead to 
*   conflict and must be carefully avoided.   
*   This conflict concerns the read-modify-write access of variable Count. 
*   It is avoided by using the qBlock variable. With qBlock set during increment
*   a simultaneous read access is blocked
*   Any changes to those write and read handlers must take into account that 
*   wheelsAllStoppedCB() and qExecuteNext() may be called any time while 
*   executing qMove command. 
*/
void Creabot::qMove(BotCmdVerb Acommand, float Aturn_angle_deg, float Adist_cm)
{   if( !qIsFull() ) 
      { cmd[writeIdx].set(Acommand, Aturn_angle_deg, Adist_cm);
        writeIdx++;
        if (writeIdx==DEPTH_Queue) writeIdx=0;
        qCollide = false; // colliding access to qCollide cannot happen while qBlock = false
        // if callback happens here, there is no collosion, the last old command gets already executed. 
        qBlock = true; 
        // if callback happens here while qBlock = true, the collision is remembered in qCollide
        Count++;
        qBlock = false;
        // if callback happens here there is no more blockage and the next (maybe the new) command is already being executed
        // 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!
            qExecuteNext();
      };
}


// Queue is worked through while still any elements inside
// ATTENTION read above consideration of conflicting accesses between qMove() and wheelsAllStoppedCB()
// Think careful if a case can occur that pExecuteNext is called twice immediately following each other, 
//   once by qMove() and once by  wheelsAllStoppedCB() ???
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] );
        readIdx++;
        if (readIdx==DEPTH_Queue) readIdx=0;
        Count--;
      };
}

// Immediately end All, the queue and the motor
void Creabot::qStopAll()
{   qReset(); // empty the queue
    iStop(); // stop the motors, do not call external callback
}    


//**************************************************************************
//    Using the BotCmdVerb to encode different commands, done immediately
//**************************************************************************

void Creabot::iMoveAndWait(BotCmdVerb moveType, float Aparam)
{   iMove(moveType,Aparam,Aparam);
    iWaitEnd();
}

void Creabot::iMoveAndWait(BotCmdVerb moveType, float Aturn_angle_deg, float Adist_cm)
{   iMove(moveType,Aturn_angle_deg,Adist_cm);
    iWaitEnd();
}

void Creabot::iMove(BotCmdVerb moveType, float 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);
    iExeCommand(&Anew_cmd);
}

/** High level, immediate: move bot according to prefilled command structures.
* 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,*/
void Creabot::iExeCommand(BotCommand *cmd) {
          switch (cmd->command) {
            case FORWARD:
                moveForward(cmd->dist_cm);
                break;
            case BACKWARD:
                moveBackward(cmd->dist_cm);
                break;
            case LEFT:
                moveLeft(cmd->turn_angle_deg, cmd->dist_cm);
                break;
            case RIGHT:
                moveRight(cmd->turn_angle_deg, cmd->dist_cm);
                break;
            case BACKLEFT:
                moveBackLeft(cmd->turn_angle_deg, cmd->dist_cm);
                break;
            case BACKRIGHT:
                moveBackRight(cmd->turn_angle_deg, cmd->dist_cm);
                break;
            case ROTATE:
                moveRotate(cmd->turn_angle_deg);
                break;
            default:
                break;
        };
}; // executeCommand

/* spends all time with waits, returns only once wheelState = LRWHEELS_STOP  */
void Creabot::iWaitEnd()
{   while(wheelsState!=LRWHEELS_STOP) wait(0.1);   }

/*  spends all time with waits, returns only once wheelState = LRWHEELS_STOP,
  but maximum  max_wait_ms milli seconds */
void Creabot::iWaitEnd(uint32_t max_wait_ms)
{   uint32_t waited=0;
    while( (wheelsState!=LRWHEELS_STOP) &&  (waited < max_wait_ms) ) 
      { wait(0.1);  waited += 100; }
} // botWaitEnd

void Creabot::iStop()
{   // Call the StopRun method, so wheels detach their tickers
    wheelLeft->StopRun();
    wheelRight->StopRun();
    // wheelsAllStoppedCB could handle the rest of the cleanup, but would also trigger external callback
    // Turn motor power off!
    wheelLeft->MotorOFF();
    wheelRight->MotorOFF();
    wheelsState = LRWHEELS_STOP;
}

// *****************************************************************
//   Creabot Mid level control functions:  
//     all directly access the wheel, no Queue */ 
// *****************************************************************

// set botSpeed_cm_sec parameter for next high level commands 
void Creabot::setSpeed(float AbotSpeed_cm_sec)
{   if (AbotSpeed_cm_sec > MAX_SPEED_CM_SEC) botSpeed_cm_sec = MAX_SPEED_CM_SEC;
        else if (AbotSpeed_cm_sec<MIN_SPEED_CM_SEC) botSpeed_cm_sec = MIN_SPEED_CM_SEC;
        else botSpeed_cm_sec = AbotSpeed_cm_sec;
    // Low level setting of wheel speed is done inside SetMotsSpeed: 
}

// Mid level control function:  advance bot straight forward for a given distance
void Creabot::moveForward(float dist_cm)
{   
    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)
{
    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 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);
    wheelLeft->RunTurnAngle(turn_angle_deg,botDiameter);
    wheelsState = LWHEEL_RUNS;
}

/* Mid level control function: turn bot forward right, 
around a radius that is turn_radius_cm away from the right wheel*/
void Creabot::moveRight(float turn_angle_deg, float turn_radius_cm)
{   // If Center_cm is too small the right wheel has practically nothing to do ;)
    if (fabs(double(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 equal to the bot size; = the left wheel stands still */
void Creabot::moveLeft(float turn_angle_deg)
{
    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 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 (fabs(double(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)
{
    float angleWheel = angleBot*ratio_wheel_bot;
    setSpeed(botSpeed_cm_sec);
    if(angleBot<0) {
        wheelRight->RunDegrees( angleWheel);
        wheelLeft ->RunDegrees(-angleWheel);// Direction = defaultDirection
        // negative distance are run by RunDegrees in opposite direction!
    } else {
        wheelLeft ->RunDegrees( angleWheel);
        wheelRight->RunDegrees(-angleWheel);// Direction = defaultDirection
        // negative distance are run by RunDegrees in opposite direction!
    }
    wheelsState = LRWHEELS_RUN;
}

//******************************************************************************
// low level wheel functions, only filters for rot_angle_deg>0, then passes 
// to wheel Class; and remembers that a wheel moves
//******************************************************************************

/* 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);
    wheelRight->setSpeed_cm_sec(mot_speed_cm_sec);
}

//******************************************************************************
// low level wheel functions, handles callbacks from wheels
// *****************************************************************************

void Creabot::wheelLeftStoppedCB()
{   wheelsState = TWheelsState( wheelsState & LWHEEL_RUNS );  // mask out Left Wheel status bit
    if (wheelsState == LRWHEELS_STOP) // all wheels are now known to have stopped
        wheelsAllStoppedCB();
}

void Creabot::wheelRightStoppedCB()
{   wheelsState = TWheelsState( wheelsState & RWHEEL_RUNS );  // mask out Left Wheel status bit
    if (wheelsState == LRWHEELS_STOP) // all wheels are now known to have stopped
        wheelsAllStoppedCB();
}

void Creabot::wheelsAllStoppedCB()
{   // assume that already at entry wheelsState == LRWHEELS_STOP
    // Try to execute next entry in queue, if one is present
    if ( !qBlock ) 
      { qExecuteNext();// not blocked, so can execute now
        // if wheels are still STOPed, that means no command to execute was left in the queue 
        if (wheelsState==LRWHEELS_STOP) 
          { // Turn motor power off!
            wheelLeft->MotorOFF();
            wheelRight->MotorOFF();
            // Call the externall callback function to inform about the end of all movements
            if(extCallBack!=NULL) extCallBack( wheelsState );
          } // if wheelsState==LRWHEELS_STOP
      } // if ( !qBlock ) 
      else qCollide = true; // Remember the collision to execute the next command. 
}