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;
}
