void AutonomousInit(){
SmartDashboard::PutString("AutonomousInit", "Autonomous Init Reached");
autonomous->setSelected(*((std::string*)chooser->GetSelected()));
//autoSelected = SmartDashboard::GetString("Auto Selector", *((std::string*)chooser->GetSelected()));
autoSelected = *((std::string*)chooser->GetSelected());
SmartDashboard::PutString("Auto Selected", autoSelected);
//testSelected= SmartDashboard::GetString("Auto Selector", *((std::string*)chooser2->GetSelected()));
testSelected = *((std::string*)chooser2->GetSelected());
Drive->AutonomousInit();
autonomous->startTimer();
autonomous->reset();
}
/**
* 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 AutonomousInit()
{
autoSelected = *((std::string*)chooser->GetSelected());
//std::string autoSelected = SmartDashboard::GetString("Auto Selector", autoNameDefault);
std::cout << "Auto selected: " << autoSelected << std::endl;
rotation = 0.0;
//*((double*)posChooser->GetSelected());
//goal = *((std::string*)goalChooser->GetSelected());
shoot = "No";
//*((std::string*)shootChooser->GetSelected());
defenseCrossed = false;
done = false;
std::cout << "Here" << std::endl;
drive->SetMaxOutput(1.0);
std::cout << "there" << std::endl;
//Make sure to reset the encoder!
leftEnc->Reset();
rightEnc->Reset();
gyro->Reset();
autoCounter = 0;
timer->Reset();
}
virtual void TeleopInit() {
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
if (_autonomousCommand)
{
_autonomousCommand->Cancel();
}
clearLCD();
_teleopCommand = (Command*)_teleopChooser->GetSelected();
// _teleopCommand = (Command*)_noOpCmd;
printCommandToLCD(_teleopCommand->GetName());
// if (! CommandBase::azimuthSubsystem->IsCalibrated())
// {
// CommandBase::azimuthSubsystem->CalibrateAzimuth();
// }
_teleopCommand->Start();
// _testRobotCmd->Start();
// _cmd = new TestBridgeArmCmd(CommandBase::oi->driveTrigger);
// _cmd->Start();
}
virtual void TeleopInit() {
CommandBase::turret->Reset();
CommandBase::turret->Start();
driveCommand = (Command*) driveStyle->GetSelected();
driveCommand->Start();
}
void Autonomous(){
Option *num = (Option *) chooser->GetSelected();
myRobot->ResetDisplacement();
Modes->SetMode(num->Get());
Modes->Run();
while(IsAutonomous() && IsEnabled()){
Wait(0.05);
Scheduler::GetInstance()->Run();
}
}
// Start auto mode
void AutonomousInit() override {
autoSelected = *((int*) chooser.GetSelected()); // autonomous mode chosen in dashboard
currentState = 1;
ahrs->ZeroYaw();
ahrs->GetFusedHeading();
autoLength = SmartDashboard::GetNumber(AUTO_LENGTH, AUTO_LENGTH_DEFAULT);
autoSpeed = SmartDashboard::GetNumber(AUTO_SPEED, AUTO_SPEED_DEFAULT);
autoIntakeSpeed = SmartDashboard::GetNumber(AUTO_INTAKE_SPEED, AUTO_INTAKE_SPEED_DEFAULT);
liftdown->Set(false);
}
void TeleopPeriodic()
{
double throttle;
bool BuddyBoxEnabled = BuddyBoxEnableChooser->GetSelected();
bool SlaveInControl = MasterInterLink->GetCh5();
SmartDashboard::PutBoolean( "Slave In Control", BuddyBoxEnabled ? SlaveInControl : false );
bool SlaveControlsSpeed = SlaveSpeedControlChooser->GetSelected();
if( BuddyBoxEnabled && SlaveInControl ) ActiveInterLink = SlaveInterLink;
else ActiveInterLink = MasterInterLink;
if( SlaveInControl && SlaveControlsSpeed ) throttle = SlaveInterLink->getCh6();
else throttle = MasterInterLink->getCh6();
double aile = ActiveInterLink->getAile();
double elev = ActiveInterLink->getElev();
double rudd = ActiveInterLink->getRudd();
SmartDashboard::PutNumber( "Rudder", rudd );
SmartDashboard::PutNumber( "Throttle", throttle );
throScale = throttle + 1;
double driveAngle = atan2( -aile*aileScale, elev*elevScale );
SmartDashboard::PutNumber( "Drive Angle", driveAngle );
double driveSpeed = sqrt( aile*aile + elev*elev );
SmartDashboard::PutNumber( "Drive Speed", driveSpeed );
frontLeft->Set( (float)(
throScale * frontLeftSpeed * ( driveSpeed * sin( frontLeftAngle-driveAngle ) + ruddScale * rudd )
) );
backLeft->Set( (float)(
throScale * backLeftSpeed * ( driveSpeed * sin( backLeftAngle-driveAngle ) + ruddScale * rudd )
) );
frontRight->Set( (float)(
throScale * frontRightSpeed * ( driveSpeed * sin( frontRightAngle-driveAngle ) + ruddScale * rudd )
) );
backRight->Set( (float)(
throScale * backRightSpeed * ( driveSpeed * sin( backRightAngle-driveAngle ) + ruddScale * rudd )
) );
}
/**
* 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 AutonomousInit()
{
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
} else {
//Default Auto goes here
}
}
/**
* 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 code to get the auto name from the text box
* below the Gyro
*
* You can add additional auto modes by adding additional commands to the chooser code above (like the commented example)
* or additional comparisons to the if-else structure below with additional strings & commands.
*/
void AutonomousInit()
{
/* std::string autoSelected = SmartDashboard::GetString("Auto Selector", "Default");
if(autoSelected == "My Auto") {
autonomousCommand.reset(new MyAutoCommand());
} else {
autonomousCommand.reset(new ExampleCommand());
} */
autonomousCommand = (Command *)chooser->GetSelected();
if (autonomousCommand != NULL)
autonomousCommand->Start();
}
virtual void AutonomousInit()
{
if (_teleopCommand)
{
_teleopCommand->Cancel();
}
clearLCD();
_autonomousCommand = (Command*)_autoChooser->GetSelected();
// _autonomousCommand = (Command*) _autoFireCmd;
printCommandToLCD(_autonomousCommand->GetName());
_autonomousCommand->Start();
}
/**
* 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
*s
* 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 AutonomousInit() {
autoSelected = *((std::string*) chooser->GetSelected());
std::string autoSelected = SmartDashboard::GetString("Auto Selector",
autoNameDefault);
std::cout << "Auto selected: " << autoSelected << std::endl;
//myRobot.SetSafetyEnabled(false);
//myRobot.Drive(-0.5, 0.0);
if (autoSelected == autoNameCustom) {
//Custom Auto goes here
//autoSpeed = 1;
//Wait(2);
//autoSpeed = 0;
} else {
//Default Auto goes here
}
}
/**
* 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
}
}
/*Initializes objects necessary for teleop
* Adds options to sendable chooser on SmartDashboard
*/
void RobotInit(){
/*if (fork()==0){
system("/home/lvuser/start_vision &");
}*/
//creates DriveTrain and sendable choosers
Drive= new DriveTrain();
chooser = new SendableChooser();
chooser2 = new SendableChooser();
//adds options to both
chooser->AddDefault(DoNothing, (void*)&DoNothing);
chooser->AddObject(autoNameLeft, (void*)&autoNameLeft);
chooser->AddObject(autoNameRight, (void*)&autoNameRight);
chooser->AddObject(autoNameOver, (void*)&autoNameOver);
chooser->AddObject(autoNameP1, (void*)&autoNameP1);
chooser->AddObject(autoNameP2, (void*)&autoNameP2);
chooser->AddObject(autoNameP3, (void*)&autoNameP3);
chooser->AddObject(autoNameP4, (void*)&autoNameP4);
chooser->AddObject(autoNameP5, (void*)&autoNameP5);
chooser->AddObject(autoNameRW, (void*)&autoNameRW);
SmartDashboard::PutData("Auto Modes", chooser);
chooser2->AddDefault(autoNameTest1, (void*)&autoNameTest1);
chooser2->AddObject(autoNameTest2, (void*)&autoNameTest2);
chooser2->AddObject(autoNameTest3, (void*)&autoNameTest3);
chooser2->AddObject(autoNameTest4, (void*)&autoNameTest4);
SmartDashboard::PutData("Auto Modes 2", chooser2);
Drive->AutonomousInit();
autonomous = new Autonomous(Drive, *((std::string*)chooser->GetSelected()));
//passes driveTrain and sendable chooser option to Autonomous class and creates it!
}
void OperatorControl (void) {
GetWatchdog().SetEnabled(true); // We do want Watchdog in Teleop, though.
DriverStationLCD *dsLCD = DriverStationLCD::GetInstance();
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " ");
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Joystick Mode");
SmartDashboard::GetInstance()->PutData("kinectMode?", kinectModeSelector);
SmartDashboard::GetInstance()->PutData("demoMode?", demoModeSelector);
SmartDashboard::GetInstance()->PutData("speedMode?", speedModeSelector);
while (IsOperatorControl() && IsEnabled()) {
GetWatchdog().Feed(); // Feed the Watchdog.
kinectMode = (bool) kinectModeSelector->GetSelected();
demoMode = (bool) demoModeSelector->GetSelected();
speedModeMult = static_cast<double*>(speedModeSelector->GetSelected());
if (kinectMode) {
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " ");
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Kinect Mode");
if (!demoMode) {
if (kinectDrive.GetRawButton(KINECT_FORWARD_BUTTON)) {
motorDriveLeft.Set(LEFT_DRIVE_POWER * *speedModeMult);
motorDriveRight.Set(RIGHT_DRIVE_POWER * -1 * *speedModeMult);
} else if (kinectDrive.GetRawButton(KINECT_REVERSE_BUTTON)) {
motorDriveLeft.Set(LEFT_DRIVE_POWER * -1 * *speedModeMult);
motorDriveRight.Set(RIGHT_DRIVE_POWER * *speedModeMult);
} else if (kinectDrive.GetRawButton(KINECT_LEFT_BUTTON)) {
motorDriveLeft.Set(LEFT_DRIVE_POWER * -1 * *speedModeMult);
motorDriveRight.Set(RIGHT_DRIVE_POWER * -1 * *speedModeMult);
} else if (kinectDrive.GetRawButton(KINECT_RIGHT_BUTTON)) {
motorDriveLeft.Set(LEFT_DRIVE_POWER * *speedModeMult);
motorDriveRight.Set(RIGHT_DRIVE_POWER * *speedModeMult);
} else {
motorDriveLeft.Set(0);
motorDriveRight.Set(0);
}
}
if (kinectManipulator.GetRawButton(KINECT_SHOOT_BUTTON)) {
motorShooter.Set(SHOOTER_MOTOR_POWER);
motorFeed.Set(FEED_MOTOR_POWER);
motorPickup.Set(PICKUP_MOTOR_POWER);
} else if (kinectManipulator.GetRawButton(KINECT_SUCK_BUTTON)) {
motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
motorFeed.Set(FEED_MOTOR_POWER * -1);
motorPickup.Set(PICKUP_MOTOR_POWER * -1);
} else {
motorShooter.Set(0);
motorFeed.Set(0);
motorPickup.Set(0);
}
if (kinectManipulator.GetRawButton(KINECT_TURRET_LEFT_BUTTON)) {
motorTurret.Set(TURRET_POWER);
} else if(kinectManipulator.GetRawButton(KINECT_TURRET_RIGHT_BUTTON)) {
motorTurret.Set(TURRET_POWER * -1);
} else {
motorTurret.Set(0);
}
} else {
if (joystickDriveLeft.GetThrottle() == 0) {
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " ");
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Xbox Mode");
motorDriveLeft.Set(Deadband(joystickManipulator.GetRawAxis(2) * -1 * *speedModeMult));
motorDriveRight.Set(Deadband(joystickManipulator.GetRawAxis(5) * *speedModeMult));
if (joystickManipulator.GetRawButton(XBOX_SHOOT_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SHOOT_BUTTON))) {
motorShooter.Set(SHOOTER_MOTOR_POWER);
motorFeed.Set(FEED_MOTOR_POWER);
motorPickup.Set(PICKUP_MOTOR_POWER);
} else if (joystickManipulator.GetRawButton(XBOX_SUCK_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SUCK_BUTTON))) {
motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
motorFeed.Set(FEED_MOTOR_POWER * -1);
motorPickup.Set(PICKUP_MOTOR_POWER * -1);
} else {
motorShooter.Set(0);
motorFeed.Set(0);
motorPickup.Set(0);
}
if (joystickManipulator.GetRawAxis(3) > 0.2 || (demoMode && joystickDriveRight.GetRawAxis(3) > 0.2)) {
motorTurret.Set(TURRET_POWER);
} else if(joystickManipulator.GetRawAxis(3) < -0.2 || (demoMode && joystickDriveRight.GetRawAxis(3) < -0.2)) {
motorTurret.Set(TURRET_POWER * -1);
} else {
motorTurret.Set(0);
}
} else {
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " ");
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Joystick Mode");
motorDriveLeft.Set(Deadband(joystickDriveLeft.GetY() * -1 * *speedModeMult));
motorDriveRight.Set(Deadband(joystickDriveRight.GetY() * *speedModeMult));
if (joystickManipulator.GetRawButton(MANIPULATOR_SHOOT_BUTTON)) {
motorShooter.Set(SHOOTER_MOTOR_POWER);
motorFeed.Set(FEED_MOTOR_POWER);
motorPickup.Set(PICKUP_MOTOR_POWER);
} else if (joystickManipulator.GetRawButton(MANIPULATOR_SUCK_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SUCK_BUTTON))) {
motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
motorFeed.Set(FEED_MOTOR_POWER * -1);
motorPickup.Set(PICKUP_MOTOR_POWER * -1);
} else {
motorShooter.Set(0);
motorFeed.Set(0);
motorPickup.Set(0);
}
motorTurret.Set(joystickManipulator.GetX() * -1 * TURRET_POWER);
}
}
dsLCD->UpdateLCD();
}
GetWatchdog().SetEnabled(false); // Teleop is done, so let's turn off Watchdog.
}
void AutonomousInit()
{
autonomousCommand = (Command *) chooser->GetSelected(); //Sends which autonomous was chosen
autonomousCommand->Start();
}
void SmartAutoPicker() {
AutonomousIndex* indexed = (AutonomousIndex*) autoMode->GetSelected();
int index = indexed->getIndex();
switch (index) {
case 0: {
SmartDashboard::PutString("STATUS:", "NOT PERFORMING ANY AUTO");
break;
}
case 1: {
AutonomousType1();
break;
}
case 2: {
AutonomousType2();
break;
}
case 3: {
AutonomousType3();
break;
}
case 4: {
AutonomousType4();
break;
}
case 5: {
AutonomousType5();
break;
}
case 6: {
AutonomousType6();
break;
}
case 7: {
AutonomousType7();
break;
}
case 8: {
AutonomousType8();
break;
}
case 9: {
AutonomousType9();
break;
}
case 10: {
AutonomousType10();
break;
}
case 11: {
AutonomousType11();
break;
}
case 12: {
AutonomousType12();
break;
}
case 13: {
AutonomousType13();
break;
}
case 99: {
CupidShuffle();
break;
}
}
}