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