File content as of revision 6:4d8938b686a6:

#include "CreaBot.h"

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

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

// *****************************************************************
//    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 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;
    setSpeed(DEFAULT_SPEED);

    // 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
    endMoveLeft  = true;
    endMoveRight = true;
    endMove      = true;
    executingFifo= false;
}

void Creabot::moveForward(float dist_cm)
{
    if (dist_cm<0) moveBackward(dist_cm);
    else {
        float angle_deg = dist_cm*degree_wheel_per_cm;
        setSpeed(last_speed);
        moveMotorLeft(COUNTERCLOCKWISE, angle_deg);
        moveMotorRight(CLOCKWISE, angle_deg);
        }     
}

void Creabot::moveBackward(float dist_cm)
{
    if(dist_cm<0) moveForward(dist_cm);
    else {
        float angle_deg = dist_cm*degree_wheel_per_cm;
        setSpeed(last_speed);
        moveMotorLeft(CLOCKWISE, angle_deg);
        moveMotorRight(COUNTERCLOCKWISE, angle_deg);
        }
}
void Creabot::rotate(float angleBot)
{
    float angleWheel = angleBot*ratio_wheel_bot;
    setSpeed(last_speed);
    if(angleBot<0) {
        moveMotorLeft(CLOCKWISE, -angleWheel);
        moveMotorRight(CLOCKWISE, -angleWheel);
    } else {
        moveMotorLeft(COUNTERCLOCKWISE, angleWheel);
        moveMotorRight(COUNTERCLOCKWISE, angleWheel);
    }
}


void Creabot::waitEndMove()
{
   while(!endMove || executingFifo) {
       wait(0.1);
     }
   
}
void Creabot::waitEndMove(uint32_t delay_us)
{
    uint32_t v=0;
    while(!endMove || executingFifo) {
        wait(0.1);
        v++;
        if((v*10000)>delay_us) endMove = true;
    }
} // watchdog

void Creabot::setCallBack(void (*Acallback)(int status))
{
    extCallBack = Acallback;
}

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::moveAndWait(BotCmdVerb moveType, float angle_or_cm)
{
    move(moveType,angle_or_cm);
    waitEndMove();
}

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

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

//******************************************
//    Using the FIFO to queue up Motor Commands 
//******************************************

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

void Creabot::move(BotCmdVerb moveType, float angle_or_cm)
{
    current_cmd.set(moveType, angle_or_cm, 0);
    executeCommand(&current_cmd);
}

void Creabot::move(BotCmdVerb moveType, float angle_deg, float dist_cm)
{
    current_cmd.set(moveType, angle_deg, dist_cm);
    executeCommand(&current_cmd);
}

// Direct Command Execution, 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:
              //  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... 
                break;
            default:
                break;
        };
}; // executeCommand

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


//***********************************************
//    Execute high level motor functions directly, no FIFO
//***********************************************

void Creabot::moveRight(float angle_deg)
{
    moveRight(angle_deg,distance_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+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;
    setSpeedMot(motor_left,last_speed);
    setSpeedMot(motor_right,speedRight);
    moveMotorLeft(COUNTERCLOCKWISE, angleLeft);
    moveMotorRight(CLOCKWISE, angleRight);
}

void Creabot::moveLeft(float angle_deg)
{
    moveLeft(angle_deg,0);
}

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+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;
    float speedRLeft = last_speed * ratio;
    setSpeedMot(motor_left,speedRLeft);
    setSpeedMot(motor_right,last_speed);
    moveMotorLeft(COUNTERCLOCKWISE, angleLeft);
    moveMotorRight(CLOCKWISE, angleRight);
}

//******************************************************************************
// low level motor functions, filters for angle_deg>0, remembers that a motor moves
//******************************************************************************

void Creabot::moveMotorLeft(motorDir dir, float angle_deg)
{
   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_deg)
{
    if(angle_deg>0) {
        motor_right->RunDegrees(dir, angle_deg);
        if (MotState==LMOT_RUNS) MotState=MOTORS_RUN;
          else  MotState=RMOT_RUNS;
    }
}

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

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