void RobotInit() override {
chooser = new SendableChooser();
chooser->AddDefault("Low Bar Low Goal", new Option(1));
chooser->AddObject("Low Bar High Goal", new Option(2));
SmartDashboard::PutData("Auto", chooser);
CameraServer::GetInstance()->SetQuality(30);
CameraServer::GetInstance()->StartAutomaticCapture("cam0");
lstick = new Joystick(0);
rstick = new Joystick(1);
controller = new Joystick(2);
myRobot = new SigmaDrive();
myRobot->setExpiration(0.1);
mySword = new ShooterIntake();
Robot *bot = this;
Modes = new AutonomousModes(myRobot, mySword);
Drive = new Task("Drive", driveWrapper, bot);
Shooter_Intake= new Task("ShooterIntake", shootWrapper, bot);
myRobot->ResetDisplacement();
myRobot->gyro->Calibrate();
}
void RobotInit()
{
camImage = imaqCreateImage(IMAQ_IMAGE_RGB, 0);
binImage = imaqCreateImage(IMAQ_IMAGE_U8, 0);
imMask = new ImageMask(camImage, binImage, imaqError = 0);
chooser = new SendableChooser();
chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
SmartDashboard::PutData("Auto Modes", chooser);
imaqError = IMAQdxOpenCamera("cam1", IMAQdxCameraControlModeController, &session);
if(imaqError != IMAQdxErrorSuccess){
DriverStation::ReportError("IMAQdxOpencamera error: " + std::to_string((long)imaqError));
}
imaqError = IMAQdxConfigureGrab(session);
if(imaqError != IMAQdxErrorSuccess){
DriverStation::ReportError("IMAQdxConfigureGram error: " + std::to_string((long)imaqError));
}
/*
CameraServer::GetInstance()->SetQuality(50);
//the camera name (ex "cam0") can be found through the roborio web interface
CameraServer::GetInstance()->StartAutomaticCapture("cam1");
*/
//camImage = imaqCreateImage(IMAQ_IMAGE_RGB, 0);
//tal = new CANTalon(1);
//banner = new DigitalInput(4);
//banner2 = new DigitalInput(5);
//tal->SetFeedbackDevice(CANTalon::CtreMagEncoder_Absolute);
//x = 0;
}
void RobotInit()
{
chooser = new SendableChooser();
chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
SmartDashboard::PutData("Auto Modes", chooser);
}
void RobotInit()
{
chooser = new SendableChooser();
chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
SmartDashboard::PutData("Auto Modes", chooser);
triggerPin = new DigitalOutput(8);
echoPin = new DigitalInput(9);
}
Robot2009Kinect (void):
joystickManipulator(MANIPULATOR_JOYSTICK_USB_PORT),
joystickDriveLeft(LEFT_DRIVE_JOYSTICK_USB_PORT),
joystickDriveRight(RIGHT_DRIVE_JOYSTICK_USB_PORT),
kinectDrive(DRIVE_KINECT_IDENT),
kinectManipulator(MANIPULATOR_KINECT_IDENT),
motorDriveLeft(LEFT_DRIVE_MOTOR_PORT),
motorDriveRight(RIGHT_DRIVE_MOTOR_PORT),
motorShooter(SHOOTER_MOTOR_PORT),
motorTurret(TURRET_MOTOR_PORT),
motorPickup(PICKUP_MOTOR_PORT),
motorFeed(FEED_MOTOR_PORT)
{
GetWatchdog().SetEnabled(false); // If you're just beginning, and nothing's going on, there's no need for Watchdog to be doing anything.
kinectMode = false;
kinectModeSelector = new SendableChooser();
kinectModeSelector->AddDefault("Kinect Mode - OFF", (void*) false);
kinectModeSelector->AddObject("Kinect Mode - ON", (void*) true);
demoMode = false;
demoModeSelector = new SendableChooser();
demoModeSelector->AddDefault("Demo Mode - OFF", (void*) false);
demoModeSelector->AddObject("Demo Mode - ON", (void*) true);
const100 = 1;
const90 = 0.9;
const80 = 0.8;
const70 = 0.7;
const60 = 0.6;
const50 = 0.5;
const40 = 0.4;
const30 = 0.3;
const20 = 0.2;
const10 = 0.1;
const0 = 0;
speedModeMult = &const100;
speedModeSelector = new SendableChooser();
speedModeSelector->AddDefault("Speed - 100%", &const100);
speedModeSelector->AddObject("Speed - 90%", &const90);
speedModeSelector->AddObject("Speed - 80%", &const80);
speedModeSelector->AddObject("Speed - 70%", &const70);
speedModeSelector->AddObject("Speed - 60%", &const60);
speedModeSelector->AddObject("Speed - 50%", &const50);
speedModeSelector->AddObject("Speed - 40%", &const40);
speedModeSelector->AddObject("Speed - 30%", &const30);
speedModeSelector->AddObject("Speed - 20%", &const20);
speedModeSelector->AddObject("Speed - 10%", &const10);
speedModeSelector->AddObject("Speed - 0%", &const0);
}
void RobotInit()
{
joystick = new Joystick(0);
chooser = new SendableChooser();
chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
SmartDashboard::PutData("Auto Modes", chooser);
holder=new Holder(HOLDER_GATE,HOLDER_PUSHER,REVGATELIMIT,FWDGATELIMIT,IRSENSOR);
state = WAIT_FOR_BUTTON;
}
CommandBasedRobot() {
compressor = new Compressor(PRESSURE_SWITCH_PORT, COMPRESSOR_RELAY_PORT);
compressor->Start();
driveStyle = new SendableChooser();
driveStyle->AddDefault("Arcade", new ArcadeDrive());
driveStyle->AddObject("Tank", new TankDrive());
CommandBase::init();
}
void RobotInit() override {
lw = LiveWindow::GetInstance();
drive->SetExpiration(20000);
drive->SetSafetyEnabled(false);
//Gyroscope stuff
try {
/* Communicate w/navX-MXP via the MXP SPI Bus. */
/* Alternatively: I2C::Port::kMXP, SerialPort::Port::kMXP or SerialPort::Port::kUSB */
/* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */
ahrs = new AHRS(SPI::Port::kMXP);
} catch (std::exception ex) {
std::string err_string = "Error instantiating navX-MXP: ";
err_string += ex.what();
//DriverStation::ReportError(err_string.c_str());
}
if (ahrs) {
LiveWindow::GetInstance()->AddSensor("IMU", "Gyro", ahrs);
ahrs->ZeroYaw();
// Kp Ki Kd Kf PIDSource PIDoutput
turnController = new PIDController(0.015f, 0.003f, 0.100f, 0.00f,
ahrs, this);
turnController->SetInputRange(-180.0f, 180.0f);
turnController->SetOutputRange(-1.0, 1.0);
turnController->SetAbsoluteTolerance(2); //tolerance in degrees
turnController->SetContinuous(true);
}
chooser.AddDefault("No Auto", new int(0));
chooser.AddObject("GyroTest Auto", new int(1));
chooser.AddObject("Spy Auto", new int(2));
chooser.AddObject("Low Bar Auto", new int(3));
chooser.AddObject("Straight Spy Auto", new int(4));
chooser.AddObject("Adjustable Straight Auto", new int(5));
SmartDashboard::PutNumber(AUTO_LENGTH, AUTO_LENGTH_DEFAULT);
SmartDashboard::PutNumber(AUTO_SPEED, AUTO_SPEED_DEFAULT);
SmartDashboard::PutNumber(AUTO_INTAKE_SPEED, AUTO_INTAKE_SPEED_DEFAULT);
SmartDashboard::PutData("Auto Modes", &chooser);
liftdown->Set(false);
comp->Start();
}
void RobotInit()
{
chooser = new SendableChooser();
chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
SmartDashboard::PutData("Auto Modes", chooser);
imu = new ADIS16448_IMU;
imu->Reset();
imu->Calibrate();
}
virtual void RobotInit() {
CommandBase::init();
DriverStationLCD *dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
dsLCD->UpdateLCD();
//Wait(0.1);
//_autonomousCommand = new AutonomousCommand();
_autonomousCommand = NULL;
_teleopCommand = NULL;
_moveToAndDropBridgeCmd = new MoveToAndDropBridgeCmd();
_noOpCmd = new NoOpCmd();
_driveInSquareCmd = new DriveInSquareCmd();
_kinectDriveCmd = new KinectDriveCmd(CommandBase::oi->kinectLeft, CommandBase::oi->kinectRight, CommandBase::oi->kinectBridgeBtn);
_testRobotCmd = new TestRobotCmd(CommandBase::oi->driveTrigger);
_autoFireCmd = new AutonomousFireCmd();
//_driveWithJoysticksCmd = new DriveWithJoysticksCmd();
_autoChooser = new SendableChooser();
// _autoChooser->AddDefault("Autonomous Fire", _autoFireCmd);
_autoChooser->AddDefault("Move To And Drop Bridge", _moveToAndDropBridgeCmd);
// _autoChooser->AddObject("Move To And Drop Bridge", _moveToAndDropBridgeCmd);
_autoChooser->AddObject("Autonomous Fire", _autoFireCmd);
_autoChooser->AddObject("Drive with Kinect", _kinectDriveCmd);
_autoChooser->AddObject("Drive In Square", _driveInSquareCmd);
SmartDashboard::GetInstance()->PutData("AutoMenu", _autoChooser);
_teleopChooser = new SendableChooser();
_teleopChooser->AddDefault("Drive With Joysticks", _noOpCmd);
_teleopChooser->AddObject("Test Robot", _testRobotCmd);
SmartDashboard::GetInstance()->PutData("TeleopMenu", _teleopChooser);
// SmartDashboard::GetInstance()->PutData("SchedulerData", Scheduler::GetInstance());
// SmartDashboard::GetInstance()->PutData("DriveTrainSubsystem", CommandBase::driveTrainSubsystem);
// SmartDashboard::GetInstance()->PutData("AzimuthSubsystem", CommandBase::azimuthSubsystem);
}
void RobotInit()
{
frontLeft = new Talon( frontLeftChannel );
backLeft = new Talon( backLeftChannel );
frontRight = new Talon( frontRightChannel );
backRight = new Talon( backRightChannel );
MasterInterLink = new InterLinkElite( Master_InterLink_ID );
SlaveInterLink = new InterLinkElite( Slave_InterLink_ID );
ds = DriverStation::GetInstance();
lw = LiveWindow::GetInstance();
BuddyBoxEnableChooser = new SendableChooser();
BuddyBoxEnableChooser->AddDefault( "Buddy Box Disabled", (void*) false );
BuddyBoxEnableChooser->AddObject( "Buddy Box Enabled", (void*) true );
SmartDashboard::PutData( "BuddyBoxEnableChooser", BuddyBoxEnableChooser );
SlaveSpeedControlChooser = new SendableChooser();
SlaveSpeedControlChooser->AddDefault( "Trainer Controls Speed", (void*) false );
SlaveSpeedControlChooser->AddObject( "Slave Controls Speed", (void*) true );
SmartDashboard::PutData( "SlaveSpeedControlChooser", SlaveSpeedControlChooser );
}
void RobotInit()
{
manipArm = new ManipArm();
drive = new Drive();
manip = new Manipulator();
// TJF: new catapult constructor needs pointer to an OperatorInterface
//catapult = new Catapult();
oi = new OperatorInterface();
catapult = new Catapult(oi);
chooser = new SendableChooser();
autonomous = new Autonomous();
chooser->AddDefault("Go Straight Auto", new Autonomous()); //the second parameter require constructor not a function
chooser->AddObject("Random Auto", new Autonomous(true));
SmartDashboard::PutData("Autonomous Modes", chooser); //not displaying because there are no input
}
void RobotInit()
{
chooser = new SendableChooser();
chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
SmartDashboard::PutData("Auto Modes", chooser);
//Initialize the I2C connectin on address 84
spi = new SPI(SPI::Port::kOnboardCS0); //you can change the port; kOnboardCS0-3
spi->Init();
spi->SetClockRate(500000);
spi->SetMSBFirst();
spi->SetSampleDataOnFalling();
spi->SetClockActiveLow();
spi->SetChipSelectActiveLow();
}
void RobotInit()
{ //pusher = 2, gate = 4
chooser = new SendableChooser();
chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
SmartDashboard::PutData("Auto Modes", chooser);
lidar = new Lidar(I2C::kMXP, 0x62);
leftWheel = new SRXSpeed(3, P_CONSTANT, 0.0, 0.0, 1);
rightWheel = new SRXSpeed(1, P_CONSTANT, 0.0, 0.0, 1);
leftWheel->SetInverted(true);
stick = new Joystick(0);
angle = new AngleAccelerometer(I2C::kOnboard);
angleMotor = new CANTalon(5);
anglePID = new PIDController(.05, 0, 0, angle, angleMotor);
launch = new Launcher(leftWheel, rightWheel, anglePID);
}
void RobotInit() {
CameraServer::GetInstance()->SetQuality(50);
CameraServer::GetInstance()->StartAutomaticCapture("cam0");
chooser = new SendableChooser();
chooser->AddDefault(autoNameDefault, (void*) &autoNameDefault);
chooser->AddObject(autoNameCustom, (void*) &autoNameCustom);
SmartDashboard::PutData("Auto Modes", chooser);
SmartDashboard::PutString("hello", "I'm here");
//LEFTDRIVE1 = new CANTalon(3);
//LEFTDRIVE2 = new CANTalon(2);
//RIGHTDRIVE1 = new CANTalon(1);
//RIGHTDRIVE2 = new CANTalon(4);
LEFTDRIVE1 = new CANTalon(1);
LEFTDRIVE2 = new CANTalon(4);
RIGHTDRIVE1 = new CANTalon(3);
RIGHTDRIVE2 = new CANTalon(2);
TOPMOTOR1 = new CANTalon(5);
//m_canTalonRightRear + m_canTalonRightFront = new CANTalon(RightCANWheels);
//m_canTalonLeftRear + m_canTalonLeftFront = new CANTalon (LeftCANWheels);
//m_PWMTalonRightRearTop + m_PWMTalonRightFrontTop = new Talon(RightWheels);
//m_PWMTalonLeftRearTop + m_PWMTalonLeftFrontTop = new Talon(LeftWheels);
// m_PWMTalonRightRearTop = new Talon(8);
// m_PWMTalonRightFrontTop = new Talon(6);
// m_PWMTalonLeftRearTop = new Talon(9);
// m_PWMTalonLeftFrontTop = new Talon(7);
m_lStick = new Joystick(1);
//m_rStick = new Joystick(0);
//drive = new RobotDrive(RightCANWheels, LeftCANWheels, RightWheels, LeftWheels);
//drive = new RobotDrive(m_PWMTalonLeftFrontTop, m_PWMTalonLeftRearTop,m_PWMTalonRightFrontTop, m_PWMTalonRightRearTop);
drive = new RobotDrive(LEFTDRIVE1, LEFTDRIVE2, RIGHTDRIVE1, RIGHTDRIVE2);
//drive->SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
//drive->SetInvertedMotor(RobotDrive::kRearLeftMotor, false);
//drive->SetInvertedMotor(RobotDrive::kRearRightMotor, false);
leftEncoder = new Encoder(0, 1);
rightEncoder = new Encoder(2, 3);
}
void RobotInit()
{
// Auto chooser
chooser = new SendableChooser();
chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
SmartDashboard::PutData("Auto Modes", chooser);
// Drive declarations
frontLeftTalon = new CANTalon(frontLeftChannel);
rearLeftTalon = new CANTalon(rearLeftChannel);
frontRightTalon = new Victor(frontRightChannel);
rearRightTalon = new CANTalon(rearRightChannel);
//frontLeftTalon->SetInverted(true);
rearLeftTalon->SetInverted(true);
yawGyro = new ADXRS450_Gyro();
robotDrive = new RobotDrive(frontLeftTalon, rearLeftTalon, frontRightTalon, rearRightTalon);
flightStick = new Joystick(flightstickChannel);
robotDrive -> SetSafetyEnabled(false);
// Shooter variable declarations MAKE SURE YOU PLUG THEM INTO THE RIGHT PORTS WENDY
motor1 = new Talon(8); // loook above
motor2 = new Jaguar(4);
motor3 = new Jaguar(5);
shootStick = new Joystick(0);
shooterSwitch = new DigitalInput(0);
// Distance sensor declarations
distanceSensor = new AnalogInput(0);
getVcc = new AnalogInput(2);
// Servo
doorLift = new Servo(9);
// Camera stuff
camera1 = new USBCamera("cam0", false);
camera2 = new USBCamera("cam1", false);
camera1->OpenCamera();
camera2->OpenCamera();
camera1->StartCapture();
camera2->StartCapture();
//Camera servos
frontCamServo = new Servo(10); // WHAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAT
backCamServo = new Servo(11); // WHAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAT
}
/*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 RobotInit()
{
constants = new Constants();
Drivestick = new Joystick(0);
Operatorstick = new Joystick(1);
lw = LiveWindow::GetInstance();
solenoid = new Solenoid(5);
Drive = new SixWheelDrive (constants);
ShootIntake = new ShooterIntake (constants);
chooser = new SendableChooser();
chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
SmartDashboard::PutData("Auto Modes", chooser);
//Timer for the motion profiler
//double CurrentTime=0;
//double Time;
//Timer Time;
//RollerEncoder = new Encoder(constants->Get("RollerEncoderArmA"), constants->Get("RollerEncoderArmB"));
}
Robot() :
robotDrive(new Talon(frontLeftChannel), new Talon(rearLeftChannel),
new Talon(frontRightChannel), new Talon(rearRightChannel)), stick(
STICK_CHANNEL), liftStick(LIFTSTICK_CHANNEL), chainLift(
CHAINLIFT_PWM), maxUp(MAXUP_DIO), maxDown(MAXDOWN_DIO), midPoint(
MIDPOINT_DIO), autoSwitch1(AUTOSWITCH1_DIO), autoSwitch2(
AUTOSWITCH2_DIO), leftIR(LEFTIR_LOC), rightIR(RIGHTIR_LOC), canGrabber(
CANGRAB_PWM) {
SmartDashboard::init();
ds = DriverStation::GetInstance();
SmartDashboard::PutString("STATUS:", "INITIALIZING");
robotDrive.SetExpiration(0.1);
robotDrive.SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
robotDrive.SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
autoMode = new SendableChooser();
autoMode->AddDefault("00: Do Nothing", new AutonomousIndex(0));
autoMode->AddObject("01: Just Drive Forward", new AutonomousIndex(1));
autoMode->AddObject("02: Stack bin on tote, turn 90, go",
new AutonomousIndex(2));
autoMode->AddObject("03: Lift up and go forward",
new AutonomousIndex(3));
autoMode->AddObject("04: Lift up, turn 90, go", new AutonomousIndex(4));
autoMode->AddObject("05: Stack 3 bins w/ correction constant",
new AutonomousIndex(5));
autoMode->AddObject("06: Stack 3 bins w/ acclerometer",
new AutonomousIndex(6));
autoMode->AddObject("07: Grab first bin from landfill",
new AutonomousIndex(7));
autoMode->AddObject("08: Grab yellow bin and back up TO RAMP",
new AutonomousIndex(8));
autoMode->AddObject("09: Grab two bins from landfill, slide sideways",
new AutonomousIndex(9));
autoMode->AddObject("10: Grab from landfill, over by turning",
new AutonomousIndex(10));
autoMode->AddObject("11: Grab yellow bin, back up to AUTOZONE, drop",
new AutonomousIndex(11));
autoMode->AddObject("12: Grab yellow bin, back up to AUTOZONE, stop",
new AutonomousIndex(12));
autoMode->AddObject("13: Steal two green cans. Gottta go fast",
new AutonomousIndex(13));
autoMode->AddObject("99: CUPID SHUFFLE", new AutonomousIndex(99));
SmartDashboard::PutString("Both Off", "Pick from radio list");
SmartDashboard::PutString("On-Off", "Grab yellow and back up");
SmartDashboard::PutString("Off-On",
"Grab, turn right, drive, turn left");
SmartDashboard::PutString("Both On",
"Lift up, turn 90, drive to auto zone");
SmartDashboard::PutData("BOTH SWITCHES ON: Pick One:", autoMode);
SmartDashboard::PutData(Scheduler::GetInstance());
SmartDashboard::PutString("STATUS:", "READY");
SmartDashboard::PutBoolean("Smart Dashboard Enabled", false);
tick = 0;
leftIRZero = 0;
rightIRZero = 0;
}
void RobotInit()
{
chooser = new SendableChooser();
chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
}
void RobotInit()
{
chooser.AddDefault(autoNameDefault, (void*)&autoNameDefault);
chooser.AddObject(autoNameCustom, (void*)&autoNameCustom);
SmartDashboard::PutData("Auto Modes", &chooser);
}
void RobotInit()
{
frame = imaqCreateImage(IMAQ_IMAGE_RGB, 0);
//camera.reset(new AxisCamera("axis-camera.local"));
table = NetworkTable::GetTable("GRIP/myContoursReport");
chooser = new SendableChooser();
chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
for (std::map<std::string, bool (*)()>::const_iterator it = Autonomous::crossFunctions.begin(); it!= Autonomous::crossFunctions.end(); it++) {
chooser->AddObject(it->first, (void*)&(it->first));
}
SmartDashboard::PutData("Auto Modes", chooser);
posChooser = new SendableChooser();
posChooser->AddDefault(posDefault, (void*)&posToDegrees[stoi(posDefault)]);
for (int i = 1; i < 6; i++) {
posChooser->AddObject(std::to_string(i), (void*)&posToDegrees[i]);
}
SmartDashboard::PutData("Position", posChooser);
shootChooser = new SendableChooser();
shootChooser->AddDefault(shootDefault, (void*)&shootDefault);
shootChooser->AddObject("No", (void*)"No");
SmartDashboard::PutData("Shoot", shootChooser);
drive = new RobotDrive(2,3,0,1);
//drive = new RobotDrive(frontLeft, backLeft, frontRight, backRight);
drive->SetInvertedMotor(RobotDrive::MotorType::kFrontLeftMotor, true);
drive->SetInvertedMotor(RobotDrive::MotorType::kRearLeftMotor, true);
drive->SetInvertedMotor(RobotDrive::MotorType::kFrontRightMotor, true);
drive->SetInvertedMotor(RobotDrive::MotorType::kRearRightMotor, true);
drive->SetExpiration(0.2);
drive->SetMaxOutput(0.5);
driveStick = new Joystick(0);
shootStick = new Joystick(1);
launchPiston = new Solenoid(3);
//tiltPiston = new DoubleSolenoid(0,1);
defensePiston = new DoubleSolenoid(0,1);
launch1 = new VictorSP(4);
launch2 = new VictorSP(5);
launch1->SetInverted(true);
winch = new VictorSP(6);
otherWinch = new Victor(7);
gyro = new AnalogGyro(1);
leftEnc = new Encoder(2, 3, false, Encoder::EncodingType::k1X);
rightEnc = new Encoder(0,1, false, Encoder::EncodingType::k1X);
//Configure for inches.t551
leftEnc->SetDistancePerPulse(-0.06);
rightEnc->SetDistancePerPulse(0.06);
launcherSensor = new DigitalInput(9);
Autonomous::init(drive, gyro, leftEnc, rightEnc);
timer = new Timer();
defenseUp = false;
debounce = false;
//if (fork() == 0) {
// system("/home/lvuser/grip &");
//}
launcherDown = false;
}