Robot() :
robotDrive(frontLeftChannel, rearLeftChannel,
frontRightChannel, rearRightChannel), // initialize variables in same
stick(joystickChannel) // order as declared above.
{
rotateToAngleRate = 0.0f;
robotDrive.SetExpiration(0.1);
robotDrive.SetInvertedMotor(RobotDrive::kFrontLeftMotor, true); // invert left side motors
robotDrive.SetInvertedMotor(RobotDrive::kRearLeftMotor, true); // change to match your robot
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());
}
turnController = new PIDController(kP, kI, kD, kF, ahrs, this);
turnController->SetInputRange(-180.0f, 180.0f);
turnController->SetOutputRange(-1.0, 1.0);
turnController->SetAbsoluteTolerance(kToleranceDegrees);
turnController->SetContinuous(true);
/* Add the PID Controller to the Test-mode dashboard, allowing manual */
/* tuning of the Turn Controller's P, I and D coefficients. */
/* Typically, only the P value needs to be modified. */
LiveWindow::GetInstance()->AddActuator("DriveSystem", "RotateController", turnController);
if ( ahrs ) {
LiveWindow::GetInstance()->AddSensor("IMU", "Gyro", ahrs);
}
}
Robot() :
timer(),
robotDrive(Constants::driveFrontLeftPin, Constants::driveRearLeftPin, Constants::driveFrontRightPin, Constants::driveRearRightPin),//tells the robot where everything is plugged in
i2c(I2C::kOnboard,Constants::ledAddress),
driveStick(Constants::driveStickChannel),
grabStick(Constants::grabStickChannel),
prox(Constants::driveUltrasonicPin),
grabTalon(Constants::grabTalonPin),
grabInnerLimit(Constants::grabInnerLimitPin),
grabOuterLimit(Constants::grabOuterLimitPin),
liftTalon(Constants::liftTalonPin),
liftEncoder(Constants::liftEncoderAPin, Constants::liftEncoderBPin),
liftUpperLimit(Constants::liftUpperLimitPin),
liftLowerLimit(Constants::liftLowerLimitPin),
grabEncoder(Constants::grabEncoderAPin, Constants::grabEncoderBPin),
pdp(),
gyro(Constants::driveGyroRate),
pickup(grabTalon, grabInnerLimit, grabOuterLimit, liftTalon, liftEncoder, liftUpperLimit, liftLowerLimit, pdp)
{
robotDrive.SetExpiration(0.1); // safety feature
robotDrive.SetInvertedMotor(RobotDrive::kFrontRightMotor, true); // make the motors go the right way
robotDrive.SetInvertedMotor(RobotDrive::kRearRightMotor, true);
robotDrive.SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
liftEncoder.SetReverseDirection(Constants::liftEncoderReverse);
}
DragonBotDriveTrain(void) {
drive = new RobotDrive(
new Victor(FRONT_LEFT_PWM),
new Victor(BACK_LEFT_PWM),
new Victor(FRONT_RIGHT_PWM),
new Victor(BACK_RIGHT_PWM)
);
drive->SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
drive->SetInvertedMotor(RobotDrive::kRearRightMotor, true);
//makingSmoke = false;
smoke_cannon = new Victor(SMOKE_CANNON_PWM);
left_eye_x = new Servo(LEFT_EYE_X_PWM);
//left_eye_y = new Servo(LEFT_EYE_Y_PWM);
right_eye_x = new Servo(RIGHT_EYE_X_PWM);
//right_eye_y = new Servo(RIGHT_EYE_Y_PWM);
gamepad = new Gamepad(1);
gamepad2 = new Gamepad(2);
smoke_machine = new DigitalOutput(SMOKE_MACHINE_RELAY);
lcd = DriverStationLCD::GetInstance();
jaw_motor = new Victor(JAW_MOTOR_PWM);
head_motor = new Victor(HEAD_MOTOR_PWM);
tophead_limit = new DigitalInput(TOPHEAD_LIMIT_PIN);
bottomjaw_limit = new DigitalInput(BOTTOMJAW_LIMIT_PIN);
crash_limit = new DigitalInput(CRASH_LIMIT_PIN);
default_eye_position = 15.0f; //TODO: figure this out
making_smoke_timer = new Timer();
firing_smoke_timer = new Timer();
}
/*this fuction will need to be updated for the final robot*/
void FunctionBot::configDrive() {
drive->SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
drive->SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
//drive->SetInvertedMotor(RobotDrive::kFrontLeftMotor,true);
drive->SetSafetyEnabled(false);
//not sure on this value at all
drive->SetMaxOutput(333);
}
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;
}
Wheels()
{
drive = new RobotDrive(0,2,4,3); //frontLeft, rearLeft, frontRight, rearRight
drive->SetInvertedMotor(RobotDrive::kFrontLeftMotor, true); // 0 is front left wheel
drive->SetInvertedMotor(RobotDrive::kRearLeftMotor, true); // 2 is back left wheel
drive->SetExpiration(0.1);
drive->SetSafetyEnabled(true);
driveSafety = new MotorSafetyHelper(drive);
}
/****************************************
* MainRobot: (The constructor)
* Mandatory method.
* TODO:
* - Tweak anything related to the scissor lift - verify values.
* - Find out how to configure Victor.
*/
MainRobot(void):
// Below: The constructor needs to know which port connects to which
// motor so it can control the Jaguars correctly.
// See constants at top.
robotDrive(LEFT_FRONT_MOTOR_PORT, LEFT_REAR_MOTOR_PORT,
RIGHT_FRONT_MOTOR_PORT, RIGHT_REAR_MOTOR_PORT)
{
SmartDashboard::init();
GetWatchdog().SetExpiration(0.1); // In seconds.
stick1 = new Joystick(MOVE_JOYSTICK_USB); // Joystick 1
stick2 = new Joystick(LIFT_JOYSTICK_USB); // Joystick 2
minibot = new MinibotDeployment (
MINIBOT_DEPLOY_PORT,
MINIBOT_DEPLOYED_DIO,
MINIBOT_RETRACTED_DIO);
lineSensors = new LineSensors (
LEFT_CAMERA_PORT,
MIDDLE_CAMERA_PORT,
RIGHT_CAMERA_PORT);
lift = new LiftController (
LIFT_MOTOR_PORT,
HIGH_LIFT_DIO,
LOW_LIFT_DIO);
lift->initButtons(
kJSButton_2, // Botton top button
kJSButton_4, // Left top button
kJSButton_3, // Center top button
kJSButton_5); // Right top button
// The wiring was inverted on the left motors, so the below
// is necessary.
robotDrive.SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
robotDrive.SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
isFastSpeedOn = false;
isSafetyModeOn = true;
isLiftHigh = false;
// isSafetyModeOn: Controlled by the driver -- should only have to
// choose once.
// isLiftHigh: Controlled by the program -- turns true only
// when height is too high, otherwise turns false.
isDoingPreset = false;
GetWatchdog().SetEnabled(true);
UpdateDashboard("TestingTestingTestingTesting"
"TestingTestingTestingTestingTesting");
UpdateDashboard("Finished initializing.");
}
RobotDemo(void):
myRobot(8, 1, 9, 2),
stick(1),
digEncoder(13, 14),
ultra(1, 1)
{
myRobot.SetExpiration(0.1);
myRobot.SetInvertedMotor(myRobot.kFrontRightMotor, true);
//myRobot.SetInvertedMotor(myRobot.kFrontLeftMotor, true);
myRobot.SetInvertedMotor(myRobot.kRearRightMotor, true);
//myRobot.SetInvertedMotor(myRobot.kRearLeftMotor, true);
}
//Class constructor
// Initialize the robot drive to:
// Drive base:
// Left Front Right
// +---------------------+
// | PWM 9 PWM 7|
// | Robot |
// | PWM 8 PWM 6|
// +---------------------+
// Back
Robot() :
robotDrive(9, 8, 7, 6), // these must be initialized in the same order
leftStick(0),
rightStick(1),// as they are declared above.
lw(NULL),
autoLoopCounter(0)
{
robotDrive.SetExpiration(0.1);
robotDrive.SetInvertedMotor(robotDrive.kFrontLeftMotor,false);
robotDrive.SetInvertedMotor(robotDrive.kFrontRightMotor,false);
robotDrive.SetInvertedMotor(robotDrive.kRearLeftMotor,false);
robotDrive.SetInvertedMotor(robotDrive.kRearRightMotor,false);
}
Robot() :
frontLeftChannel(15),
frontRightChannel(11),
rearLeftChannel(12),
rearRightChannel(10),
robotDrive(frontLeftChannel, rearLeftChannel,
frontRightChannel, rearRightChannel), // these must be initialized in the same order
stick(joystickChannel) // as they are declared above.
{
robotDrive.SetExpiration(0.1);
robotDrive.SetInvertedMotor(RobotDrive::kFrontLeftMotor, true); // invert the motors
robotDrive.SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
robotDrive.SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
robotDrive.SetInvertedMotor(RobotDrive::kRearRightMotor, true); // you may need to change or remove this to match your robot
}
Mustybot(void) :
//constucts the objects (in the same order), perameters are the port their PWM cable is connected to unless specified
drive(1, 2), //both wheels
//controllers - perameter is the number joystick on the list in the driver station
xbox(1),
//Digital Inputs - The first perameter is the module number (in our case it will probably always be 1). The devices are pluged into the Digital IO side of the digital sidecar.
dumperSwitch(1),
//Analog Inputs - PWM Cables attach to the Analog Breakout Board on top of Module 1 or 5
gyro(1)
{
drive.SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
drive.SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
}
Chimichanga(void)
{
//Hardware
drivetrain = new RobotDrive(PWM_DRIVE_FL, PWM_DRIVE_RL, PWM_DRIVE_FR, PWM_DRIVE_RR);
drivetrain->SetInvertedMotor(drivetrain->kFrontLeftMotor, false);
drivetrain->SetInvertedMotor(drivetrain->kRearLeftMotor, false);
drivetrain->SetInvertedMotor(drivetrain->kFrontRightMotor, false);
drivetrain->SetInvertedMotor(drivetrain->kRearRightMotor, false);
compressor = new Compressor(DIO_PRESSURE, RELAY_COMPRESSOR);
//Sensors
leftDrivetrainEncoder = new Encoder(DIO_ENCODER_DRIVE_LEFT_A, DIO_ENCODER_DRIVE_LEFT_B);
rightDrivetrainEncoder = new Encoder(DIO_ENCODER_DRIVE_RIGHT_A, DIO_ENCODER_DRIVE_RIGHT_B);
driverJoystick = new Joystick(JOYSTICK_DRIVE);
//Systems
kicker = new Kicker();
}
Robot(void) : dseio(ds->GetInstance()->GetEnhancedIO())
{
this->SetPeriod(.05);
FL = new Jaguar(PWM_PORT_5);
RL = new Jaguar(PWM_PORT_7);
FR = new Jaguar(PWM_PORT_4);
RR = new Jaguar(PWM_PORT_6);
mygyro = new Gyro(AI_PORT_1);
drive = new RobotDrive(FL,RL,FR,RR);
drive->SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
drive->SetInvertedMotor(RobotDrive::kRearRightMotor,true);
drive->SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);//inversed because motor physically spins in the rwrong direction
drive->SetInvertedMotor(RobotDrive::kRearLeftMotor, false);
drive->SetExpiration(15);
left = new DigitalInput(DI_PORT_1);
middle = new DigitalInput(DI_PORT_2);
right = new DigitalInput(DI_PORT_3);
StraightLineSwitch = new DigitalInput(DI_PORT_5);
GoLeftSwitch = new DigitalInput(DI_PORT_6);
robot_compressor = new Compressor(DI_PORT_4,DO_PORT_1);
rightStick = new Joystick(1);
leftStick = new Joystick(2);
myarm = new DDCArm();
deploy = new Deployment();
autotimer = new Timer();
}
Robot() :
robotDrive(frontLeftChannel, rearLeftChannel,
frontRightChannel, rearRightChannel), // initialize variables in
stick(joystickChannel) // same order declared above
{
robotDrive.SetExpiration(0.1);
robotDrive.SetInvertedMotor(RobotDrive::kFrontLeftMotor, true); // invert left side motors
robotDrive.SetInvertedMotor(RobotDrive::kRearLeftMotor, true); // change to match your robot
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);
}
}
// For the RobotDemo() constructor list the component constructors (myrobot, rightstick etc) in the order declared above.
RobotDemo()://This is the constructer function
lcd(DriverStationLCD::GetInstance()),
dashboardPreferences(Preferences::GetInstance()),
driveTrain(PHOENIX2014_DRIVEMOTOR_LEFT_FRONT,PHOENIX2014_DRIVEMOTOR_LEFT_REAR,PHOENIX2014_DRIVEMOTOR_RIGHT_FRONT,PHOENIX2014_DRIVEMOTOR_RIGHT_REAR), // rearleftmotor (pwm channel),frontleftmotor , rearrightmotor (pwm channel)
rightJoyStick(2),// as they are declared above.
leftJoyStick(1),
gamePad(3),
ballGrabber(&gamePad),
driveDistanceRight(PHOENIX2014_R_DRIVE_ENCODER_A,PHOENIX2014_R_DRIVE_ENCODER_B ),
driveDistanceLeft(PHOENIX2014_L_DRIVE_ENCODER_A, PHOENIX2014_L_DRIVE_ENCODER_B),
testSwitch(3),
testTalons(PHOENIX2014_DRIVEMOTOR_LEFT_FRONT),
frontUltrasonic(PHOENIX2014_ANALOG_MODULE_NUMBER, PHOENIX2014_ANALOG_ULTRASONIC_FRONT),
backUltrasonic(PHOENIX2014_ANALOG_MODULE_NUMBER, PHOENIX2014_ANALOG_ULTRASONIC_BACK),
analogTestSwitch(PHOENIX2014_ANALOG_MODULE_NUMBER, 5),
lightBulb(PHOENIX2014_RELAY_LIGHTBULB)
{
driveTrain.SetExpiration(0.1);
driveTrain.SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
driveTrain.SetInvertedMotor(RobotDrive::kRearRightMotor, false);
m_FromAutonomous = false;
m_MinDriveLoop = 2*200;
}
RobotDemo()
{
#if YEAR_2013
drive = new RobotDrive(left_drive_motor_A_PWM, left_drive_motor_B_PWM,
right_drive_motor_A_PWM, right_drive_motor_B_PWM);
drive->SetExpiration(15);
drive->SetSafetyEnabled(false);
#endif
drive->SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
drive->SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
drive->SetInvertedMotor(RobotDrive::kRearRightMotor, true);
drive->SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
//Joystick
//ds = new DriverStation();
drive_stick_sec = new Joystick(right_stick);
drive_stick_prim = new Joystick(left_stick);
operator_stick = new Joystick(operator_joystick);
//Motors
#if JAGUAR_SWITCH
shooter_motor_front = new Jaguar(shooter_front_motor);
shooter_motor_back = new Jaguar(shooter_back_motor);
#else
shooter_motor_front = new Talon(shooter_front_motor);
shooter_motor_back = new Talon(shooter_back_motor);
#endif
#if DUMB_DRIVE_CODE
left_drive_motor_A = new Victor(left_drive_motor_A_PWM);
left_drive_motor_B = new Victor(left_drive_motor_B_PWM);
right_drive_motor_A = new Victor(right_drive_motor_A_PWM);
right_drive_motor_B = new Victor(right_drive_motor_B_PWM);
#endif
#if YEAR_2013
climbing_motor = new Victor(climbing_motor_PWM);
#endif
//limit switches
top_claw_limit_switch = new DigitalInput(top_claw_limit_switch_port);
bottom_claw_limit_switch = new DigitalInput(
bottom_claw_limit_switch_port);
//Encoders
front_shooter_encoder = new Encoder(shooter_motor_front_encoder_A_port,
shooter_motor_front_encoder_B_port, false);
back_shooter_encoder = new Encoder(shooter_motor_back_encoder_A_port,
shooter_motor_back_encoder_B_port, false);
//solenoids
shooter_angle_1 = new Solenoid(SHOOTER_ANGLE_SOLENOID_1);
shooter_angle_2 = new Solenoid(SHOOTER_ANGLE_SOLENOID_2);
shooter_fire_piston_A = new Solenoid(shooter_fire_piston_solenoid_A);
shooter_fire_piston_B = new Solenoid(shooter_fire_piston_solenoid_B);
//Timers
shooter_piston_timer = new Timer();
VC_timer = new Timer();
loop_time_measure_timer = new Timer();
shooter_reset = new Timer();
pid_code_timer = new Timer();
autonomous_timer = new Timer();
error_timer = new Timer();
retraction_timer = new Timer();
override_timer = new Timer();
stabilizing_timer = new Timer();
shooter_stop_timer = new Timer();
//Compressor
compressor1 = new Compressor(PRESSURE_SWITCH, compressor_enable);
#if CAMERA
//Camera
camera = &(AxisCamera::GetInstance(AxisCameraIpAddr));
camera->WriteResolution(AxisCamera::kResolution_640x480);
AxisCamera::GetInstance();
#endif
//Function starter
compressor1 ->Start();
//float RPS;
//PIDController pid1 (0.1 , 0.001 ,0.0 , &RPS , test_motor );
loop_time_measure_timer ->Start();
VC_timer ->Start();
//Variable Initialization
arcadedrive = true;
test_encoder_value = 0;
total_test_encoder_value = 0;
old_RPS = 0;
new_RPS = 0;
second_count = 0;
set_speed = 0.5;
cycle_counter = 0;
//test_motor ->Set(set_speed);
// float RPS;
//int old_encoder_value;
//double old_timer;
speed2 = 0.0;
test_speed = 0.3;
button = false;
pickup_on = false;
switch_on = false;
kicker_piston_on = false;
kicker_button_on = false;
average_counter = 0;
counter = 1;
number = 1;
additive_error = 0;
prev_error_front = 0;
prev_desired_RPS = 0;
ROC = 0;
claw_ = false;
claw_go_down = false;
constant_ = false;
constant_desired_RPS = false;
divided = 0;
//RPS_encoder_1 = new Encoder(9, 10);// for 2012 robot
shooter_motor_back_RPS = shooter_motor_back->Get();
//RPS_encoder_1 -> SetDistancePerPulse(1 / 250);//TODO figure out how this works
first_press = true;
test_RPS = false;
desired_RPS_control = 0.0;
slow_control = 0;
//RPS_encoder_1 ->Start();
pid_code_timer ->Start();
front_shooter_encoder->Start();
back_shooter_encoder->Start();
autonomous_timer ->Start();
error_timer ->Start();
retraction_timer->Start();
stabilizing_timer->Start();
override_timer->Start();
}
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;
}
void OperatorControl()
{
compressor.Start();
myRobot.SetSafetyEnabled(true);
myRobot.SetInvertedMotor(myRobot.kRearLeftMotor, true);//Invert rear left Motor
myRobot.SetInvertedMotor(myRobot.kRearRightMotor, true);//Invert rear right motor
myRobot.SetInvertedMotor(myRobot.kFrontLeftMotor, true);//Invert rear right motor
myRobot.SetInvertedMotor(myRobot.kFrontRightMotor, true);
DriverStation *ds;
DriverStationLCD *dsLCD;
ds = DriverStation::GetInstance();
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Printf(DriverStationLCD::kUser_Line1,1, "Starting Teleop");
dsLCD->UpdateLCD();
while (true)
{
if (!compressor.GetPressureSwitchValue()){
compressor.Start();
}
myRobot.ArcadeDrive(stick);
/*PNEUMATIC CODE*/
if (stick.GetRawButton(8)){
shooterSole.Set(shooterSole.kForward);
}
if (stick.GetRawButton(1)){
shooterSole.Set(shooterSole.kReverse);
}
/*SHOOTER CODE*/
if (stick.GetRawButton(2)){
shooter.SetSpeed(1);
}
if (stick.GetRawButton(4)){
shooter.SetSpeed(-1);
dsLCD->Printf(DriverStationLCD::kUser_Line1,(int)forklift.Get(), "Shooter Should be negative");
dsLCD->UpdateLCD();
}
if (!stick.GetRawButton(4) || !stick.GetRawButton(2)){
shooter.SetSpeed(0);
}
/* FORKLIFT CODE*/
if (!stick.GetRawButton(5) || !stick.GetRawButton(6)){
forklift.SetSpeed(0);
}
if (stick.GetRawButton(5)){
forklift.SetSpeed(1);
}
if (stick.GetRawButton(6)){
forklift.SetSpeed(-1);
dsLCD->Printf(DriverStationLCD::kUser_Line1,(int)forklift.Get(), "Forklift Should be negative");
dsLCD->UpdateLCD();
}
if (!shoot.Get()){
shooter.SetSpeed(0);
shooterSole.Set(shooterSole.kForward);
}
if (stick.GetRawButton(11)){
//myRobot.m_rearLeftMotor.SetSpeed(1);
//myRobot.m_rearRightMotor.SetSpeed(1);
//myRobot.m_frontLeftMotor.SetSpeed(1);
//myRobot.m_frontRightMotor.SetSpeed(1);
}
//Wait(0.005);// wait for a motor update time
}
}