void Drive (float speed, int dist)
{
leftDriveEncoder.Reset();
leftDriveEncoder.Start();
int reading = 0;
dist = abs(dist);
// The encoder.Reset() method seems not to set Get() values back to zero,
// so we use a variable to capture the initial value.
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "initial=%d\n", leftDriveEncoder.Get());
dsLCD->UpdateLCD();
// Start moving the robot
robotDrive.Drive(speed, 0.0);
while ((IsAutonomous()) && (reading <= dist))
{
reading = abs(leftDriveEncoder.Get());
dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "reading=%d\n", reading);
dsLCD->UpdateLCD();
}
robotDrive.Drive(0.0, 0.0);
leftDriveEncoder.Stop();
}
//robot turns to desired position with a deadband of 2 degrees in each direction
bool GyroTurn (float desiredTurnAngle, float turnSpeed)
{
bool turning = true;
float myAngle = gyro->GetAngle();
//normalizes angle from gyro to [-180,180) with zero as straight ahead
while(myAngle >= 180)
{
GetWatchdog().Feed();
myAngle = myAngle - 360;
}
while(myAngle < -180)
{
GetWatchdog().Feed();
myAngle = myAngle + 360;
}
if(myAngle < (desiredTurnAngle - 2))// if robot is too far left, turn right a bit
{
myRobot->Drive(turnSpeed, -turnSpeed); //(right,left)
}
if(myAngle > (desiredTurnAngle + 2))// if robot is too far right, turn left a bit
{
myRobot->Drive(-turnSpeed, turnSpeed); //(right,left)
}
else
{
myRobot->Drive(0, 0);
turning = false;
}
return turning;
}
/**
* Drive left & right motors for 2 seconds then stop
*/
void Autonomous(void)
{
myRobot.SetSafetyEnabled(false);
myRobot.Drive(0.5, 0.0); // drive forwards half speed
Wait(2.0); // for 2 seconds
myRobot.Drive(0.0, 0.0); // stop robot
}
/**
* Drive left & right motors for 2 seconds then stop
*/
void Autonomous(void)
{
GetWatchdog().SetEnabled(false);
myRobot.Drive(0.5, 0.0); // drive forwards half speed
Wait(2.0); // for 2 seconds
myRobot.Drive(0.0, 0.0); // stop robot
}
/**
* Drive left & right motors for 2 seconds, enabled by a jumper (jumper
* must be in for autonomous to operate).
*/
void Autonomous(void) {
myRobot->SetSafetyEnabled(false);
if (ds->GetDigitalIn(ENABLE_AUTONOMOUS) == 1) // only run the autonomous program if jumper is in place
{
myRobot->Drive(0.5, 0.0); // drive forwards half speed
Wait(2.0); // for 2 seconds
myRobot->Drive(0.0, 0.0); // stop robot\
}
myRobot->SetSafetyEnabled(true);
}
void AutonomousPeriodic()
{
if(autoLoopCounter < 100) //Check if we've completed 100 loops (approximately 2 seconds)
{
myRobot.Drive(-0.5, 0.0); // drive forwards half speed
autoLoopCounter++;
} else {
myRobot.Drive(0.0, 0.0); // stop robot
}
}
void Autonomous(void)
{
GetWatchdog().SetEnabled(false);
AverageWindowFilter<double, 20> fx, fy;
double ax, ay, lastAx = 0, lastAy = 0;
int state = 0;
while (IsAutonomous())
{
GetAcceleration(ax, ay);
fx.AddPoint( ax - avgX );
fy.AddPoint( ay - avgY );
ax = fx.GetAverage();
ay = fy.GetAverage();
switch (state)
{
case 0:
myRobot.Drive(1.0, 0.0);
if (fabs(ay - lastAy) > 1.0)
++state;
break;
case 1:
myRobot.Drive(-.5, 0.0);
Wait(3);
++state;
break;
case 2:
myRobot.Drive(0.0, 0.0);
break;
}
lastAx = ax;
lastAy = ay;
Wait(0.05);
}
}
/**
* Do auto stuff
*/
void Autonomous()
{
cout << "Hello autonomous!" << endl;
myRobot.SetSafetyEnabled(false);
myRobot.Drive(-0.125, 0.0); // drive forwards half speed
Wait(5.0); // for 5 seconds
myRobot.Drive(0.0, 0.0); // stop robot
Wait(1.0); // Wait 1 sec
myRobot.Drive(-0.125, 0.25); // Drive fwd @ 1/2 speed, 0.25 curve (right?)
Wait(1.0);
myRobot.Drive(0, 0);
cout << "Bye autonomous!" << endl;
}
void AutonomousPeriodic()
{
if((Auto==1)||(Auto==2))
{
while(gyro.GetAngle()<ang)
{
motorspeed=(1/90)*abs(gyro.GetAngle()-ang);
myRobot.TankDrive(motorspeed/2,motorspeed/-2);
}
}
if((Auto==3)||(Auto==4))
{
while(gyro.GetAngle()>ang)
{
motorspeed=(1/90)*abs(gyro.GetAngle()-ang);
myRobot.TankDrive(motorspeed/-2,motorspeed/2);
}
}
while(EncDist.GetDistance()<EncVal1)
{
myRobot.Drive(-0.5, 0.0);
}
ang = -ang;
if((Auto==1)||(Auto==2))
{
while(gyro.GetAngle()<ang)
{
motorspeed=(1/90)*abs(gyro.GetAngle()-ang);
myRobot.TankDrive(motorspeed/2,motorspeed/-2);
}
}
if((Auto==3)||(Auto==4))
{
while(gyro.GetAngle()>ang)
{
motorspeed=(1/90)*abs(gyro.GetAngle()-ang);
myRobot.TankDrive(motorspeed/-2,motorspeed/2);
}
}
launcher.Set(1.0);
sonic.SetAutomaticMode(true);
while(sonic.GetRangeInches()>DefToShot)
{
myRobot.Drive(-0.5,0.0);
}
myRobot.Drive(0.0,0.0);
intake.Set(1.0);
}
void AutonomousContinuous(void) {
printf("Running in autonomous continuous...\n");
GetWatchdog().Feed();
if (kicker->HasBall())
{
//We have a ball, thus stop moving and kick the ball
drivetrain->Drive(0.0, 0.0);
kicker->SetKickerMode(KICKER_MODE_KICK);
} else {
//We do not have a ball
if (kicker->IsKickerInPosition())
{
//Move forward!
drivetrain->ArcadeDrive(autonomousForwardPower, 0.0);
} else {
//If not in position, wait for it to be there...
drivetrain->ArcadeDrive(0.0, 0.0);
kicker->SetKickerMode(KICKER_MODE_ARM);
}
}
//Run the kicker
kicker->Act();
}
void MoveShoulderTo(float shoulderDestinationVoltageToReach) {
if (VoltageReached(shoulderPotentiometerReading, shoulderDestinationVoltageToReach) == 1) {
//pot voltage is less than required, move shoulder up
shoulderMotor->Drive(kMaxShoulderMotorSpeed, 0);
}
else if (VoltageReached(shoulderPotentiometerReading, shoulderDestinationVoltageToReach) == 2) {
//pot voltage is greater than required, move shoulder down
shoulderMotor->Drive(-kMaxShoulderMotorSpeed, 0);
}
else {
//reached destination
shoulderMotor->Drive(0.0, 0.0);
shoulderDestinationVoltage = shoulderPotentiometerReading;
}
}
/******************************** CONTINUOUS ROUTINES ********************************/
void DisabledContinuous(void) {
printf("Running in disabled continuous...\n");
GetWatchdog().Feed();
//Stop the presses...
drivetrain->Drive(0.0, 0.0);
compressor->Stop();
}
void DisabledInit(void) {
printf("Robot disabled initializing...\n");
GetWatchdog().Feed();
//Stop the presses...
drivetrain->Drive(0.0, 0.0);
compressor->Stop();
printf("Robot disabled initialization complete.\n");
}
/**
* Tells the robot to drive to a set distance (in inches) from an object using
* proportional control.
*/
void OperatorControl() {
double currentDistance; //distance measured from the ultrasonic sensor values
double currentSpeed; //speed to set the drive train motors
while (IsOperatorControl() && IsEnabled()) {
currentDistance = ultrasonic->GetValue() * valueToInches; //sensor returns a value from 0-4095 that is scaled to inches
currentSpeed = (holdDistance - currentDistance) * pGain; //convert distance error to a motor speed
myRobot->Drive(currentSpeed, 0); //drive robot
}
}
void Autonomous()
{
compressor.Start();//start compressor
myRobot.SetSafetyEnabled(false);
myRobot.Drive(-0.5, 0.0); // drive forwards half speed
Wait(2.0); // for 2 seconds
myRobot.Drive(0.0, 0.0); // stop robot
while (!shoot.Get()){//while shooter isnt cocked pull it back
shooter.SetSpeed(1); //set motor
}
shooterSole.Set(shooterSole.kForward);//Sets Solenoid forward, shoot ball
shooterSole.Set(shooterSole.kReverse);//Sets Solenoid backward
myRobot.Drive(1.0, -1.0);//turn around for 0.5 seconds, we have to check to see if it takes that long
Wait(0.5);//wait for 0.5 seconds
myRobot.Drive(0.0, 0.0);//stop robot
}
// When the robot is on autonomous period, it will drive forwards at half speed for about two
// seconds, then stops
void Robot::AutonomousPeriodic() {
/*
if(autoLoopCounter < 100) { //Check if we've completed 100 loops (approximately 2 seconds)
myRobot.Drive(-0.5, 0.0); // drive forwards half speed
autoLoopCounter++;
} else {
myRobot.Drive(0.0, 0.0); // stop robot
}
*/
// Edits the gyro angle to account for drift
if (fabs(gyro.GetRate()) > GYRO_DRIFT_VALUE)
editedGyroAngle = gyro.GetAngle();
else {
gyro.Reset();
editedGyroAngle = 0;
}
// Adjust course based on gyro data
if (editedGyroAngle > 0)
autoTurn = 0.3;
else if (editedGyroAngle < 0)
autoTurn = -0.3;
else
autoTurn = 0;
// Go foward 3 feet at 1/3 speed, stop (adjusting for drift)
if (encoder1.GetRaw() < ONE_FOOT_LEFT_ENCODER*3 || encoder2.GetRaw() < ONE_FOOT_RIGHT_ENCODER*3)
myRobot.Drive(0.3, autoTurn);
else
myRobot.Drive(0.0, 0.0);
// Print the raw encoder data
SmartDashboard::PutNumber("Encoder L get raw", encoder1.GetRaw());
SmartDashboard::PutNumber("Encoder R get raw", encoder2.GetRaw());
}
/**
* This autonomous (along with the chooser code above) shows how to select between different autonomous modes
* using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
* Dashboard, remove all of the chooser code and uncomment the GetString line to get the auto name from the text box
* below the Gyro
*
* You can add additional auto modes by adding additional comparisons to the if-else structure below with additional strings.
* If using the SendableChooser make sure to add them to the chooser code above as well.
*/
void Autonomous()
{
std::string autoSelected = *((std::string*)chooser->GetSelected());
//std::string autoSelected = SmartDashboard::GetString("Auto Selector", autoNameDefault);
std::cout << "Auto selected: " << autoSelected << std::endl;
if(autoSelected == autoNameCustom){
//Custom Auto goes here
std::cout << "Running custom Autonomous" << std::endl;
myRobot.SetSafetyEnabled(false);
myRobot.Drive(-0.5, 1.0); // spin at half speed
Wait(2.0); // for 2 seconds
myRobot.Drive(0.0, 0.0); // stop robot
} else {
//Default Auto goes here
std::cout << "Running default Autonomous" << std::endl;
myRobot.SetSafetyEnabled(false);
myRobot.Drive(-0.5, 0.0); // drive forwards half speed
Wait(2.0); // for 2 seconds
myRobot.Drive(0.0, 0.0); // stop robot
}
}
bool GyroDrive(float desiredDriveAngle, float speed, int desiredDistance)
{
bool driving = true;
double encoderInchesTraveled = fabs(leftEnco->GetDistance());//absolute value distance
float myAngle = gyro->GetAngle();
//normalizes angle from gyro to [-180,180) with zero as straight ahead
while(myAngle >= 180)
{
GetWatchdog().Feed();
myAngle = myAngle - 360;
}
while(myAngle < -180)
{
GetWatchdog().Feed();
myAngle = myAngle + 360;
}
float my_speed = 0.0;
float turn = 0.0;
if(speed > 0)
//30.0 is the number you have to change to adjust properly
turn = -((myAngle + desiredDriveAngle) / 30.0); //proportionally adjust turn. As the robot gets more off 0, the greater the turn will be
else
turn = (myAngle + desiredDriveAngle) / 30.0; //proportionally adjust turn. As the robot gets more off 0, the greater the turn will be
if (encoderInchesTraveled < desiredDistance)
my_speed = speed;
else
{
my_speed = 0.0;
driving = false;
}
myRobot->Drive(my_speed, turn);
return driving;
}
void Autonomous(void)
{
char funcName[] = "Autonomous";
DPRINTF(LOG_DEBUG, "start VisionDemo autonomous");
//GetWatchdog().Feed();
// image data for tracking
ColorMode mode = IMAQ_HSL; // RGB or HSL
// TrackingThreshold td = GetTrackingData(RED, FLUORESCENT);
TrackingThreshold td = GetTrackingData(GREEN, FLUORESCENT);
int panIncrement = 0; // pan needs a 1-up number for each call
DPRINTF(LOG_DEBUG, "SERVO - looking for COLOR %s ", td.name);
/* initialize position and destination variables
* position settings range from -1 to 1
* setServoPositions is a wrapper that handles the conversion to
* range for servo
*/
horizontalDestination = 0.0; // final destination range -1.0 to +1.0
verticalDestination = 0.0;
// current position range -1.0 to +1.0
horizontalPosition = RangeToNormalized(horizontalServo->Get(), 1);
verticalPosition = RangeToNormalized(verticalServo->Get(), 1);
// incremental tasking toward dest (-1.0 to 1.0)
float incrementH, incrementV;
// set servos to start at center position
setServoPositions(horizontalDestination, verticalDestination);
/* for controlling loop execution time */
float loopTime = 0.05;
double currentTime = GetTime();
double lastTime = currentTime;
double savedImageTimestamp = 0.0;
bool foundColor = false;
bool staleImage = false;
while (IsAutonomous())
{
/* calculate gimbal position based on color found */
if (FindColor
(mode, &td.hue, &td.saturation, &td.luminance, &par,
&cReport))
{
foundColor = true;
panIncrement = 0; // reset pan
if (par.imageTimestamp == savedImageTimestamp)
{
// This image has been processed already,
// so don't do anything for this loop
staleImage = true;
}
else
{
staleImage = false;
savedImageTimestamp = par.imageTimestamp;
// compute final H & V destination
horizontalDestination = par.center_mass_x_normalized;
verticalDestination = par.center_mass_y_normalized;
}
// ShowActivity("Found color ");
}
else
{ // need to pan
foundColor = false;
// ShowActivity("No color found");
}
PrintReport(&cReport);
if (foundColor && !staleImage)
{
/* Move the servo a bit each loop toward the destination.
* Alternative ways to task servos are to move immediately
* vs. incrementally toward the final
* destination. Incremental method reduces the need for
* calibration of the servo movement while moving toward
* the target.
*/
incrementH = horizontalDestination - horizontalPosition;
incrementV = verticalPosition - verticalDestination;
adjustServoPositions(incrementH, -incrementV);
ShowActivity
("** %s found: Servo: x: %f y: %f increment: %f y: %f ",
td.name, horizontalDestination, verticalDestination,
incrementH, incrementV);
}
else if (!staleImage)
{
/* pan to find color after a short wait to settle servos
* panning must start directly after panInit or timing
* will be off
*/
// adjust sine wave for panning based on last movement
// direction
if (horizontalDestination > 0.0)
{
sinStart = PI / 2.0;
}
else
{
sinStart = -PI / 2.0;
}
if (panIncrement == 3)
{
panInit();
}
else if (panIncrement > 3)
{
panForTarget(horizontalServo, sinStart);
/* Vertical action: center the vertical after several
* loops searching */
if (panIncrement == 20)
{
verticalServo->Set(0.5);
}
}
panIncrement++;
} // end if found color
dashboardData.UpdateAndSend();
// sleep to keep loop at constant rate
// elapsed time can vary significantly due to debug printout
currentTime = GetTime();
lastTime = currentTime;
if (loopTime > ElapsedTime(lastTime))
{
Wait(loopTime - ElapsedTime(lastTime));
}
} // end while
myRobot->Drive(0.0, 0.0); // stop robot
DPRINTF(LOG_DEBUG, "end autonomous");
ShowActivity
("Autonomous end ");
} // end autonomous
/******************************************************
* Drive left & right motors for 2 seconds then stop *
******************************************************/
void Autonomous(void)
{
//increase 2nd RPM
compressor.Start(); // starts the compressor;
bool BRIDGE = bridge.Get();
bool HIGH = high.Get();
bool MIDDLE = middle.Get();
bool TWOSEC = twosec.Get();
bool FIVESEC = fivesec.Get();
int highRPM = 1800; // 1st 1800 short about 5 ft
int secondhighRPM = 1800; //1st 1400 (did not fire)
int rpmForShooter;
DriverStationLCD *dslcd = DriverStationLCD::GetInstance(); // don't press SHIFT 5 times; this line starts up driver station messages (in theory
char debugout [100];
// baddog.SetExpiration(30.0);
// baddog.Feed();
dslcd->Clear();
sprintf(debugout,"Bridge=%u",BRIDGE);
dslcd->Printf(DriverStationLCD::kUser_Line1,1,debugout);
sprintf(debugout,"High=%u",HIGH);
dslcd->Printf(DriverStationLCD::kUser_Line2,2,debugout);
sprintf(debugout,"Middle=%u",MIDDLE);
dslcd->Printf(DriverStationLCD::kUser_Line3,3,debugout);
sprintf(debugout,"TwoSec=%u",TWOSEC);
dslcd->Printf(DriverStationLCD::kUser_Line4,4,debugout);
sprintf(debugout,"FiveSec=%u",FIVESEC);
dslcd->Printf(DriverStationLCD::kUser_Line5,5,debugout);
dslcd->UpdateLCD(); // update the Driver Station with the information in the code */
if (BRIDGE == 0 && HIGH == 0 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 0)
{
myRobot.Drive(0.0, 0.0);
Wait(10.0);
}
if (BRIDGE == 1 && HIGH == 0 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 0) //1700 RPM
{
/*myRobot.Drive(-0.5, 0.0);
Wait(3.0);
*/
flashring.Set(Relay::kForward);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<7)
{
shooter.setTargetRPM(1700);
//wait-0.01
Wait(0.005);
}
lynx.Set(-1.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<1)
{
shooter.setTargetRPM(1700);
Wait(0.005);
}
turret.Set(-0.05);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<0.2)
{
shooter.setTargetRPM(1700);
Wait(0.005);
}
turret.Set(0.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1700);
Wait(0.005);
}
// lynx.Set(0.0);
shepard.Set(true);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1700);
Wait(0.005);
}
shepard.Set(false);
shooter.setTargetRPM((int)0);
flashring.Set(Relay::kOff);
lynx.Set(0.0);
}
if (BRIDGE == 1 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 0) //main autonomous code-default
{
flashring.Set(Relay::kForward);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<7)
{
shooter.setTargetRPM((int)highRPM);
//wait-0.01
Wait(0.005);
}
lynx.Set(-1.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<1)
{
shooter.setTargetRPM((int)highRPM);
Wait(0.005);
}
turret.Set(-0.05);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<0.2)
{
shooter.setTargetRPM((int)secondhighRPM);
Wait(0.005);
}
turret.Set(0.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM((int)secondhighRPM);
Wait(0.005);
}
// lynx.Set(0.0);
shepard.Set(true);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM((int)secondhighRPM);
Wait(0.005);
}
shepard.Set(false);
shooter.setTargetRPM((int)0);
flashring.Set(Relay::kOff);
lynx.Set(0.0);
// baddog.Feed();
}
if (BRIDGE == 1 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 1 && FIVESEC == 0)
{
//Wait(2.0);
//Robot aims
//Robot shoots
//myRobot.Drive(-0.5, 0.0);
//Wait(3.0);
flashring.Set(Relay::kForward);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<7)
{
shooter.setTargetRPM(1900);
//wait-0.01
Wait(0.005);
}
lynx.Set(-1.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<1)
{
shooter.setTargetRPM(1900);
Wait(0.005);
}
turret.Set(-0.05);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<0.2)
{
shooter.setTargetRPM(1800);
Wait(0.005);
}
turret.Set(0.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1800);
Wait(0.005);
}
// lynx.Set(0.0);
shepard.Set(true);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1800);
Wait(0.005);
}
shepard.Set(false);
shooter.setTargetRPM((int)0);
flashring.Set(Relay::kOff);
lynx.Set(0.0);
}
if (BRIDGE == 1 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 1)
{
Wait(5.0);
//Robot aims
//Robot shoots
//myRobot.Drive(-0.5, 0.0);
myRobot.Drive(0.0,0.0);
Wait(3.0);
}
if (BRIDGE == 1 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 0 && FIVESEC == 0)
{
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 1 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 1 && FIVESEC == 0)
{
Wait(2.0);
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 1 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 0 && FIVESEC == 5)
{
Wait(5.0);
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 0 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 0) //position robot-front of key-low RPMs
{
//Robot aims
//Robot shoots
//myRobot.Drive(-0.5, 0.0);
//Wait(3.0);
flashring.Set(Relay::kForward);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<7)
{
shooter.setTargetRPM(1600);
//wait-0.01
Wait(0.005);
}
lynx.Set(-1.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<1)
{
shooter.setTargetRPM(1600);
Wait(0.005);
}
turret.Set(-0.05);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<0.2)
{
shooter.setTargetRPM(1600);
Wait(0.005);
}
turret.Set(0.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1600);
Wait(0.005);
}
// lynx.Set(0.0);
shepard.Set(true);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1600);
Wait(0.005);
}
shepard.Set(false);
shooter.setTargetRPM((int)0);
flashring.Set(Relay::kOff);
lynx.Set(0.0);
}
if (BRIDGE == 0 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 1 && FIVESEC == 0)
{
Wait(2.0);
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 0 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 1)
{
Wait(5.0);
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 0 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 0 && FIVESEC == 0)
{
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 0 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 1 && FIVESEC == 0)
{
Wait(2.0);
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 0 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 0 && FIVESEC == 5)
{
Wait(5.0);
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 0 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 1 && FIVESEC == 1)
{
flashring.Set(Relay::kForward);
ShooterTimer.Reset();
ShooterTimer.Start();
targeting.SetLMHTarget(BOGEY_H);
while(ShooterTimer.Get()<2)
{
if (targeting.ProcessOneImage())
{
targeting.ChooseBogey();
targeting.MoveTurret();
rpmForShooter = targeting.GetCalculatedRPM();
shooter.setTargetRPM(rpmForShooter);
Wait(0.01);
}
}
targeting.StopPID();
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get() < 7)
{
shooter.setTargetRPM(rpmForShooter);
Wait(0.005);
}
lynx.Set(-1.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<1)
{
shooter.setTargetRPM(rpmForShooter);
Wait(0.005);
}
turret.Set(-0.05);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<0.2)
{
shooter.setTargetRPM(rpmForShooter);
Wait(0.005);
}
turret.Set(0.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(rpmForShooter);
Wait(0.005);
}
// lynx.Set(0.0);
shepard.Set(true);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(rpmForShooter);
Wait(0.005);
}
shepard.Set(false);
shooter.setTargetRPM((int)0);
flashring.Set(Relay::kOff);
lynx.Set(0.0);
}
if (BRIDGE == 1 && HIGH == 1 && MIDDLE == 1 && TWOSEC == 0 && FIVESEC == 0)
{
flashring.Set(Relay::kForward);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<7)
{
shooter.setTargetRPM(2000); //high RPM
//wait-0.01
Wait(0.005);
}
lynx.Set(-1.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<1)
{
shooter.setTargetRPM(2000); //high RPM
Wait(0.005);
}
turret.Set(-0.05);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<0.2)
{
shooter.setTargetRPM(1800); //low RPM
Wait(0.005);
}
turret.Set(0.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1800); //low RPM
Wait(0.005);
}
// lynx.Set(0.0);
shepard.Set(true);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1800); //low RPM
Wait(0.005);
}
shepard.Set(false);
shooter.setTargetRPM((int)0);
flashring.Set(Relay::kOff);
lynx.Set(0.0);
}
//baddog.Feed();
myRobot.SetSafetyEnabled(false);
}
/*
* OPERATOR CONTROL using a arcade style drive
*/
void OperatorControl(void) {
GetWatchdog().SetEnabled(true);
while (IsOperatorControl()) //This code will loop continuously as long it is operator control mode
{
GetWatchdog().Feed(); // Feed the watchdog
shoulderPotentiometerReading = (5-(shoulderPotentiometerChannel->GetVoltage())); // reads the potentiometer at channel 1
/* COMMENT GRIPPER CODE BELOW
if (shoulderPotentiometerReading == 0) {
gripperMotor->SetAngle(0); //opens gripper
}
else if (shoulderPotentiometerReading > 3) {
gripperMotor->SetAngle(55); //closes gripper
}
*/
if (leftstick->GetRawButton(4) == true) {
shoulderMotor->Drive(-kMaxShoulderMotorSpeed, 0); //moves shoulderMotor down
shoulderDestinationVoltage = shoulderPotentiometerReading;
} else if (leftstick->GetRawButton(5) == true) {
shoulderMotor->Drive(kMaxShoulderMotorSpeed, 0); //moves shoulderMotor up
shoulderDestinationVoltage = shoulderPotentiometerReading;
} else {
//LEFTSTICK PRESETS
if (leftstick->GetRawButton(2) == true) {
//shoulderDestinationVoltage = kPickUpPosition;
} else if (leftstick->GetRawButton(6) == true) {
shoulderDestinationVoltage = kTopRow;
} else if (leftstick->GetRawButton(7) == true) {
shoulderDestinationVoltage = kMiddleRow;
} else if (leftstick->GetRawButton(8) == true) {
shoulderDestinationVoltage = kBottomRow;
} else if (leftstick->GetRawButton(9) == true) {
shoulderDestinationVoltage = kBottomRowSecondColumn;
} else if (leftstick->GetRawButton(10) == true) {
shoulderDestinationVoltage = kMiddleRowSecondColumn;
} else if (leftstick->GetRawButton(11) == true) {
shoulderDestinationVoltage = kTopRowSecondColumn;
}
MoveShoulderTo(shoulderDestinationVoltage);
}
//----------------------------------------------------------------
//GRIPPER OPEN AND CLOSE
if (leftstick->GetRawButton(1) == true) {
gripperMotor->SetAngle(0); //opens gripper
} else {
gripperMotor->SetAngle(55); //closes gripper
}
//MINIBOT DEPLOYMENT
if (leftstick->GetRawButton(3) == true) {
minibotDeployMotor->SetAngle(60); //releases four-bar
myRobot->Drive(0.0, 0); // stop robot since controls are going to be inverted momentarily - don't want to go from full forward to full reverse instantly
}
//RIGHTSTICK PRESETS
if (rightstick->GetRawButton(1) == true) {
//back of the robot is moved forward by pushing forward on the joystick
myRobot->ArcadeDrive((rightstick->GetY()),
(rightstick->GetX()), false); // inverted drive control
} else {
//front of the robot is moved forward by pushing forward on the joystick
myRobot->ArcadeDrive(-(rightstick->GetY()),
(rightstick->GetX()), false); // normal drive control
}
//MINIBOT CLOSING/CLIMBING POLE
if (rightstick->GetRawButton(3) == true) {
//minibot closes only after its four-bar is dropped
minibotCloseMotor1->SetAngle(45); //closes minibot so it starts climbing pole
minibotCloseMotor2->SetAngle(45); //closes minibot so it starts climbing pole
} else if (rightstick->GetRawButton(4) == true) {
//MANUAL ARM CONTROL
armMotor->Drive(-kMaxArmMotorSpeed/2.0, 0); //turn armMotor counter clockwise
} else if (rightstick->GetRawButton(5) == true) {
//MANUAL ARM CONTROL
armMotor->Drive(kMaxArmMotorSpeed/1.5, 0); //turn armMotor cw
}
}
}
/**
* Autonomous code
*/
void Autonomous(void) {
//disable watchdog and start timer
GetWatchdog().SetEnabled(false);
gameTimer->Start();
gameTimer->Reset();
float speed = 0.15; //CHECK-> enough speed to get to the peg in time as shoulder rises slowly
/*
//variables used to hold light sensors' values
rightSensor = 0;
leftSensor = 0;
middleSensor = 0;
*/
while (IsAutonomous())
{
time_to_send++;
if(time_to_send >50 )
{
time_to_send=0;
//Print a message to the Driver Station LCD
dsLCD->Printf(DriverStationLCD::kUser_Line,1, "Autonomous is running, RUN FOR COVER"); //Give output to dsLCD
dsLCD->UpdateLCD();
}
for(int x=0; x<4; x++)
{
if (x==1)
{
myRobot->Drive(0.0,0.0); //Stop the robot initially
}
else if (x==2)
{
myRobot->Drive(speed,0.0); //Move the robot
}
else if (x==3)
{
myRobot->Drive(0.5,0.); //Decrease the speed
}
else
{
myRobot->Drive(0.0,0.0); //If anything else happens, STOP the robot
}
}
break;
}
/*
float releaseVoltage;
bool reachedEndOfLine = false;
int followingLineNumber = 1; //2 is for the 'y' line; reading line number from left to right
//keep following line until robot reaches the 'T' - move shoulder to appropriate position simultaneously
while (reachedEndOfLine == false) {
//read the various sensors
//encoderReading = wheelEncoder->GetDistance();
shoulderPotentiometerReading = (5
-(shoulderPotentiometerChannel->GetVoltage())); // reads the potentiometer at channel 1
// LINE FOLLOWING CODE
leftSensor = left->Get() ? 1 : 0;
middleSensor = middle->Get() ? 1 : 0;
rightSensor = right->Get() ? 1 : 0;
if (leftSensor == 0 && middleSensor == 1 && rightSensor == 1) {
myRobot->Drive(speed, -0.5); // right and middle sensors are on line
} else if (leftSensor == 1 && middleSensor == 1 && rightSensor == 0) {
myRobot->Drive(speed, 0.5); // left and middle sensors are on line
} else if ((leftSensor == 1 && middleSensor == 1 && rightSensor
== 1) || (leftSensor == 1 && middleSensor == 0
&& rightSensor == 1)) {
//CHECK - add support for 'y' line, if necessary
reachedEndOfLine = true;
} else if (leftSensor == 0 && middleSensor == 0 && rightSensor == 0) {
//robot is off the line, stop
myRobot->Drive(speed, 0.0);
} else if (leftSensor == 0 && middleSensor == 1 && rightSensor == 0) {
myRobot->Drive(speed, 0.0); //only middle sensor is on line, go forward
}
//raise the shoulder while following line, depending on which line robot is following
if (followingLineNumber == 1 || followingLineNumber == 3) {
//scoring on the highest, second column, peg
MoveShoulderTo(kTopRowSecondColumn);
releaseVoltage = kTopRowSecondColumn-0.1;
} else {
//scoring on the top row, first or third column, peg
MoveShoulderTo(kTopRow);
releaseVoltage = kTopRow-0.1;
}
}
//drop shoulder a bit, open gripper, and stop driving
while (VoltageReached(shoulderPotentiometerReading, releaseVoltage)
!= 0) {
//at the end of the line, stop
myRobot->Drive(0.0, 0); // stop robot
gripperMotor->SetAngle(55); //opens gripper
MoveShoulderTo(releaseVoltage);
}
//reverse robot and lower shoulder
while (gameTimer->Get() < 14) {
myRobot->Drive(-speed, 0); // reverse robot
MoveShoulderTo(kPickUpPosition);
}
*/
}
/****************************************
* Runs the motors with arcade steering.*
****************************************/
void OperatorControl(void)
{
//TODO put in servo for lower camera--look in WPI to set
// Watchdog baddog;
// baddog.Feed();
myRobot.SetSafetyEnabled(true);
//SL Earth.Start(); // turns on Earth
// SmartDashboard *smarty = SmartDashboard::GetInstance();
//DriverStationLCD *dslcd = DriverStationLCD::GetInstance(); // don't press SHIFT 5 times; this line starts up driver station messages (in theory)
//char debugout [100];
compressor.Start();
gyro.Reset(); // resets gyro angle
int rpmForShooter;
while (IsOperatorControl()) // while is the while loop for stuff; this while loop is for "while it is in Teleop"
{
// baddog.Feed();
//myRobot.SetSafetyEnabled(true);
//myRobot.SetExpiration(0.1);
float leftYaxis = driver.GetY();
float rightYaxis = driver.GetTwist();//RawAxis(5);
myRobot.TankDrive(leftYaxis,rightYaxis); // drive with arcade style (use right stick)for joystick 1
float random = gamecomponent.GetY();
float lazysusan = gamecomponent.GetZ();
//bool elevator = Frodo.Get();
float angle = gyro.GetAngle();
bool balance = Smeagol.Get();
SmartDashboard::PutNumber("Gyro Value",angle);
int NumFail = -1;
//bool light = Pippin.Get();
//SL float speed = Earth.GetRate();
//float number = shooter.Get();
//bool highspeed = button1.Get()
//bool mediumspeed = button2.Get();
//bool slowspeed = button3.Get();
bool finder = autotarget.Get();
//bool targetandspin = autodistanceandspin.Get();
SmartDashboard::PutString("Targeting Activation","");
//dslcd->Clear();
//sprintf(debugout,"Number=%f",angle);
//dslcd->Printf(DriverStationLCD::kUser_Line2,2,debugout);
//SL sprintf(debugout,"Number=%f",speed);
//SL dslcd->Printf(DriverStationLCD::kUser_Line4,4,debugout);
//sprintf(debugout,"Number=%f",number);
//dslcd->Printf(DriverStationLCD::kUser_Line1,1,debugout);
//sprintf(debugout,"Finder=%u",finder);
//dslcd->Printf(DriverStationLCD::kUser_Line5,5,debugout);
//dslcd->UpdateLCD(); // update the Driver Station with the information in the code
// sprintf(debugout,"Number=%u",maxi);
// dslcd->Printf(DriverstationLCD::kUser_Line6,5,debugout)
bool basketballpusher = julesverne.Get();
bool bridgetipper = joystickbutton.Get();
if (bridgetipper) // if joystick button 7 is pressed (is true)
{
solenoid.Set(true); // then the first solenoid is on
}
else
{
//Wait(0.5); // and then the first solenoid waits for 0.5 seconds
solenoid.Set(false); //and then the first solenoid turns off
}
if (basketballpusher) // if joystick button 6 is pressed (is true)
{
shepard.Set(true); // then shepard is on the run
//Wait(0.5); // and shepard waits for 0.5 seconds
}
else
{
shepard.Set(false); // and then shepard turns off
} //10.19.67.9 IP address of computer;255.0.0.0 subnet mask ALL FOR WIRELESS CONNECTION #2
//}
//cheetah.Set(0.3*lazysusan);
// smarty->PutDouble("pre-elevator",lynx.Get());
lynx.Set(random);
// smarty->PutDouble("elevator",lynx.Get());
// smarty->PutDouble("joystick elevator",random);
if (balance) // this is the start of the balancing code
{
angle = gyro.GetAngle();
myRobot.Drive(-0.03*angle, 0.0);
Wait(0.005);
}
/*if (light) //button 5 turns light on oand off on game controller
flashring.Set(Relay::kForward);
else
flashring.Set(Relay::kOff);
*/
if (finder)
{
flashring.Set(Relay::kForward);
if (button_H.Get()==true)
{
targeting.SetLMHTarget(BOGEY_H);
SmartDashboard::PutString("Targeting","High Button Pressed");
}
if (button_M.Get()==true)
{
targeting.SetLMHTarget(BOGEY_M);
SmartDashboard::PutString("Targeting","Medium Button Pressed");
}
if (button_L.Get()==true)
{
targeting.SetLMHTarget(BOGEY_L);
SmartDashboard::PutString("Targeting","Low Button Pressed");
}
if (button_H.Get()==true || button_M.Get()==true || button_L.Get()==true)
{
if (targeting.ProcessOneImage())
{
NumFail = 0;
SmartDashboard::PutString("Targeting Activation","YES");
targeting.ChooseBogey();
targeting.MoveTurret();
#ifdef USE_HARDWIRED_RPM
shooter.setTargetRPM(HARDWIRED_RPM);
#else
rpmForShooter = targeting.GetCalculatedRPM();
shooter.setTargetRPM(rpmForShooter);
#endif
targeting.InteractivePIDSetup();
}
else
{
NumFail++;
if (NumFail > 10)
targeting.StopPID();
}
SmartDashboard::PutNumber("Numfail", NumFail);
shooter.setTargetRPM(rpmForShooter);
}
else
{
SmartDashboard::PutString("Targeting Activation","NO");
shooter.setTargetRPM(0);
targeting.StopPID();
}
}
else
{
flashring.Set(Relay::kOff);
targeting.StopPID();
turret.Set(lazysusan); // the lazy susan would turn right & left based on how far the person moves the right joystick#2 side to side
//targeting.StopPID();
//if (elevator) //shooter would move at full speed if button is pressed
//TODO Change RPM values
//TODO Disable calculation of RPM values
SmartDashboard::PutNumber("CurrentRPM",shooter.GetCurrentRPM());
if (button_H.Get() == true)
shooter.setTargetRPM((int)2100);
//From front of free throw line, should hit the backboard and go in
//used to be 2700 RPMs
else if (button_M.Get() == true)
shooter.setTargetRPM((int)1900);
//From front of free throw line, should go in the net--can shoot the next ball on the overshoot?
//Used to be 2250 RPMs
else if (button_L.Get() == true)
shooter.setTargetRPM((int)1350);
//From fender, should hit the backboard
//Used to be 2000 RPMs
//shooter.Set(0.5);
else
shooter.setTargetRPM(0);
// else if (mediumspeed)
//shooter.setTargetRPM((int)0);
//else if (slowspeed)
//shooter.setTargetRPM((int)0);
/*if (targetandspin) //code for autotargeting and speed will go here
{
shooter.setTargetRPM((int)1800);
}
else
{*/
//}
myRobot.TankDrive(leftYaxis,rightYaxis);
}
//Wait(0.005);
}
}
void AutonomousPeriodic()
{
drive->Drive(0.25,0.0);
}