void MyRobot::run()
{
while (step(_time_step) != -1)
{
//Get the encoder values
_left_encoder = getLeftEncoder();
_right_encoder = getRightEncoder();
//Converts rad to meters
if(flag == 2 || flag == 8){
_distance = _right_encoder/ENCODER_RESOLUTION*WHEEL_RADIO;
}else{
_distance = _left_encoder/ENCODER_RESOLUTION*WHEEL_RADIO;
}
print_odometry_data();
//switch case to make the necesary steps to avoid the obstacle
switch (flag){
case 1:
cout << "linea recta"<< endl;
goStraight(5);
break;
case 2:
turn_left();
break;
case 3:
goStraight(3.5);
break;
case 4:
turn_right();
break;
case 5:
goStraight( 7);
break;
case 6:
turn_right();
break;
case 7:
goStraight(3.5);
break;
case 8:
turn_left();
break;
case 9:
goStraight(5);
break;
default:
_left_speed = 0;
_right_speed = 0;
break;
}
setSpeed(_left_speed, _right_speed);
}
}
/**
* @function approach
* @brief
*/
void robotController::approach( float _dist, float _danceDist, int _numberSteps ) {
// Go straight
goStraight( _dist - _danceDist, 0.5 );
// Dance
for( unsigned int i = 0; i < _numberSteps; ++i ) {
goStraight( _danceDist, 0.5 );
goStraight( -1*_danceDist, 0.3 );
}
}
task main()
{
wait1Msec(2000); //Robot waits for 2000 milliseconds before executing program
bMotorReflected[port2] = 1; //Reflects the direction of the motor on port2
goRight();
wait1Msec(10*1000); //Robot runs previous code for 10*1000 milliseconds before moving on
stopBot();
wait1Msec(1000);
goLeft();
wait1Msec(10*1000); //Robot runs previous code for 10*1000 milliseconds before moving on
stopBot();
wait1Msec(1000);
goStraight();
wait1Msec(10*1000); //Robot runs previous code for 10*1000 milliseconds before moving on
stopBot();
wait1Msec(1000);
goBack();
wait1Msec(10*1000); //Robot runs previous code for 10*1000 milliseconds before moving on
stopBot();
wait1Msec(1000);
} //Program ends, and the robot stops
/**
* @function peekaboo_left
* @brief Walk - look - walk - look
*/
void robotController::peekaboo_left( float _lengthStride, float _visionAngle, int _numStrides ) {
printf("Start peekaboo left\n");
for( unsigned int i = 0; i < _numStrides; ++i ) {
goStraight( _lengthStride );
rotate( _visionAngle );
rotate( -1*_visionAngle );
}
printf("[peekaboo_left] Finished number of strides: %d \n", _numStrides);
}
/**
* Gives power to motors, keeping the center of the block aligned with the requested center
* set in the Drivetrain constructor.
*/
void Drivetrain::goToFish(Block block)
{
//If the center of the block is on the left side of the camera
if(block.x < _center)
{
turnLeft(_power);
}
//If the center of the block is on the right side of the camera
else if(block.x > _center)
{
turnRight(_power);
}
//The center of the block is in the center of the camera
else
{
goStraight(_power);
}
}
task main()
{
intDirections();
dirDecode();
StartTask(sensors); //start all our other tasks
StartTask(motors);
StartTask(line);
jumpTo();
bFloatDuringInactiveMotorPWM = false; //make sure the motors brake, not float
// resetMotors = true;
while(true)
{
if(directions[dirIndex][0] == 000)
{
StopAllTasks();
}
eraseDisplay();
nxtDisplayRICFile(0, 0, "nKISA.RIC"); // displaying our logo
nxtDisplayBigStringAt(65, 55, "%d", dirIndex);
nxtDisplayStringAt(65, 25, "%d", directions[dirIndex][0]);
nMotorPIDSpeedCtrl[left] = mtrSpeedReg; //turn off the PID for the motors,
nMotorPIDSpeedCtrl[right] = mtrSpeedReg; //better reaction time
if(dirIndex>1)
{
if(directions[dirIndex-1][1] == 2 && directions[dirIndex][2] == 2) //if our last turn was a left turn AND we
{ //are going to follow the opposite line, move over to that side
turning = true;
motor[left] = 50;
while(nMotorEncoder[left] <= 200) {}
motor[left] = 0;
motor[right] = 50;
while(nMotorEncoder[right] <= 200) {}
motor[right] = 0;
turning = false;
}
if(directions[dirIndex-1][1] == 3 && directions[dirIndex][2] == 1) //same as last, except this is done if
{ //last turn was a right, AND we are
turning = true; //following the left line
motor[right] = 50;
while(nMotorEncoder[right] <= 200) {}
motor[right] = 0;
motor[left] = 50;
while(nMotorEncoder[left] <= 200) {}
motor[left] = 0;
turning = false;
}
}
resetMotors = true;
targetDPS = 450;
lineFollow = true;
wait1Msec(100);
if(directions[dirIndex][1] == 6 || directions[dirIndex][1] == 7) // if parking...
{
for(int i = 0; i < 30; i = i) //if we have seen blue continually for a
{ //certain amount of time,
if(lineColor == 2)
{
i++;
}
else
{
i = 0;
}
writeDebugStreamLine("lineColor is: %d", lineColor);//white to the debug
wait1Msec(10); //stream, better than
} //viewing variables over BT
resetMotors = true;
wait1Msec(20);
while(lastPos < ((parkDistance * (directions[dirIndex][3] - 1)) + 100))
{
wait1Msec(10);
}
lineFollow = false;
targetDPS = 0;
wait1Msec(500);
resetMotors = true;
wait1Msec(500);
turning = true;
switch(directions[dirIndex][1]) // which side of the road are we parking on?
{
case 6:
parkLeft();
break;
case 7:
parkRight();
break;
}
}
else // if not parking, follow the procedure for stopping
{
while(stopSensor != 5) // while the stopSensor is not seeing a stop sign
{
writeDebugStreamLine("stopsensor: %d", stopSensor);
wait1Msec(10); // keep following the line
}
writeDebugStreamLine("!!!STOPPED AT: %d", stopSensor);
PlaySound(soundBeepBeep);
lineFollow = false;
targetDPS = 0;
wait1Msec(500);
resetMotors = true;
wait1Msec(500);
turning = true;
// squareLine(1);
switch(directions[dirIndex][1]) // go to the respective turning functioin
{
case 1: goStraight(); break;
case 2: turnLeft(); break;
case 3: turnRight(); break;
case 4: turnLeftL(); break;
case 5: turnRightL(); break;
}
}
if(directions[dirIndex][1] == 9) //for exiting the city
{
while(lineColor != 5) {}
resetMotors = true;
wait1Msec(20);
while(lastPos < 200)
{
wait1Msec(10);
}
lineFollow = false;
targetDPS = 0;
wait1Msec(500);
resetMotors = true;
wait1Msec(500);
turning = true;
exit();
}
turning = false;
resetMotors = true;
//directions[dirIndex-1][1] = directions[dirIndex][1]; // needed for if we need to move over to the
dirIndex ++; //other side of the road after turn
// if(dirIndex == 14) // for an endless loop on our practice mat
// {
// dirIndex = 0;
// }
//dirDecode();
}
}
void firsteight(){
getIRDist();
goStraight();
//turnRight(90, TURNSPEED);
turnRight(100, TURNSPEED);
straight(70, SPEED);
//turnRight(45,TURNSPEED);
turnRight(55, TURNSPEED);
goStraight();
turnRight(90, TURNSPEED);
straight(30, SPEED);
//straight(50, SPEED);
//turnRight(45, TURNSPEED);
turnRight (55, TURNSPEED);
getIRDist();
while (1){
if (rightSideIR < 40)
{
break;}
straight(18, SPEED);
getIRDist();
}
goStraight();
// The second bit
straight(15, SPEED);
//straight(18, SPEED);
turnLeft(90, TURNSPEED);
getIRDist();
while (1){
if (leftSideIR < 40)
{
break;}
straight(18, SPEED);
getIRDist();
}
goStraightLeft();
if (SPEED <= 20)
{ straight(25, SPEED); }
turnLeft(90, TURNSPEED);
if (SPEED <= 20)
{ straight(80, SPEED); }
else {
//straight(40, SPEED);
straight(25, SPEED);
}
//turnLeft (45, TURNSPEED);
turnLeft(55, TURNSPEED);
/*
//for speed 40
getIRDist();
while (1){
if (leftSideIR < 40)
{ straight(15, SPEED);
break;
}
straight(10, SPEED);
getIRDist();
}
*/
straight(15, SPEED);
goStraightLeft();
turnLeft(90, TURNSPEED);
//straight(25, SPEED);
straight (60, SPEED);
//turnLeft(45, TURNSPEED);
turnLeft(55, TURNSPEED);
getIRDist();
while (1){
if (leftSideIR < 40)
{
break;
}
straight(10, SPEED);
getIRDist();
}
goStraightLeft();
//For 60 only
straight(25, SPEED);
turnRight(90, TURNSPEED);
getIRDist();
while (1){
if (rightSideIR < 40)
{ straight (25, SPEED);
break;}
straight(18, SPEED);
getIRDist();
}
}
void loop() {
//Constant Variables
zeroVec[0] = zeroVec[1] = zeroVec[2] = floatZero;
//Variable Initialization
time ++;
api.getMyZRState(myState);
api.getOtherZRState(otherState);
phase = game.getCurrentPhase();
dust = game.getRemainingMaterial();
chg = game.getCharge();
if (!time)
red = myState[0] < floatZero;
if (time < 30 && otherState[1] > 0.1f)
otherRush = true;
//Trilateration
api.getMyZRState(tripos[time]);
game.pingForItems(tridis[time]);
//Debug Console
#if printDebug
DEBUG(("\nUS#7 DevilTech BACON Hart District\nMessage Screen: Fall 2012\n"));
DEBUG(("Debug Info\n"));
DEBUG(("\tTime (s)\t%d\tPhase\t%d\tPlayer\t%s\n", time, phase, (red ? "Red" : "Blue")));
DEBUG(("\tFuel\t%.3f\tDust\t%.3f\tChg\t%d\n", game.getFuelRemaining(), dust, chg));
DEBUG(("\tMy Score\t%.3f\tOther Score\t%.3f\n", game.getScore(), game.getOtherScore()));
DEBUG(("\tObstacles Created\t%d\n", obsCreated));
DEBUG(("\tOther Rush\t%d\n", otherRush));
printVec("\tMy Pos (m)", myState);
printVec("\tMy Vel (m/s)", (myState + 3));
printVec("\tMy Att (unitvec)", (myState + 6));
printVec("\tMy AngVel (rad/s)", (myState + 9));
printVec("\tOther Pos (m)", otherState);
printVec("\tOther Vel (m/s)", (otherState + 3));
printVec("\tOther Att (unitvec)", (otherState + 6));
printVec("\tOther AngVel (rad/s)", (otherState + 9));
#endif
//Game Logic
if (time < 4) {
api.setVelocityTarget(makeVec(floatZero, floatZero, 0.0095f));
}
if (!time) {
game.startObstacle();
return;
}
if (time == 1) {
game.stopObstacle();
}
if (!game.haveObject(0)) {
if (!game.haveObject(1) && !game.otherHasObject(1))
getOne;
else
getZero;
}
else {
vec temp = {(red ? 0.15f : -0.15f), floatZero, myState[2]};/*
if (!obsDetected && ((red && myState[0] > -0.05f) || (!red && myState[0] < 0.05f))) {
int res = game.extendView();
if (printDebug) {
DEBUG(("Extending View: %d\n", res));
}
}*/
float tryy[7] = {(otherRush ? myState[2] : 0.38f), 0.11f, 0.22f, 0.34f, 0.45f, 0.55f, 0.68f};
obsDetected = game.getIdentifiedObstacles(obstacles);
bool shr = false;
for (int i = 0; i < 7; i ++) {
if (printDebug)
DEBUG(("\tTrying %.3f\n", tryy[i]));
bool good = true;
temp[2] = tryy[i];
for (int j = 0; j < obsDetected; j ++) {
temp[1] = obstacles[j].loc[1];
if (printDebug) {
DEBUG(("\t\tID %d, rad %.3f\n", obstacles[j].ID, obstacles[j].size));
printVec("\t\tLocation", obstacles[j].loc);
DEBUG(("\t\tDis %.3f\n", disBetweenPts(obstacles[j].loc, temp)));
}
if (disBetweenPts(obstacles[j].loc, temp) < obstacles[j].size + 0.058f) {
good = false;
break;
}
if (!shr && obstacles[j].visible) {
DEBUG(("\t\tShrinking\n"));
shr = true;
game.shrinkObstacle(obstacles[j].ID);
}
}
if (good) {
if (printDebug)
DEBUG(("Decided %.3f\n", tryy[i]));
temp[2] = tryy[i];
break;
}
}
temp[1] = floatZero;
if ((red && myState[0] < floatZero) || ((!red && myState[0] > floatZero))) {
goStraight(temp, 1, -0.024f);
}
else if (myState[1] > -0.66f) {
goStraight(temp, 1, (myState[1] > -0.48f && fabs(myState[2] - temp[2]) < 0.058f ? -0.0475f : -0.0235f));
}
else {
if (!alreadySet) {
alreadySet = true;
top = myState[2] > floatZero;
}
temp[1] = -0.71f;
goStraight(temp, 2, top ? -0.055f : 0.055f);
}
}
}
