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);
}
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()
{
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();
}
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();
}
virtual void TeleopInit() {
CommandBase::turret->Reset();
CommandBase::turret->Start();
driveCommand = (Command*) driveStyle->GetSelected();
driveCommand->Start();
}
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()
{
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();
}
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();
}
/**
* 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();
}
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
}
void RobotInit()
{
CommandBase::init();
chooser = new SendableChooser();
chooser->AddDefault("Default Auto", new AutoParameterizedDriveCmd());
//chooser->AddObject("My Auto", new MyAutoCommand());
SmartDashboard::PutData("Auto Modes", chooser);
}
// 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 Autonomous(){
Option *num = (Option *) chooser->GetSelected();
myRobot->ResetDisplacement();
Modes->SetMode(num->Get());
Modes->Run();
while(IsAutonomous() && IsEnabled()){
Wait(0.05);
Scheduler::GetInstance()->Run();
}
}
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
}
}
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()
{
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"));
}
/**
* 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
}
}
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();
}
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);
}