CreaBot.cpp

00001 #include "CreaBot.h"
00002 
00003 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)
00004 {
00005     perimeter_wheel = PI*diameter_wheel;
00006     perimeter_bot = PI*distance_wheel;
00007     degree_wheel_per_cm=360.0f/perimeter_wheel;
00008     degree_bot_per_cm=360.0f/perimeter_bot;;
00009     ratio_wheel_bot=perimeter_bot/perimeter_wheel;
00010     status=0;
00011 
00012  motor_left->setMotorCallback(this, &Creabot::callBackLeft);
00013  motor_right->setMotorCallback(this, &Creabot::callBackRight);
00014     extCallBack = NULL;
00015     setSpeed(DEFAULT_SPEED);
00016     callLeft = true;
00017     callRight = true;
00018     endMove=true;
00019     executingFifo=false;
00020 }
00021 
00022 void Creabot::moveForward(float cm)
00023 {
00024     float angle;
00025     if(cm<0) moveBackward(cm);
00026     angle = cm*degree_wheel_per_cm;
00027     setSpeedLeft(current_speed);
00028     setSpeedRight(current_speed);
00029    moveMotorLeft(COUNTERCLOCKWISE, angle);
00030     moveMotorRight(CLOCKWISE, angle);
00031 
00032 }
00033 
00034 void Creabot::moveBackward(float cm)
00035 {
00036     float angle;
00037     if(cm<0) moveForward(cm);
00038     angle = cm*degree_wheel_per_cm;
00039    setSpeedLeft(current_speed);
00040     setSpeedRight(current_speed);
00041     moveMotorLeft(CLOCKWISE, angle);
00042     moveMotorRight(COUNTERCLOCKWISE, angle);
00043 
00044 }
00045 void Creabot::rotate(float angleBot)
00046 {
00047     float angleWheel;
00048     setSpeedLeft(current_speed);
00049     setSpeedRight(current_speed);
00050    angleWheel = angleBot*ratio_wheel_bot;
00051     if(angleBot<0) {
00052         moveMotorLeft(CLOCKWISE, -angleWheel);
00053         moveMotorRight(CLOCKWISE, -angleWheel);
00054     } else {
00055         moveMotorLeft(COUNTERCLOCKWISE, angleWheel);
00056         moveMotorRight(COUNTERCLOCKWISE, angleWheel);
00057     }
00058 }
00059 void Creabot::moveRight(float angle)
00060 {
00061     moveRight(angle,distance_wheel);
00062 }
00063 void Creabot::moveRight(float angle, float center_cm)
00064 {
00065     if(center_cm<0) center_cm=0; /* TO remove? */
00066     float perimeter_outer = 2.0f*angle*(center_cm+distance_wheel)*PI/360.0f;
00067     float perimeter_inner = 2.0f*angle*(center_cm)*PI/360.0f;
00068     float  angleLeft = perimeter_outer*degree_wheel_per_cm;
00069     float  angleRight = perimeter_inner*degree_wheel_per_cm;
00070     float ratio = angleRight/angleLeft;
00071     float speedRight = current_speed * ratio;
00072    // 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);
00073     setSpeedLeft(current_speed);
00074     setSpeedRight(speedRight);
00075     moveMotorLeft(COUNTERCLOCKWISE, angleLeft);
00076     moveMotorRight(CLOCKWISE, angleRight);
00077 
00078 }
00079 void Creabot::moveLeft(float angle)
00080 {
00081     moveLeft(angle,0);
00082 }
00083 void Creabot::moveLeft(float angle, float center_cm)
00084 {
00085     if(center_cm<0) center_cm=0; /* TO remove? */
00086     float perimeter_outer = 2.0f*angle*(center_cm+distance_wheel)*PI/360.0f;
00087     float perimeter_inner = 2.0f*angle*(center_cm)*PI/360.0f;
00088     float  angleLeft = perimeter_inner*degree_wheel_per_cm;
00089     float  angleRight = perimeter_outer*degree_wheel_per_cm;
00090     float ratio = angleLeft/angleRight;
00091     float speedRLeft = current_speed * ratio;
00092     setSpeedLeft(speedRLeft);
00093     setSpeedRight(current_speed);
00094     moveMotorLeft(COUNTERCLOCKWISE, angleLeft);
00095     moveMotorRight(CLOCKWISE, angleRight);
00096 
00097 }
00098 void Creabot::stopMove()
00099 {
00100     motor_left->Stop();
00101     motor_right->Stop();
00102     callLeft = true;
00103     callRight = true;
00104     endMove = true;
00105     flushFifo();
00106     //  callBack(); // ? To be called or set as parameter?
00107 }
00108 int Creabot::getStatus()
00109 {
00110     if(endMove == true) return 0; // No Movement
00111     if(callRight == true) return 1; // Left Motor still running
00112     if(callLeft == true) return 3; // ERROR
00113     return 2; // Left Motor still running
00114 }
00115 
00116 void Creabot::setSpeed(float cm_per_second)
00117 {
00118     setSpeedLeft(cm_per_second);
00119     setSpeedRight(cm_per_second);
00120     current_speed = cm_per_second;
00121 }
00122 
00123 uint32_t Creabot::computeSpeed(Motor *motor, float cm_per_second)
00124 {
00125     uint32_t tick, timeUnit;
00126     tick = motor->getCalibration();
00127   if(cm_per_second <0.0001f) timeUnit = 1000000; else 
00128     timeUnit = perimeter_wheel * 1000000 / cm_per_second / tick;
00129     return timeUnit;
00130 }
00131 
00132 void Creabot::setSpeedLeft(float cm_per_second)
00133 {
00134     motor_left->setDelayBtwTicks(computeSpeed(motor_left, cm_per_second));
00135 }
00136 
00137 void Creabot::setSpeedRight(float cm_per_second)
00138 {
00139     motor_right->setDelayBtwTicks(computeSpeed(motor_right, cm_per_second));
00140 }
00141 
00142 void Creabot::waitEndMove()
00143 {
00144    while(!endMove || executingFifo) {
00145        wait(0.1);
00146      }
00147    
00148 }
00149 void Creabot::waitEndMove(uint32_t delay_us)
00150 {
00151     uint32_t v=0;
00152     while(!endMove || executingFifo) {
00153         wait(0.1);
00154         v++;
00155         if((v*10000)>delay_us) endMove = true;
00156     }
00157 } // watchdog
00158 
00159 void Creabot::setCallBack(void (*callback)(int status))
00160 {
00161     extCallBack = callback;
00162 }
00163 
00164 void Creabot::fifo(cmdbot_t moveType, float angle_or_cm)
00165 {
00166     CommandBot *cmd = fifoBot.put();
00167     if(cmd!=NULL) {
00168         cmd->add_command(moveType, angle_or_cm, 0);
00169     }
00170     executeFifo();
00171 }
00172 void Creabot::fifo(cmdbot_t moveType, float angle, float cm)
00173 {
00174     CommandBot *cmd = fifoBot.put();
00175     if(cmd!=NULL) {
00176         cmd->add_command(moveType, angle, cm);
00177    }
00178     executeFifo();
00179 }
00180 
00181 
00182 void Creabot::moveAndWait(cmdbot_t moveType, float angle_or_cm)
00183 {
00184         move(moveType,angle_or_cm);
00185         waitEndMove();
00186 }
00187 void Creabot::moveAndWait(cmdbot_t moveType, float angle, float cm)
00188 {
00189         move(moveType,angle,cm);
00190         waitEndMove();
00191 }
00192 
00193 void Creabot::move(cmdbot_t moveType, float angle_or_cm)
00194 {
00195         current_cmd.add_command(moveType, angle_or_cm, 0);
00196     executeCommand(&current_cmd);
00197 }
00198 void Creabot::move(cmdbot_t moveType, float angle, float cm)
00199 {
00200          current_cmd.add_command(moveType, angle, cm);
00201      executeCommand(&current_cmd);
00202 }
00203 
00204 void Creabot::flushFifo()
00205 {
00206     fifoBot.empty();
00207     if(executingFifo) botTicker.detach();
00208     executingFifo=false;
00209 }
00210 int Creabot::moveInFifo()
00211 {
00212     return fifoBot.getSize();
00213 }
00214 void Creabot::executeCommand(CommandBot *cmd) {
00215           switch (cmd->command) {
00216             case FORWARD:
00217                 moveForward(cmd->cm);
00218                 break;
00219             case BACKWARD:
00220                 moveBackward(cmd->cm);
00221                 break;
00222             case ROTATE:
00223                 rotate(cmd->angle);
00224                 break;
00225             case LEFT:
00226                 moveLeft(cmd->angle, cmd->cm);
00227                 break;
00228             case RIGHT:
00229                 moveRight(cmd->angle, cmd->cm);
00230                 break;
00231             case IDLE:
00232               //  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... 
00233                 break;
00234             default:
00235                 break;
00236         };
00237 }
00238 void Creabot::executeFifo()
00239 {
00240 
00241     if(fifoBot.getSize()==0) flushFifo();
00242     else {
00243         if(endMove) {
00244             CommandBot *cmd = fifoBot.get();
00245             executeCommand(cmd);
00246             if(!executingFifo) botTicker.attach_us(callback(this,&Creabot::executeFifo),3150);
00247             executingFifo=true;
00248 
00249         }
00250     }
00251 }
00252 
00253 void Creabot::callBackLeft()
00254 {
00255     callLeft=true;
00256     if(callRight)callBack();
00257 }
00258 void Creabot::callBackRight()
00259 {
00260     callRight=true;
00261     if(callLeft)callBack();
00262 }
00263 
00264 void Creabot::callBack()
00265 {
00266     endMove=true;
00267     if(extCallBack!=NULL) extCallBack(status);
00268 }
00269 
00270 void Creabot::moveMotorLeft(MotorDir dir, float angle)
00271 {
00272    if(angle>0) {
00273         motor_left->RunDegrees(dir, angle);
00274         endMove = callLeft = false;
00275    }
00276 }
00277 void Creabot::moveMotorRight(MotorDir dir, float angle)
00278 {
00279     if(angle>0) {
00280         motor_right->RunDegrees(dir, angle);
00281         endMove = callRight = false;
00282     }
00283 }