task main()
{
initGyro();
// waitForStart();
// wait1Msec(2000);
driveTo(10000); // come off the ramp
turnDegrees(-90); // turn so we are pointed to the ball
resetEncoders();
driveTo(-6500, -90); // drive towards ball
turnDegrees(-50); // angle toward ball so we can push it and end up inside the park zone
resetEncoders();
driveTo(-15000, -43); // drive into zone
resetEncoders();
driveTo(1000);
holdSpinners();
}
task main()
{
initGyro();
waitForStart();
//wait1Msec(5000);
driveTo(10000);
turnDegrees(90);
resetEncoders();
driveTo(-4000, 90);
turnDegrees(60);
resetEncoders();
driveTo(-17000, 60);
resetEncoders();
driveTo(500);
holdSpinners();
}
void FlightController::turnTowardsPoint(Command waypoint) {
Vector3 pos = navData->getPosition();
ROS_INFO("X = %f ", pos.x);
ROS_INFO("Y = %f ", pos.y);
double target_angle = atan2(waypoint.x - pos.x,waypoint.y - pos.y); // angle towards waypoint position
double target_deg = target_angle * 180 / M_PI; // conversion to degrees
ROS_INFO("Target angle = %f", target_angle);
ROS_INFO("Target degrees = %f", target_deg);
if (target_deg < 0)
target_deg += 360;
if (target_deg > 360)
target_deg -= 360;
double ori_deg = navData->getRotation();
ROS_INFO("Original degrees = %f", ori_deg);
turnDegrees(target_deg - ori_deg);
}
void onCustom(message_t* msg) {
CustomCommands cmd = (CustomCommands)getType(msg);
switch(cmd) {
#ifdef MAZESOLVER
case INIT_MAZE: {
LOGi("mazSolver.init");
// Storage::setCurrentProgram(Prog_MazeSolver);
LOGi("Program MazeSolver selected");
// mazeSolver.init();
break;
}
case START_MAZE: {
LOGi("mazSolver.start");
// mazeSolver.start();
break;
}
case STOP_MAZE: {
LOGi("mazSolver.stop");
// mazeSolver.stop();
break;
}
case REPEAT_MAZE: {
LOGi("mazSolver.repeat");
// mazeSolver.repeat();
break;
}
case INIT_LINE_FOLLOWER: {
LOGi("linefollower.init");
// Storage::setCurrentProgram(Prog_LineFollower);
LOGi("Program LineFollower selected");
// mazeSolver.init();
break;
}
case START_LINE_FOLLOWER: {
LOGi("linefollower.start");
// mazeSolver.start();
break;
}
case STOP_LINE_FOLLOWER: {
LOGi("linefollower.stop");
// mazeSolver.stop();
break;
}
#endif
#ifdef LINE_FOLLOWER
case INIT_LINE_FOLLOWER: {
LOGi("linefollower.init");
// linefollower.init();
break;
}
case START_LINE_FOLLOWER: {
LOGi("linefollower.start");
// linefollower.start();
break;
}
case STOP_LINE_FOLLOWER: {
LOGi("linefollower.stop");
// linefollower.stop();
break;
}
#endif
#ifdef USE_COMPASS
case CALIBRATE_COMPSS: {
LOGi("compass.calibrate");
compass.calibrate();
break;
}
case INIT_HEADING: {
LOGi("compass.reset");
calibrateHeading();
break;
}
case TURN_DEG: {
LOGi("compass.turn");
turndegree_payload* payload = (turndegree_payload*) msg->payload;
turnDegrees(payload->angle);
break;
}
case SET_HEADING: {
LOGi("setAbsAngle");
turndegree_payload* payload = (turndegree_payload*) msg->payload;
setTargetHeading(payload->angle);
break;
}
#endif
#ifdef SUMO
case START_SUMO: {
LOGi("sumo.start");
// Storage::setCurrentProgram(Prog_Sumo);
LOGi("Program Sumo selected");
// sumo.start();
break;
}
case STOP_SUMO: {
LOGi("sumo.stop");
// sumo.stop();
break;
}
#endif
// case SET_PROGRAM: {
// program_payload* payload = (program_payload*)msg->payload;
// _currentProgram = (Program)payload->program;
// Storage::setCurrentProgram(_currentProgram);
// break;
// }
case SET_WHITE_LINES: {
whitelines_payload* payload = (whitelines_payload*)msg->payload;
_whiteLines = (bool)payload->whiteLines;
Storage::setWhiteLines(_whiteLines);
break;
}
}
}
void FlightController::run() {
double yUpperLimit = 9300;
double lowerLimit = 1500;
double xUpperLimit = 8100;
Route myRoute;
myRoute.initRoute(true);
ros::Rate loop_rate(LOOP_RATE);
setStraightFlight(true);
bool firstIteration = true;
takeOff();
bool turning = true;
double amountTurned = 0;
double turnStepSize = 30;
hoverDuration(3);
navData->resetRawRotation();
hoverDuration(2);
cmd.linear.z = 0.5;
pub_control.publish(cmd);
while (navData->getPosition().z < 1200)
ros::Rate(LOOP_RATE).sleep();
hoverDuration(1);
Vector3 pos;
double starting_orientation = navData->getRawRotation();
//ROS_INFO("Start while");
while (!dronePossision.positionLocked) {
//ROS_INFO("in while");
if (turning) {
turnDegrees(turnStepSize);
double orientation = navData->getRawRotation();
amountTurned += angleDifference(starting_orientation,orientation);
//ROS_INFO("AMOUNTTURNED: %f",amountTurned);
starting_orientation = orientation;
if (amountTurned >= 360)
turning = false;
} else {
navData->resetRaw();
flyForward(0.4);
turning = true;
amountTurned = 0;
}
}
lookingForQR = false;
cmd.linear.z = -0.5;
pub_control.publish(cmd);
while (navData->getPosition().z > 900)
ros::Rate(LOOP_RATE).sleep();
//ROS_INFO("end while");
navData->resetToPosition(dronePossision.x * 10, dronePossision.y * 10, dronePossision.heading);
int startWall = dronePossision.wallNumber;
turnDegrees(-dronePossision.angle);
if(startWall == 0) {
while (navData->getPosition().y - 700 > lowerLimit) {
flyBackward(0.7);
navData->addToY(-700);
hoverDuration(3);
}
strafe(1, 0.8);
navData->addToX(1000);
hoverDuration(2);
while (navData->getPosition().y + 700 < yUpperLimit) {
flyForward(0.5);
navData->addToX(700);
hoverDuration(3);
}
} else if(startWall == 2){
while (navData->getPosition().y + 700 < yUpperLimit) {
flyForward(0.5);
navData->addToX(700);
hoverDuration(3);
}
strafe(1, 0.8);
navData->addToX(-1000);
hoverDuration(2);
while (navData->getPosition().y - 700 > lowerLimit) {
flyBackward(0.7);
navData->addToY(-700);
hoverDuration(3);
}
} else if(startWall == 3) {
while (navData->getPosition().x + 700 < xUpperLimit){
flyBackward(0.7);
navData->addToX(700);
hoverDuration(3);
}
strafe(1, 0.8);
navData->addToY(1000);
hoverDuration(2);
while (navData->getPosition().x - 700 > xUpperLimit){
flyBackward(0.7);
navData->addToX(-700);
hoverDuration(3);
}
} else if(startWall == 1){
while (navData->getPosition().x + 700 < xUpperLimit){
flyBackward(0.7);
navData->addToX(700);
hoverDuration(3);
}
strafe(1, 0.8);
navData->addToY(-1000);
hoverDuration(2);
while (navData->getPosition().x - 700 > xUpperLimit){
flyBackward(0.7);
navData->addToX(-700);
hoverDuration(3);
}
}
/* else if(dronePossision.wallNumber == 1){
while (navData->getPosition().y - 1000 > 1500) {
flyForward(0.7);
navData->addToY(-1000);
hoverDuration(3);
cvHandler->swapCam(false);
cvHandler->checkCubes();
cvHandler->swapCam(true);
}
}*/
/*double rotation = navData->getRotation();
double target;
int wallNumber = dronePossision.wallNumber;
ROS_INFO("WALL NUMBER RECEIVED START: %d", wallNumber);
switch(dronePossision.wallNumber) {
case 0:
target = 180;
break;
case 1:
target = 270;
break;
case 2:
target = 0;
break;
case 3:
target = 90;
break;
}
ROS_INFO("Target = %f", target);*/
/*int i = 0;
while(i < 4) {
double difference = angleDifference(rotation, target);
int direction = angleDirection(rotation, target);
ROS_INFO("Difference = %f", difference);
ROS_INFO("Direction = %f", direction);
//turnDegrees(difference*direction);
cmd.angular.z = 1;
pub_control.publish(cmd);
while(navData->getRotation() > 190 || navData->getRotation() < 170 )
ros::Rate(50).sleep();
//ROS_INFO("Difference = %f", difference);
//ROS_INFO("Direction = %f", direction);
//turnTowardsAngle(target, 0);
/* lookingForQR = true;
flyForward(0.7);
cvHandler->swapCam(false);
hoverDuration(3);
cvHandler->checkCubes();
cvHandler->swapCam(true);
flyForward(0.7);
cvHandler->swapCam(false);
hoverDuration(3);
cvHandler->checkCubes();
cvHandler->swapCam(true);
flyForward(0.7);
cvHandler->swapCam(false);
hoverDuration(3);
cvHandler->checkCubes();
cvHandler->swapCam(true);
flyForward(0.7);
cvHandler->swapCam(false);
hoverDuration(3);
cvHandler->checkCubes();
cvHandler->swapCam(true);
lookingForQR = true;
strafe(0.7, -1);
cvHandler->swapCam(false);
hoverDuration(3);
cvHandler->checkCubes();
cvHandler->swapCam(true);
strafe(0.7, -1);
cvHandler->swapCam(false);
hoverDuration(3);
cvHandler->checkCubes();
cvHandler->swapCam(true);
strafe(0.7, -1);
cvHandler->swapCam(false);
hoverDuration(3);
cvHandler->checkCubes();
cvHandler->swapCam(true);
strafe(0.7, -1);
cvHandler->swapCam(false);
hoverDuration(3);
cvHandler->checkCubes();
cvHandler->swapCam(true);
if(target == 0)
while(navData->getPosition().y +1000 < 9300) {
flyForward(0.7);
navData->addToY(1000);
hoverDuration(4);
cvHandler->swapCam(false);
cvHandler->checkCubes();
cvHandler->swapCam(true);
if (navData->getRotation() != target) {
difference = angleDifference(rotation, target);
direction = angleDirection(rotation, target);
ROS_INFO("Difference = %f", difference);
ROS_INFO("Direction = %f", direction);
turnDegrees(difference * direction);
}
}
else if (target == 180)
while (navData->getPosition().y - 1000 > 1500) {
flyForward(0.7);
navData->addToY(-1000);
hoverDuration(4);
cvHandler->swapCam(false);
cvHandler->checkCubes();
cvHandler->swapCam(true);
if (navData->getRotation() != target) {
difference = angleDifference(rotation, target);
direction = angleDirection(rotation, target);
ROS_INFO("Difference = %f", difference);
ROS_INFO("Direction = %f", direction);
turnDegrees(difference * direction);
}
}
}
*/
land();
return;
}
task main()
{
/*
* INITIALIZATION AND SETUP
* This first section gets the robot ready to start the match. It sets all the motor and servo
* starting positions, and prints some diagnostic information to the NXT and the debug stream.
*/
// Print a copyright notice to the debug stream
string programName = "Autonomous";
printWelcomeMessage(programName, programVersion);
writeDebugStreamLine("Getting autonomous settings...");
PlaySound(soundBeepBeep);
runMenu(); // Get gameplay decisions from the operators
initializeRobot(); // Set the robot to its starting positions
// Notify the users that the program is ready and running
writeDebugStreamLine("Waiting for start of match...");
nxtDisplayTextLine(0, "2015 Powerstackers");
nxtDisplayCenteredBigTextLine(1, "AUTO");
nxtDisplayCenteredBigTextLine(3, "READY");
PlaySound(soundFastUpwardTones);
wait10Msec(200);
/*
* DEBUG MODE
* If debug mode is activated, bypass the waitForStart function.
* Display a 3 second audio and visual countdown, and then execute.
*/
if(debugMode)
{
ClearSounds();
for(int i = 3; i>0; --i)
{
nxtDisplayCenteredBigTextLine(3, "IN %d", i);
PlaySound(soundException);
wait10Msec(100);
}
}
else
{
// Wait for the "starting gun" from the field control system
waitForStart();
}
// Notify the users that the program is running
nxtDisplayCenteredBigTextLine(3, "RUNNING");
PlaySound(soundUpwardTones);
//StartTask(watchMotors);
/*
* GAMEPLAY
* From this point on, the program is split into different sections based on the options chosen in
* menu earlier. The two main options are the starting position and the game mode (offense or defense).
*/
if(startingPosition==STARTING_RAMP)
{
/*
* RAMP POSITION
* Starting from this position means that you can only access the rolling goals and the kickstand.
* In this position, offensive is mostly the only available game mode.
*/
// OFFENSIVE MODE
if(offenseOrDefense==OFFENSIVE_MODE)
{
// Go straight down the ramp
goTicks(inchesToTicks(-25), 30);
turnDegrees(5, 50);
goTicks(inchesToTicks(-40), 50);
//this will make the robot drop the balls into the rolling goal
grabTube();
//this will make the robot turn to move twards the parking zone
turnDegrees (-8,preferredTurnSpeed);
goTicks(inchesToTicks(12), 75);
turnDegrees(-20, preferredTurnSpeed);
//this will make the robot move twards the parking zone
goTicks (inchesToTicks(78), 75);
//this will make the robot turn so it can be in place so it will move in the parking zone
turnDegrees (-80,preferredTurnSpeed);
goTicks(inchesToTicks(10), 50);
dropBall(liftTargetMed);
} // END OFFENSE
// DEFENSIVE MODE
else if(offenseOrDefense==DEFENSIVE_MODE)
{
// Just go in a straight line down the ramp
goTicks(inchesToTicks(80), 50);
} // END DEFENSE
} // END RAMP START
else if(startingPosition==STARTING_FLOOR)
{
/*
* FLOOR POSITION
* In this position, you are available to block the other team, or to score in the high goal. You
* also have easy access to the kickstand. It is easiest to determine the orientation of the
* center tower from this position.
*/
// Detect and store the center goal position
goTicks(inchesToTicks(-30), 100);
char goalFacing = findGoalOrientation();
// OFFENSIVE MODE
if(offenseOrDefense==OFFENSIVE_MODE)
{
// Move slightly forwards to get into a better detecting position
//goTicks(inchesToTicks(-20), 50);
// Make a movement based on the position of the center goal
switch (goalFacing)
{
// GOOD
case CENTGOAL_POSITION_A :
{
// In this position, simply go in a straight line
goTicks(inchesToTicks(-10),50);
break;
}
// BAD
case CENTGOAL_POSITION_B :
{
// Turn 45 degrees, tangent to the center structure
turnDegrees (45,preferredTurnSpeed);
// Move into position in front of the goal
goTicks(inchesToTicks(-30),75);
// Turn to face the goal
turnDegrees (-90,preferredTurnSpeed); writeDebugStreamLine("Done");
break;
}
// GOOD
case CENTGOAL_POSITION_C :
{
// Turn 90 degrees, parallel to the center structure
turnDegrees (-90,preferredTurnSpeed);
// Move along the center structure until we're past the end
goTicks(inchesToTicks(-38),50);
// Turn 90 degrees, tangent to the center structure
turnDegrees (90,preferredTurnSpeed);
// Move into position in front of the center structure
goTicks(inchesToTicks(-62),65);
// Turn to face the goal
turnDegrees(90,preferredTurnSpeed);
break;
}
default:
{
// In an ambiguous situtation, assume the goal is in A position
goTicks(inchesToTicks(-20),75); writeDebugStreamLine("Done");
break;
}
}
// Print that the alignment is done
writeDebugStreamLine("Aligned with center structure");
// Position the robot to drop the ball in the center goal
nMotorEncoder[mLift] = 0;
moveMotorTo(mLift, liftTargetCent, 75);
// Drop the ball in the center goal
servo[rTrapDoor]=trapDoorOpenPosition;
wait10Msec (500);
servo[rTrapDoor]=trapDoorClosedPosition;
// Drop down the lift
moveMotorTo(mLift, liftTargetBase, 75);
// Position the robot correctly to kick the kickstand
turnDegrees (90, preferredTurnSpeed);
goTicks(inchesToTicks(-15), 75);
turnDegrees (90, preferredTurnSpeed);
goTicks(inchesToTicks(35), 100);
// Go to the robot's ending position
} // END OFFENSE
// DEFENSIVE MODE
else if(offenseOrDefense==DEFENSIVE_MODE)
{
/*
* Move from the starting position to block the opponent's rolling goals
*/
// Move 2 feet backwards at full power, to get out of the parking zone
//goTicks(inchesToTicks(-24), 100);
// Turn towards the opponent rolling goals
turnDegrees(90, preferredTurnSpeed);
// Drive forward and backward to disrupt the opponent 60cm goal
goTicks(inchesToTicks(60), 100);
goTicks(inchesToTicks(-12), 100);
// turn slightly to get the 90cm goals
turnDegrees(-45, preferredTurnSpeed);
// Move forward and backwards to disrupt the 90cm goal
goTicks(inchesToTicks(36), 75);
goTicks(inchesToTicks(-36), 100);
return;
// Turn slightly to face the 30cm goal
turnDegrees(-45, preferredTurnSpeed);
// Move foward and backward to disrupt the 30cm goal
goTicks(inchesToTicks(36), 75);
goTicks(inchesToTicks(-4), 100);
// Move into position to align on the wall of the ramp
turnDegrees(-90, preferredTurnSpeed);
//goTicks(inchesToTicks(12), 100);
//turnDegrees(90, preferredTurnSpeed);
return;
// Align ourselves with this wall
//wallAlign(ALIGN_FORWARD);
/*
* BLOCK CETNER GOAL
* Move the robot in front of the opponent's center goal
*/
// Move into position for use to block the center goal
goTicks(inchesToTicks(-6), 100); // Move away from the wall
turnDegrees(90, preferredTurnSpeed); // Turn to face OUR ramp
goTicks(inchesToTicks(-36), 100); // Move foward, past the center goal
//turnDegrees(90, preferredTurnSpeed); // Turn parallel to the cetner structure
// Move to block the center goal, in one of three positions
switch(goalFacing)
{
// Farthest position
case CENTGOAL_POSITION_A:
{
writeDebugStreamLine("-- BLOCKING GOAL POSITION A --");
goTicks(inchesToTicks(-80), 100); // Move alongside the goal
turnDegrees(90, preferredTurnSpeed); // Turn tangent to the center structure
goTicks(inchesToTicks(20), 100); // Move in front of the goal
break;
}
// Middle position
case CENTGOAL_POSITION_B:
{
writeDebugStreamLine("-- BLOCKING GOAL POSITION B --");
goTicks(inchesToTicks(-40), 100); // Move alongside the goal
turnDegrees(45, preferredTurnSpeed); // Turn tangent to the center structure
goTicks(inchesToTicks(20), 100); // Move in front of the goal
break;
}
// Closest position
case CENTGOAL_POSITION_C:
{
writeDebugStreamLine("-- BLOCKING GOAL POSITION C --");
goTicks(inchesToTicks(-20), 100); // Move in front of the goal
break;
}
default:
{
break;
}
}
} // END DEFENSE
} // END FLOOR START
// After the program has been carried out, play a cute "done" sound
nxtDisplayCenteredBigTextLine(3, "DONE");
PlaySound(soundBeepBeep);
wait10Msec(200);
} // END
task main()
{
// Some kind of gyro sensor calibration
calibrateGyro(gyro);
while(true)
{
// Update joystick values
getJoystickSettings(joystick);
// Drive code
motor[left] = speedCurve(joystick.joy1_y2);
motor[right] = speedCurve(joystick.joy1_y1);
//motor[slide] = speedCurve(joystick.joy1_y1) / 2;
//motor[intake] = speedCurve(joystick.joy1_y2) / 2;
// Bumper controls for linear slides
if(joy1Btn(5) || joy2Btn(5))
{
motor[slide] = 75;
}
else if(joy1Btn(6) || joy2Btn(6))
{
motor[slide] = -75;
}
else
{
motor[slide] = 0;
}
// Trigger controls for intake systems
if(joy1Btn(7) || joy2Btn(7))
{
if (motor[intake] == 100)
{
motor[intake] = 0;
}
else
{
motor[intake] = 100;
}
wait1Msec(100);
}
if(joy1Btn(8) || joy2Btn(8))
{
if (motor[intake] == -50)
{
motor[intake] = 0;
}
else
{
motor[intake] = -50;
}
wait1Msec(100);
}
if(joystick.joy1_TopHat == 0)
{
motor[right] = 75;
motor[left] = 75;
}
// Turn on the gyro stabilization, ask Ethan
while(joystick.joy1_TopHat == 0)
{
getJoystickSettings(joystick);
if(SensorValue(gyro) > offset) //turning right
{
motor[right] = motor[right] + 1;
}
if(SensorValue(gyro) < offset) //turning left
{
motor[left] = motor[left] + 1;
}
gyroed = true;
}
if(joystick.joy1_TopHat == 4)
{
motor[right] = -75;
motor[left] = -75;
}
// Turn on the gyro stabilization backwards, ask Ethan
while(joystick.joy1_TopHat == 4)
{
getJoystickSettings(joystick);
if(SensorValue(gyro) > offset) //turning right
{
motor[right] = motor[right] - 1;
}
if(SensorValue(gyro) < offset) //turning left
{
motor[left] = motor[left] - 1;
}
gyroed = true;
}
if(joystick.joy1_TopHat == 6)
{
turnDegrees(gyro, left, 70, 75);
}
if(joystick.joy1_TopHat == 2)
{
turnDegrees(gyro, right, 70, 75);
}
// Reset gyro stuff
if(gyroed)
{
motor[left] = 0;
motor[right] = 0;
gyroed = false;
}
if(joystick.joy1_y1 == lastJoyVal && joystick.joy1_y1 != 0)//detect loss of connection
{
cycles ++;
}
else
{
cycles = 0;
lastJoyVal = joystick.joy1_y1;
}
if(cycles == 32767)//shut off motors if we lose connection
{
motor[left] = 0;
motor[right] = 0;
motor[slide] = 0;
joystick.joy1_y1 = 0;
joystick.joy1_y2 = 0;
cycles = 0;
}
if (joy1Btn(3) || joy2Btn(3))
{
if (servo[gripper] == 255)
{
servo[gripper] = 0;
}
else
{
servo[gripper] = 255;
}
wait1Msec(100);
}
}//end while loop
}//end task main
