My Version of CreaBotLib
Fork of CreaBotLib by
CreaBot.cpp
- Committer:
- garphil
- Date:
- 2017-07-26
- Revision:
- 0:a7fb03c9ea9d
- Child:
- 1:974d020bb582
File content as of revision 0:a7fb03c9ea9d:
#include "CreaBot.h" extern Serial pc_uart; 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; pc_uart.printf("PI %f %f %f\n\r",PI, diameter_wheel, perimeter_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=degree_wheel_per_cm/degree_bot_per_cm; 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; 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(); // tick=4096; if(cm_per_second <0.0001f) timeUnit = 1000000; else timeUnit = perimeter_wheel * 1000000 / cm_per_second / tick; pc_uart.printf("SET SPEED : %f %d %d %f\n\r",cm_per_second,tick,timeUnit,perimeter_wheel); return timeUnit; } void Creabot::setSpeedLeft(float cm_per_second) { motor_left->setSpeed(computeSpeed(motor_left, cm_per_second)); } void Creabot::setSpeedRight(float cm_per_second) { motor_right->setSpeed(computeSpeed(motor_right, cm_per_second)); } void Creabot::waitEndMove() { pc_uart.printf("Wait end Move\n\r"); while(!endMove || executingFifo) { wait(0.1); } pc_uart.printf("End Move %d %d \n\r", endMove, executingFifo); } void Creabot::waitEndMove(uint32_t delay_us) { pc_uart.printf("Wait end Move\n\r"); uint32_t v=0; while(!endMove || executingFifo) { wait(0.1); v++; if((v*10000)>delay_us) endMove = true; } pc_uart.printf("End Move %d %d \n\r", endMove, executingFifo); } // 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); pc_uart.printf("ADD COMMAND %d\n\r",moveType); } executeFifo(); } void Creabot::fifo(cmdbot_t moveType, float angle, float cm) { CommandBot *cmd = fifoBot.put(); if(cmd!=NULL) { cmd->add_command(moveType, angle, cm); pc_uart.printf("ADD COMMAND %d\n\r",moveType); } 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(¤t_cmd); } void Creabot::move(cmdbot_t moveType, float angle, float cm) { current_cmd.add_command(moveType, angle, cm); executeCommand(¤t_cmd); } void Creabot::flushFifo() { fifoBot.empty(); if(executingFifo) botTicker.detach(); executingFifo=false; } int Creabot::moveInFifo() { return fifoBot.getSize(); } void Creabot::executeCommand(CommandBot *cmd) { pc_uart.printf("EXECUTE COMMAND %d %f %f\n\r",cmd->command, cmd->angle, cmd->cm); 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 break; default: break; }; } void Creabot::executeFifo() { if(fifoBot.getSize()==0) flushFifo(); else { if(endMove) { CommandBot *cmd = fifoBot.get(); pc_uart.printf("Execute FIFO %d %d %d %d cmd\n\r",fifoBot.getSize(),executingFifo,endMove,cmd->command); executeCommand(cmd); if(!executingFifo) botTicker.attach_us(callback(this,&Creabot::executeFifo),3150); executingFifo=true; } } } void Creabot::callBackLeft() { callLeft=true; pc_uart.printf("CALL LEFT\n\r"); if(callRight)callBack(); } void Creabot::callBackRight() { pc_uart.printf("CALL RIGHT\n\r"); callRight=true; if(callLeft)callBack(); } void Creabot::callBack() { pc_uart.printf("CALL BACK\n\r"); 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; } }