File content as of revision 9:efe9f76d6f76:

#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(motor *left, motor *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);
    // setup the bot parameters:
    botDiameter = bot_diam_cm;
    ratio_wheel_bot=bot_diam_cm/wheel_diam_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!
            pExecuteNext();
      };
}


// 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)
{   botMove(moveType,Aparam);
    botWaitEnd();
}

void Creabot::iMoveAndWait(BotCmdVerb moveType, float Aturn_angle_deg, float Adist_cm)
{   botMove(moveType,Aturn_angle_deg,Adist_cm);
    botWaitEnd();
}

void Creabot::iMove(BotCmdVerb moveType, float Aparam)
{   botMove(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);
}

/** High level, immediate: move bot according to prefilled command structures.
* Recommended to use botMove() 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;
            case IDLE:
            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->WheelOFF();
    wheelRight->WheelOFF();
}

// *****************************************************************
//   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)
{   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);
        }     
}

// 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);
        }
}

/* Mid level control function: turn bot forward right, 
   around a radius twice the bot size */
void Creabot::moveRight(float turn_angle_deg)
{
    wheelLeft->setSpeed_cm_sec(botSpeed_cm_sec);
    wheelLeftRun(COUNTERCLOCKWISE, wheelLeft.calcAngle(turn_angle_deg,botDiameter));
}

/* Mid level control function: turn bot forward right, 
around a radius that is center_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);
}


/* Mid level control function: turn bot forward left, 
  around a radius twice the bot size */
void Creabot::moveLeft(float turn_angle_deg)
{
    moveLeft(turn_angle_deg,0);
}

/* 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);
}

/* 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) {
        wheelLeftRun(CLOCKWISE, -angleWheel);
        wheelRightRun(CLOCKWISE, -angleWheel);
    } else {
        wheelLeftRun(COUNTERCLOCKWISE, angleWheel);
        wheelRightRun(COUNTERCLOCKWISE, angleWheel);
    }
}

//******************************************************************************
// 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);
}


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
// *****************************************************************************

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

void Creabot::wheelRightStoppedCB()
{   wheelsState = wheelsState & ! wheelRight->StateValue;  // 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->WheelOFF();
            wheelRight->WheelOFF();
            // 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. 
}