/**
* Start a competition.
* This code needs to track the order of the field starting to ensure that
* everything happens in the right order. Repeatedly run the correct
* method, either Autonomous or OperatorControl when the robot is
* enabled. After running the correct method, wait for some state to
* change, either the other mode starts or the robot is disabled. Then go
* back and wait for the robot to be enabled again.
*/
void SimpleRobot::StartCompetition()
{
RobotMain();
if (!m_robotMainOverridden)
{
while (1)
{
while (IsDisabled())
Wait(.01); // wait for robot to be enabled
if (IsAutonomous())
{
Autonomous(); // run the autonomous method
while (IsAutonomous() && !IsDisabled())
Wait(.01);
}
else
{
OperatorControl(); // run the operator control method
while (IsOperatorControl() && !IsDisabled())
Wait(.01);
}
}
}
}
/**
* Start a competition.
* This code needs to track the order of the field starting to ensure that everything happens
* in the right order. Repeatedly run the correct method, either Autonomous or OperatorControl
* when the robot is enabled. After running the correct method, wait for some state to change,
* either the other mode starts or the robot is disabled. Then go back and wait for the robot
* to be enabled again.
*/
void SimpleRobot::StartCompetition()
{
RobotMain();
if (!m_robotMainOverridden)
{
while (1)
{
if (IsDisabled())
{
Disabled();
while (IsDisabled()) Wait(.01);
}
else if (IsAutonomous())
{
Autonomous();
while (IsAutonomous() && IsEnabled()) Wait(.01);
}
else
{
OperatorControl();
while (IsOperatorControl() && IsEnabled()) Wait(.01);
}
}
}
}
/**
* Start a competition.
* This code needs to track the order of the field starting to ensure that everything happens
* in the right order. Repeatedly run the correct method, either Autonomous or OperatorControl
* when the robot is enabled. After running the correct method, wait for some state to change,
* either the other mode starts or the robot is disabled. Then go back and wait for the robot
* to be enabled again.
*/
void SimpleRobot::StartCompetition()
{
RobotMain();
if (!m_robotMainOverridden)
{
// first and one-time initialization
RobotInit();
while (true)
{
if (IsDisabled())
{
m_ds->InDisabled(true);
Disabled();
m_ds->InDisabled(false);
while (IsDisabled()) m_ds->WaitForData();
}
else if (IsAutonomous())
{
m_ds->InAutonomous(true);
Autonomous();
m_ds->InAutonomous(false);
while (IsAutonomous() && IsEnabled()) m_ds->WaitForData();
}
else
{
m_ds->InOperatorControl(true);
OperatorControl();
m_ds->InOperatorControl(false);
while (IsOperatorControl() && IsEnabled()) m_ds->WaitForData();
}
}
}
}
/**
* Start a competition.
* This code needs to track the order of the field starting to ensure that everything happens
* in the right order. Repeatedly run the correct method, either Autonomous or OperatorControl
* when the robot is enabled. After running the correct method, wait for some state to change,
* either the other mode starts or the robot is disabled. Then go back and wait for the robot
* to be enabled again.
*/
void SimpleRobot::StartCompetition()
{
RobotMain();
if ( !m_robotMainOverridden)
{
while (Simulator::ShouldContinue())
{
Simulator::NextStep(0.0);
if (IsDisabled())
continue;
if (IsAutonomous())
{
Autonomous(); // run the autonomous method
while (IsAutonomous() && !IsDisabled() && Simulator::ShouldContinue()) Wait(.01);
}
else
{
OperatorControl(); // run the operator control method
while (IsOperatorControl() && !IsDisabled() && Simulator::ShouldContinue()) Wait(.01);
}
}
}
}
/**
* Start a competition.
* This code needs to track the order of the field starting to ensure that everything happens
* in the right order. Repeatedly run the correct method, either Autonomous or OperatorControl
* or Test when the robot is enabled. After running the correct method, wait for some state to
* change, either the other mode starts or the robot is disabled. Then go back and wait for the
* robot to be enabled again.
*/
void SampleRobot::StartCompetition()
{
LiveWindow *lw = LiveWindow::GetInstance();
SmartDashboard::init();
NetworkTable::GetTable("LiveWindow")->GetSubTable("~STATUS~")->PutBoolean("LW Enabled", false);
RobotMain();
if (!m_robotMainOverridden)
{
// first and one-time initialization
lw->SetEnabled(false);
RobotInit();
while (true)
{
if (IsDisabled())
{
m_ds.InDisabled(true);
Disabled();
m_ds.InDisabled(false);
while (IsDisabled()) sleep(1); //m_ds.WaitForData();
}
else if (IsAutonomous())
{
m_ds.InAutonomous(true);
Autonomous();
m_ds.InAutonomous(false);
while (IsAutonomous() && IsEnabled()) sleep(1); //m_ds.WaitForData();
}
else if (IsTest())
{
lw->SetEnabled(true);
m_ds.InTest(true);
Test();
m_ds.InTest(false);
while (IsTest() && IsEnabled()) sleep(1); //m_ds.WaitForData();
lw->SetEnabled(false);
}
else
{
m_ds.InOperatorControl(true);
OperatorControl();
m_ds.InOperatorControl(false);
while (IsOperatorControl() && IsEnabled()) sleep(1); //m_ds.WaitForData();
}
}
}
}
//Grab first two and turn to go right
void AutonomousType10() {
SmartDashboard::PutString("STATUS:", "STARTING AUTO 10");
robotDrive.MecanumDrive_Cartesian(0, -0.2, 0);
if (WaitF(1.2))
return;
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(0.5);
while (IsAutonomous() && maxUp.Get() && midPoint.Get()) {
}
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Cartesian(0, 0.4, 0);
if (WaitF(1.6))
return;
robotDrive.MecanumDrive_Polar(0, 0, 0.3);
if (WaitF(2.6))
return;
robotDrive.MecanumDrive_Cartesian(0, -0.4, 0);
if (WaitF(1))
return;
robotDrive.MecanumDrive_Polar(0, 0, -0.3);
if (WaitF(2.6))
return;
robotDrive.MecanumDrive_Cartesian(0, -0.4, 0);
if (WaitF(1.6))
return;
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
SmartDashboard::PutString("STATUS:", "AUTO 10 COMPLETE");
}
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();
}
void AutonomousStateMachine() {
pneumaticsControl->initialize();
shooterControl->initializeAuto();
driveControl.initializeAuto();
enum AutoState {
AutoDrive, AutoSetup, AutoShoot
};
AutoState autoState;
autoState = AutoDrive;
bool feederState = false;
bool hasFired = false;
Timer feeder;
while (IsAutonomous() && IsEnabled()) {
GetWatchdog().Feed();
switch (autoState) {//Start of Case Machine
case AutoDrive://Drives the robot to set Destination
bool atDestination = driveControl.autoPIDDrive2();
if (atDestination) {//If at Destination, Transition to Shooting Setup
autoState = AutoSetup;
}
break;
case AutoSetup://Starts the ballgrabber motors to place the ball and extends ballgrabber
if (!pneumaticsControl->ballGrabberIsExtended()) {//extends ballgrabber if not already extended
pneumaticsControl->ballGrabberExtend();
}
if (!feederState) {//Started the feeder timer once
feederState = true;
feeder.Start();
autoState = AutoShoot;
}
break;
case AutoShoot://Shoots the ball
if (feeder.Get() < 2.0) {//Runs ballgrabber for 2.0 Seconds at most
shooterControl->feed(true);
} else if (feeder.Get() >= 2.0) {//Transition to Shooting
shooterControl->feed(false);
feeder.Stop();
}
if (pneumaticsControl->ballGrabberIsExtended() && !hasFired) {
shooterControl->autoShoot();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1,
"The robot is(should be) shooting");
if (shooterControl->doneAutoFire()) {//Returns true only after shoot is done firing
hasFired = true;
}
} else if (hasFired) {//runs only after shoot is done
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1,
"AutoMode Finished");
}
break;
}
dsLCD->UpdateLCD();
}
}
void output (void)
{
REDRUM;
if (IsAutonomous())
driverOut->PrintfLine(DriverStationLCD::kUser_Line1, "blaarag");
else if (IsOperatorControl())
{
REDRUM;
}
outputCounter++;
if (outputCounter % 30 == 0){
REDRUM;
driverOut->PrintfLine(DriverStationLCD::kUser_Line2, "Top Shooter RPM: %f",(float)LTop.GetSpeed());
driverOut->PrintfLine(DriverStationLCD::kUser_Line3, "Bot Shooter RPM: %f",(float)LBot.GetSpeed());
// driverOut->PrintfLine(DriverStationLCD::kUser_Line6, "Pilot Z-Axis: %f",pilot.GetZ());
}
if (outputCounter % 60 == 0){
REDRUM;
driverOut->PrintfLine(DriverStationLCD::kUser_Line4, "Top CANJag Temp: %f Celcius",LTop.GetTemperature()*(9/5) + 32);
driverOut->PrintfLine(DriverStationLCD::kUser_Line5, "Bot CANJag Temp: %f Celcius",LBot.GetTemperature()*(9/5) + 32);
outputCounter = 1;
}
driverOut->UpdateLCD();
}//nom nom nom
void Michael1::Autonomous(void)
{
printf("\n\n\tStart Autonomous:\n\n");
GetWatchdog().SetEnabled(false);
ariels_light->Set(1);
while (IsAutonomous())
{
Wait(0.1); //important
dt->SmoothTankDrive(left_stick, right_stick);
//dt->UpdateSlip();
//dt->UpdateSlip(); //calling slipControl(true) should spawn a task which does this.
//printf("Encoder: %f, ", dt->encoder_left->GetDistance());
//printf("Gyro: %f, ", dt->gyro->GetAngle());
//printf("Accel: %f", dt->accel->GetAcceleration());
//printf("\n\n");s
/*Wait(.1);
if(cam->FindTargets()){
ariels_light->Set(1);
} else {
ariels_light->Set(0);
}
*/
}
}
//Grab first yellow, back up to auto zone, drop
void AutonomousType11() {
SmartDashboard::PutString("STATUS:", "STARTING AUTO 11");
chainLift.SetSpeed(0.5);
while (IsAutonomous() && IsEnabled() && maxUp.Get() && midPoint.Get()) {
}
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Cartesian(0, 0.4, 0);
if (WaitF(3))
return;
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(-0.5);
while (IsAutonomous() && IsEnabled() && maxDown.Get()) {
}
chainLift.SetSpeed(0);
SmartDashboard::PutString("STATUS:", "AUTO 11 COMPLETE");
}
/* RunScript is blocking (pauses thread until script is complete)
* Takes a pointer to a Command in a Command array (Script).
* Iterates over said array until reaches "END" command.
*/
void Michael1::RunScript(Command* scpt){
bool finished = false;
while (IsAutonomous())
{
switch(scpt->cmd){
case TURN:
dt->Turn(scpt->param1,14.5 - time->Get());
break;
case JSTK:
dt->SetMotors(scpt->param1, scpt->param2);
Wait(14.5 - time->Get());
break;
case WAIT:
dt->SetMotors(0,0);
Wait(scpt->param1);
break;
case FWD:
dt->GoDistance(scpt->param1,14.5 - time->Get());
break;
default:
dt->SetMotors(0,0);
finished = true;
}
if (finished){
break;
}
scpt++;
}
}
void Autonomous()
{
while(IsAutonomous())
{
//do stuff
}
}
void DriveController :: Run()
{
if ( IsAutonomous() )
Autonomous();
else if ( IsOperatorControl() )
OperatorControl();
}
void Autonomous() {
if (DriverStation::GetInstance()->IsFMSAttached()) log->InMatch();
log->Info("AUTONOMOUS START");
while (IsAutonomous()) {
Wait(0.10);
}
}
void Autonomous(void)
{
GetWatchdog().SetEnabled(false);
if (recorder.StartPlayback())
{
while (IsAutonomous() && recorder.Playback());
}
}
void Autonomous ()
{
DriveTrain.StartDriveTrain();
Shooter.StartShooterAuto();
AutonomousState = 1;
while(IsAutonomous())
{
if (AutonomousSwitch.Get() == 0)
{
if (AutonomousState == 1)
{
DriveTrain.RunDriveTrain(1, 1, 0, 0);
if ((DriveTrain.LeftEncTotal >= 2400) || (DriveTrain.RightEncTotal >= 2400))
{
AutonomousState = 2;
}
}
else if ((AutonomousState == 2) && (HotGoal == true))
{
Shooter.RunShooter(0, 1, 0);
}
}
else if (AutonomousSwitch.Get() == 1)
{
if (AutonomousState == 1)
{
DriveTrain.RunDriveTrain(1, 1, 0, 0);
if ((DriveTrain.LeftEncTotal >= 2400) || (DriveTrain.RightEncTotal >= 2400))
{
AutonomousState = 2;
}
}
else if ((AutonomousState == 2) && (WhichHotGoal != 0))
{
DriveTrain.RunDriveTrain(WhichHotGoal, -WhichHotGoal, 0, 0);
if (((WhichHotGoal == 1) && ((DriveTrain.LeftEncTotal >= 2800) || (DriveTrain.RightEncTotal <= 2000))) || ((WhichHotGoal == -1) && ((DriveTrain.LeftEncTotal <= 2000) || (DriveTrain.RightEncTotal >= 2800))))
{
AutonomousState = 3;
}
}
else if (AutonomousState == 3)
{
Shooter.RunShooter(0, 1, 0);
AutonomousState = 4;
}
else if (AutonomousState == 4)
{
DriveTrain.RunDriveTrain(-WhichHotGoal, WhichHotGoal, 0, 0);
if (((WhichHotGoal == 1) && ((DriveTrain.LeftEncTotal <= 2400) || (DriveTrain.RightEncTotal >= 2400))) || ((WhichHotGoal == -1) && ((DriveTrain.LeftEncTotal >= 2400) || (DriveTrain.RightEncTotal <= 2400))))
{
AutonomousState = 5;
}
}
}
}
}
void Robot::Autonomous(void)
{
printf("Auto Mode!");
while(IsAutonomous() && IsEnabled())
{
Wait(.1);
printf("autonomous\r\n");
}
}
void Autonomous(){
Option *num = (Option *) chooser->GetSelected();
myRobot->ResetDisplacement();
Modes->SetMode(num->Get());
Modes->Run();
while(IsAutonomous() && IsEnabled()){
Wait(0.05);
Scheduler::GetInstance()->Run();
}
}
void Robot::AutonFeed() {
while (IsEnabled() && IsAutonomous()) {
DS_PrintOut();
// make ball conveyors run in reverse for all of Autonomous
lift.Set(Relay::kReverse);
Wait(0.1);
}
}
bool Breakaway10::AutonomousWait(float secs)
{
//printf("chaosAutonomous::autonomousWait() -> Entered");
if ( !IsAutonomous() || IsDisabled() )
{
return true;
}
if ( secs > 0 ) Wait(secs);
return false;
} //AutonomousWait
void DashBoardInput() {
int i = 0;
GetWatchdog().SetEnabled(true);
while (IsAutonomous() && IsEnabled()) {
i++;
GetWatchdog().Feed();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "%f, %i",
driverStation->GetAnalogIn(1), i);
dsLCD->UpdateLCD();
}
}
void Autonomous()
{
drive->mecanumGyro->Reset();
while(IsAutonomous() && IsEnabled()){
//auton->Run2Tote1CanAuto(drive, lifter);
//auton->Run1Tote1CanAuto(drive, lifter);
//auton->Run1CanPickup(drive, lifter);
//auton->RunDriveForward(drive);
//printf("Angle: %f\n", drive->mecanumGyro->GetAngle());
}
}
/**
* Drive left & right motors for 2 seconds, enabled by a jumper (jumper
* must be in for autonomous to operate).
*/
void Autonomous()
{
control->initializeAuto();
while (IsAutonomous())
{
control->runAuto();
dsLCD->UpdateLCD();
Wait(0.005);
}
}
bool WaitF(double time) {
double completed = 0;
while (completed < time) {
completed += 0.05;
Wait(0.05);
if (!IsAutonomous() || !IsEnabled())
return true;
}
return false;
}
void RobotBeta1::turnRad(double radians) {
float cGyroAngle = 0.0;
const float maxAngle = radians;
while((cGyroAngle <= maxAngle) && (IsAutonomous())) {
cout << "Here\t\t"; cout << "Exit: "; cout << (cGyroAngle <= maxAngle); cout << "\r";
robotDrive->Drive(-.25, -1);
Wait(0.006);
GetWatchdog().Feed();
cGyroAngle = gyro->GetAngle();
}
}
void WorkshopRobot::Autonomous(void){ //Called once at the begginning of auto mode.
bool Stopper = false; //Should not start stopped.
printf("2250 auto start\n"); //Check in with the console.
while (IsAutonomous()){ //Main operating loop.
/* if(Stopper || driveSys->RunDriveSystem()){ //If the drive has ever returned true, then the launcher code should be deployed.
printf("Deployed/n"); //Check with the console.
LauncherMech->AutoLauncher();
Stopper = true; //This will keep the condition true if it ever becomes true.
}*/
Wait(0.005); //Minimum motor controller wait time. (also handy for ticks)
}
}
void Robot::Autonomous()
{
Singleton<Logger>::GetInstance().Logf("Starting Autonomous Mode.");
Singleton<Collector>::GetInstance().SetBallCount( 2); // preloaded with 2 balls in autonomous
primaryDisplay.PrintfLine(0, "Shooting 2");
ShootBasket( 2 );
KinectStick leftStick(1);
KinectStick rightStick(2);
Timer displayUpdateFrequency;
displayUpdateFrequency.Start();
primaryDisplay.PrintfLine(0, "Kinect Controls");
while (IsAutonomous() )
{
secondaryDisplay.PrintfLine(1, "Shot-Dir: %.2f", shotDirectionModifier());
secondaryDisplay.PrintfLine(2, "Shot-Dist: %.1f\"", shotDistanceModifier());
DRIVETRAIN.SetLeft(-leftStick.GetY());
DRIVETRAIN.SetRight(rightStick.GetY());
if (leftStick.GetRawButton(1) ) // Left Leg Out
{
primaryDisplay.PrintfLine(0, "Ramp UP");
RampUp();
}
if (leftStick.GetRawButton(2) ) // Right Leg Out
{
primaryDisplay.PrintfLine(0, "Ramp DOWN");
RampDown();
}
if ( !leftStick.GetRawButton(1) && !leftStick.GetRawButton(2) ) // both legs in
RampOff();
// The Joystick Throttle controls the scrolling of the display
// The display is updated at a controlled pace
DisplayWrapper::GetInstance()->SetScrollLocation(joystick1->GetThrottle());
if (displayUpdateFrequency.HasPeriodPassed(1.0 / 5)) {
secondaryDisplay.PrintfLine(1, "Shot-Dir: %.2f", shotDirectionModifier());
secondaryDisplay.PrintfLine(2, "Shot-Dist: %.1f\'", shotDistanceModifier());
displayUpdateFrequency.Reset();
DisplayWrapper::GetInstance()->Output();
}
Wait(0.1);
}
Singleton<Logger>::GetInstance().Logf("Stopping Autonomous Mode.");
}
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);
}
}
void DriveThenShootAuto() {
//initizlizes all parts of robot
pneumaticsControl->initialize();
shooterControl->initializeAuto();
driveControl.initializeAuto();
bool destinationPrevious = false;
bool autoShot = false; //true if autoShoot
//creates a timer for the ball grabber motors
Timer feeding;
bool started = false;
while (IsAutonomous() && IsEnabled()) {
//GetWatchdog().Feed();
//drives forward to shooting point
bool atDestination = destinationPrevious || driveControl.autoPIDDrive2(); //autoDrive returns true when robot reached it's goal
if (atDestination) {
// The robot has reached the destination on the floor and is now ready to open and shoot
if (!started) {
started = true;
destinationPrevious = true;
//starts feeding-timer controls feeder motors so the ball doesn't get stuck
feeding.Start();
}
pneumaticsControl->ballGrabberExtend();
//waits for feeding to be done
if (feeding.Get() < 2.0) {//3.0 was
shooterControl->feed(true);
} else if (feeding.Get() >= 2.0) { // 3.0 was
shooterControl->feed(false);
feeding.Stop();
}
if (pneumaticsControl->ballGrabberIsExtended() && !autoShot) {
shooterControl->autoShoot();
//TODO dsLCD->PrintfLine(DriverStationLCD::kUser_Line1,
//"The robot is(should be) shooting");
if (shooterControl->doneAutoFire()) {//works only after shoot is done firing
autoShot = true;
}
} else if (autoShot) {//runs only after shoot is done
//tODO dsLCD->PrintfLine(DriverStationLCD::kUser_Line1,
//"AutoMode Finished");
}
}
//TODO dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "Feeder Time: %f",
//feeding.Get());
//TODO dsLCD->UpdateLCD();
}
}
