File content as of revision 1:69b5d8f0ba9c:

#include "CreaBot.h"

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

 motor_left->setMotorCallback(this, &Creabot::callBackLeft);
 motor_right->setMotorCallback(this, &Creabot::callBackRight);
    extCallBack = NULL;
    setSpeed(DEFAULT_SPEED);
    callLeft = true;
    callRight = true;
    endMove=true;
    executingFifo=false;
}

void Creabot::moveForward(float cm)
{
    float angle;
    if(cm<0) moveBackward(cm);
    angle = cm*degree_wheel_per_cm;
    setSpeedLeft(current_speed);
    setSpeedRight(current_speed);
   moveMotorLeft(COUNTERCLOCKWISE, angle);
    moveMotorRight(CLOCKWISE, angle);

}

void Creabot::moveBackward(float cm)
{
    float angle;
    if(cm<0) moveForward(cm);
    angle = cm*degree_wheel_per_cm;
   setSpeedLeft(current_speed);
    setSpeedRight(current_speed);
    moveMotorLeft(CLOCKWISE, angle);
    moveMotorRight(COUNTERCLOCKWISE, angle);

}
void Creabot::rotate(float angleBot)
{
    float angleWheel;
    setSpeedLeft(current_speed);
    setSpeedRight(current_speed);
   angleWheel = angleBot*ratio_wheel_bot;
    if(angleBot<0) {
        moveMotorLeft(CLOCKWISE, -angleWheel);
        moveMotorRight(CLOCKWISE, -angleWheel);
    } else {
        moveMotorLeft(COUNTERCLOCKWISE, angleWheel);
        moveMotorRight(COUNTERCLOCKWISE, angleWheel);
    }
}
void Creabot::moveRight(float angle)
{
    moveRight(angle,distance_wheel);
}
void Creabot::moveRight(float angle, 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 ratio = angleRight/angleLeft;
    float speedRight = current_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, current_speed);
    setSpeedLeft(current_speed);
    setSpeedRight(speedRight);
    moveMotorLeft(COUNTERCLOCKWISE, angleLeft);
    moveMotorRight(CLOCKWISE, angleRight);

}
void Creabot::moveLeft(float angle)
{
    moveLeft(angle,0);
}
void Creabot::moveLeft(float angle, 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_inner*degree_wheel_per_cm;
    float  angleRight = perimeter_outer*degree_wheel_per_cm;
    float ratio = angleLeft/angleRight;
    float speedRLeft = current_speed * ratio;
    setSpeedLeft(speedRLeft);
    setSpeedRight(current_speed);
    moveMotorLeft(COUNTERCLOCKWISE, angleLeft);
    moveMotorRight(CLOCKWISE, angleRight);

}
void Creabot::stopMove()
{
    motor_left->Stop();
    motor_right->Stop();
    callLeft = true;
    callRight = true;
    endMove = true;
    flushFifo();
    //  callBack(); // ? To be called or set as parameter?
}
int Creabot::getStatus()
{
    if(endMove == true) return 0; // No Movement
    if(callRight == true) return 1; // Left Motor still running
    if(callLeft == true) return 3; // ERROR
    return 2; // Left Motor still running
}

void Creabot::setSpeed(float cm_per_second)
{
    setSpeedLeft(cm_per_second);
    setSpeedRight(cm_per_second);
    current_speed = cm_per_second;
}

uint32_t Creabot::computeSpeed(Motor *motor, float cm_per_second)
{
    uint32_t tick, timeUnit;
    tick = motor->getCalibration();
  if(cm_per_second <0.0001f) timeUnit = 1000000; else 
    timeUnit = perimeter_wheel * 1000000 / cm_per_second / tick;
    return timeUnit;
}

void Creabot::setSpeedLeft(float cm_per_second)
{
    motor_left->setDelayBtwTicks(computeSpeed(motor_left, cm_per_second));
}

void Creabot::setSpeedRight(float cm_per_second)
{
    motor_right->setDelayBtwTicks(computeSpeed(motor_right, cm_per_second));
}

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 (*callback)(int status))
{
    extCallBack = callback;
}

void Creabot::fifo(cmdbot_t moveType, float angle_or_cm)
{
    CommandBot *cmd = fifoBot.put();
    if(cmd!=NULL) {
        cmd->add_command(moveType, angle_or_cm, 0);
    }
    executeFifo();
}
void Creabot::fifo(cmdbot_t moveType, float angle, float cm)
{
    CommandBot *cmd = fifoBot.put();
    if(cmd!=NULL) {
        cmd->add_command(moveType, angle, cm);
   }
    executeFifo();
}


void Creabot::moveAndWait(cmdbot_t moveType, float angle_or_cm)
{
        move(moveType,angle_or_cm);
        waitEndMove();
}
void Creabot::moveAndWait(cmdbot_t moveType, float angle, float cm)
{
        move(moveType,angle,cm);
        waitEndMove();
}

void Creabot::move(cmdbot_t moveType, float angle_or_cm)
{
        current_cmd.add_command(moveType, angle_or_cm, 0);
    executeCommand(&current_cmd);
}
void Creabot::move(cmdbot_t moveType, float angle, float cm)
{
         current_cmd.add_command(moveType, angle, cm);
     executeCommand(&current_cmd);
}

void Creabot::flushFifo()
{
    fifoBot.empty();
    if(executingFifo) botTicker.detach();
    executingFifo=false;
}
int Creabot::moveInFifo()
{
    return fifoBot.getSize();
}
void Creabot::executeCommand(CommandBot *cmd) {
          switch (cmd->command) {
            case FORWARD:
                moveForward(cmd->cm);
                break;
            case BACKWARD:
                moveBackward(cmd->cm);
                break;
            case ROTATE:
                rotate(cmd->angle);
                break;
            case LEFT:
                moveLeft(cmd->angle, cmd->cm);
                break;
            case RIGHT:
                moveRight(cmd->angle, cmd->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;
        };
}
void Creabot::executeFifo()
{

    if(fifoBot.getSize()==0) flushFifo();
    else {
        if(endMove) {
            CommandBot *cmd = fifoBot.get();
            executeCommand(cmd);
            if(!executingFifo) botTicker.attach_us(callback(this,&Creabot::executeFifo),3150);
            executingFifo=true;

        }
    }
}

void Creabot::callBackLeft()
{
    callLeft=true;
    if(callRight)callBack();
}
void Creabot::callBackRight()
{
    callRight=true;
    if(callLeft)callBack();
}

void Creabot::callBack()
{
    endMove=true;
    if(extCallBack!=NULL) extCallBack(status);
}

void Creabot::moveMotorLeft(MotorDir dir, float angle)
{
   if(angle>0) {
        motor_left->RunDegrees(dir, angle);
        endMove = callLeft = false;
   }
}
void Creabot::moveMotorRight(MotorDir dir, float angle)
{
    if(angle>0) {
        motor_right->RunDegrees(dir, angle);
        endMove = callRight = false;
    }
}