File content as of revision 7:3a793ddc3490:

#include "CreaBot.h"

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

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

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

// *****************************************************************
//    circle_geos Structure Helper
// *****************************************************************

void circle_geos:( 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;
} 

// *****************************************************************
//    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;
        Count++;
        return &cmd[old_writeIdx];
    } else
        return NULL;
}; // put()

// Get and remove the oldest element at the tail of the FIFO
BotCommand *CommandFIFO::get()
{
    if ( !isEmpty() ) {
        int old_readIdx = readIdx;
        readIdx++;
        if (readIdx==DEPTH_FIFO) readIdx=0;
        Count--;
        return &cmd[old_readIdx];
    } else
        return NULL;
}; // get()

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

Creabot::Creabot(Motor *left, Motor *right, float diameter_wheel_cm, float bot_diam_cm):
      motor_left(left), motor_right(right), 
{
    // Initialize wheel and bot geometires
    wheel.setDiam(wheel_diam_cm);
    bot.setDiam(bot_diam_cm);
    // some other parameters:
    ratio_wheel_bot=bot_diam_cm/wheel_diam_cm;
    bot_speed_cm_sec = DEFAULT_SPEED_CM_SEC;

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

    // Initialize the own status fields
    MotState = LRMOTS_STOP;
    executingFifo= false;
}

void Creabot::fifo(BotCmdVerb moveType, float angle_or_cm)
{
    BotCommand *cmd = fifoBot.put();
    if(cmd != NULL) {
        cmd->set(moveType, angle_or_cm, 0);
        }
    executeFifo();
}

void Creabot::fifo(BotCmdVerb moveType, float angle_deg, float dist_cm)
{
    BotCommand *cmd = fifoBot.put();
    if(cmd!=NULL) {
        cmd->set(moveType, angle_deg, dist_cm);
        }
    executeFifo();
}

void Creabot::flushFifo()
{
    fifoBot.empty();
    if(executingFifo) botTicker.detach();
    executingFifo=false;
}

int Creabot::moveInFifo()
{
    return fifoBot.getCount();
}

void Creabot::spirale(float b, float 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;
    }
}

// This is the own callback function of the ticker, that works throught the FIFO stack
void Creabot::executeFifo()
{   if (fifoBot.isEmpty()) flushFifo();
    else {
        if(endMove) {
            BotCommand *cmd = fifoBot.get();
            executeCommand(cmd);
            if (!executingFifo) botTicker.attach_us( callback( this, &Creabot::executeFifo), 3150);
            executingFifo=true;
        }
    }
}

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

void Creabot::moveAndWait(BotCmdVerb moveType, float Aparam)
{
    move(moveType,Aparam);
    waitEndMove();
}

void Creabot::moveAndWait(BotCmdVerb moveType, float Aangle_deg, float Adist_cm)
{
    move(moveType,Aangle_deg,Adist_cm);
    waitEndMove();
}


void Creabot::move(BotCmdVerb moveType, float Aparam)
{   move(moveType, Aparam, Aparam)
}

void Creabot::move(BotCmdVerb moveType,  float Aangle_deg, float Adist_cm)
{   BotCommand Anew_cmd;
    Anew_cmd.set(moveType, Aangle_deg, Adist_cm);
    executeCommand(&Anew_cmd);
}

// Direct Command Execution, also called by FIFO-Handler. 
void Creabot::executeCommand(BotCommand *cmd) {
          switch (cmd->command) {
            case FORWARD:
                moveForward(cmd->dist_cm);
                break;
            case BACKWARD:
                moveBackward(cmd->dist_cm);
                break;
            case ROTATE:
                rotate(cmd->angle_deg);
                break;
            case LEFT:
                moveLeft(cmd->angle_deg, cmd->dist_cm);
                break;
            case RIGHT:
                moveRight(cmd->angle_deg, cmd->dist_cm);
                break;
            case IDLE:
            default:
                break;
        };
}; // executeCommand

void Creabot::stopAll()
{
    flushFifo();
    stopMotors();
}    



// *****************************************************************
//   Creabot Mid level control functions:  
//     all directly access the motor, no FIFO */ 
// *****************************************************************

// set bot_speed_cm_sec parameter for next high level commands 
void Creabot::setBotSpeed(float Abot_speed_cm_sec)
{
    if (Abot_speed_cm_sec < MAX_SPEED_CM_P_SEC) && (Abot_speed_cm_sec>MIN_SPEED_CM_SEC) 
        bot_speed_cm_sec = Abot_speed_cm_sec;
    // Low level setting of motor speed is done inside SetMotsSpeed: 
}

// Mid level control function:  move bot forward for a given distance
void Creabot::moveForward(float dist_cm)
{
    if (dist_cm<0) moveBackward(-dist_cm);
    else {
        float angle_deg = dist_cm*wheel.degree_per_cm;
        setMotorsSpeed(bot_speed_cm_sec);
        moveMotorLeft(COUNTERCLOCKWISE, angle_deg);
        moveMotorRight(CLOCKWISE, angle_deg);
        }     
}

// Mid level control function:  move bot backwards for a given distance
void Creabot::moveBackward(float dist_cm)
{
    if(dist_cm<0) moveForward(-dist_cm);
    else {
        float angle_deg = dist_cm*wheel.degree_per_cm;
        setMotorsSpeed(bot_speed_cm_sec);
        moveMotorLeft(CLOCKWISE, angle_deg);
        moveMotorRight(COUNTERCLOCKWISE, angle_deg);
        }
}

/* Mid level control function: turn bot forward right, 
  around a radius twice the bot size */
void Creabot::moveRight(float angle_deg)
{
    moveRight(angle_deg,bot.diam_cm);
}

/* Mid level control function: turn bot forward right, 
around a radius that is center_cm away from the right wheel*/
void Creabot::moveRight(float angle_deg, float center_cm)
{
    if(center_cm<0) center_cm=0; /* TO remove? */
    float perimeter_outer = 2.0f*angle_deg*(center_cm+bot.diam_cm)*PI/360.0f;
    float perimeter_inner = 2.0f*angle_deg*(center_cm)*PI/360.0f;
    float angleLeft = perimeter_outer*wheel.degree_per_cm;
    float angleRight = perimeter_inner*wheel.degree_per_cm;
    float ratio = angleRight/angleLeft;
    float speedRight = bot_speed_cm_sec * ratio;
    setMotSpeed(motor_left,bot_speed_cm_sec);
    setMotSpeed(motor_right,speedRight);
    moveMotorLeft(COUNTERCLOCKWISE, angleLeft);
    moveMotorRight(CLOCKWISE, angleRight);
}

/* Mid level control function: turn bot forward left, 
  around a radius twice the bot size */
void Creabot::moveLeft(float angle_deg)
{
    moveLeft(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 angle_deg, float center_cm)
{
    if (center_cm<0) center_cm=0; /* TO remove? */
    float perimeter_outer = 2.0f*angle_deg*(center_cm+bot.diam_cm)*PI/360.0f;
    float perimeter_inner = 2.0f*angle_deg*(center_cm)*PI/360.0f;
    float angleLeft = perimeter_inner*wheel.degree_per_cm;
    float angleRight = perimeter_outer*wheel.degree_per_cm;
    float ratio = angleLeft/angleRight;
    float speedRLeft = bot_speed_cm_sec * ratio;
    setMotSpeed(motor_left,speedRLeft);
    setMotSpeed(motor_right,bot_speed_cm_sec);
    moveMotorLeft(COUNTERCLOCKWISE, angleLeft);
    moveMotorRight(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::rotate(float angleBot)
{
    float angleWheel = angleBot*ratio_wheel_bot;
    setSpeed(bot_speed_cm_sec);
    if(angleBot<0) {
        moveMotorLeft(CLOCKWISE, -angleWheel);
        moveMotorRight(CLOCKWISE, -angleWheel);
    } else {
        moveMotorLeft(COUNTERCLOCKWISE, angleWheel);
        moveMotorRight(COUNTERCLOCKWISE, angleWheel);
    }
}

/* spends all time with waits, returns only once MotorState = LRMOTS_STOP  */
void Creabot::waitEndMotors()
{   while(MotState!=LRMOTS_STOP) wait(0.1);   }

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

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

/* Low level access functions: sets both motor speeds immediately  */ 
void Creabot::setMotorsSpeed(float mot_speed_cm_sec)
{
    setMotSpeed(motor_left,mot_speed_cm_sec);
    setMotSpeed(motor_right,mot_speed_cm_sec);
}


void Creabot::setMotorSpeed(Motor *motor, float speed_cm_sec)
{   if (speed_cm_sec < MIN_SPEED_CM_SEC) // catch too small or negative speeds
           motor->setStepTime_us( 1000000 ); // arbitrary value
    else 
      { motor->setStepTime_us( wheel.perim_cm * 1000000 / 
                               speed_cm_sec / motor->getStepsFullTurn() ) ; }
}

void Creabot::moveMotorLeft(motorDir dir, float angle_deg)
{   if(angle_deg>0) {
        motor_left->RunDegrees(dir, angle_deg);
        if (MotState==RMOT_RUNS) MotState=LRMOTS_RUN;
            else  MotState=LMOT_RUNS;
    }
}

void Creabot::moveMotorRight(motorDir dir, float angle_deg)
{   if(angle_deg>0) {
        motor_right->RunDegrees(dir, angle_deg);
        if (MotState==LMOT_RUNS) MotState=LRMOTS_RUN;
          else  MotState=RMOT_RUNS;
    }
}

void Creabot::stopMotors()
{
    motor_left->MotorOFF();
    motor_right->MotorOFF();
    AllMotorsStopped()    
}


//******************************************************************************
// low level motor functions, handles callbacks from motors
// *****************************************************************************
void Creabot::setCallBack(void (*Acallback)(int status))
{
    extCallBack = Acallback;
}

void Creabot::cbLeftMotStopped()
{
    if (MotState==LRMOTS_RUN) MotState = RMOT_RUNS;
      else AllMotorsStopped();
}

void Creabot::cbRightMotStopped()
{
    if (MotState==LRMOTS_RUN) MotState = LMOT_RUNS;
      else AllMotorsStopped();
}

void Creabot::AllMotorsStopped()
{
    if (MotState!=LRMOTS_STOP)
        { // Only do all this here if motor state is not already in LRMOTS_STOP!
          MotState = LRMOTS_STOP;
          if(extCallBack!=NULL) extCallBack( MotState );
        }
}