void ManageCompressor () {
if (m_compressor->GetPressureSwitchValue()) {
m_compressor->Stop();
} else {
m_compressor->Start();
}
}
void OperatorControl(void)
{
OperatorControlInit();
compressor.Start();
testActuator.Start();
while (IsOperatorControl())
{
ProgramIsAlive();
//No need to do waits because ProgramIsAlive function does a wait. //Wait(0.005);
bool isButtonPressed = stick.GetRawButton(3);
SmartDashboard::PutNumber("Actuator Button Status",isButtonPressed);
if (isButtonPressed)
{
testActuator.Go();
}
float leftYaxis = stick.GetY();
float rightYaxis = stick.GetRawAxis(5); //RawAxis(5);
TankDrive(leftYaxis,rightYaxis); // drive with arcade style (use right stick)for joystick 1
SmartDashboard::PutNumber("Left Axis",leftYaxis);
SmartDashboard::PutNumber("Right Axis",rightYaxis);
}
}
void AutonomousPeriodic(void)
{
//Start compressor
compressor->Start();
//Autonomous code goes here
}
void TeleopPeriodic(void )
{
/*
* Code placed in here will be called only when a new packet of information
* has been received by the Driver Station. Any code which needs new information
* from the DS should go in here
*/
//Start compressor
compressor->Start();
driveTrainValues();
deadzone();
//Drivetrain.....
//When button eight is pressed robot drives at 25% speed
printf("right: %f and left: %f\n", useright, useleft);
if (gamepad->GetRawButton(8))
{
drivetrain->TankDrive((-0.5*(useleft)), (-0.5*(useright)));
//Negative for switched wires
}
else
{
drivetrain->SetLeftRightMotorOutputs(-useleft, -useright);
//Normal driving
//Negative for switched wires
}
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
GetWatchdog().SetEnabled(true);
compressor->Start();
GetWatchdog().SetExpiration(0.5);
bool valve_state = false;
while (IsOperatorControl())
{
motor->Set(stick->GetY());
if (stick->GetRawButton(1) && !valve_state)
{
valve->Set(true);
valve_state = true;
}
if (!stick->GetRawButton(1) && valve_state)
{
valve->Set(false);
valve_state = false;
}
// Update driver station
//dds->sendIOPortData(valve);
GetWatchdog().Feed();
}
}
RobotSystem(void):
robotInted(false)
,stick(1) // as they are declared above.
,stick2(2)
,line1(10)
,line2(11)
,line3(12)
//,camera(AxisCamera::GetInstance())
,updateCAN("CANUpdate",(FUNCPTR)UpdateCAN)
,cameraTask("CAMERA", (FUNCPTR)CameraTask)
,compressor(14,1)
,EncArm(2,3)
,EncClaw(5,6)
,PIDArm(.04,0,0) // .002, .033
,PIDClaw(.014,.0000014,0)
,LowArm(.1)
/*
,MiniBot1(4)
,MiniBot2(2)
,ClawGrip(3)
*/
,MiniBot1a(8,1)
,MiniBot1b(8,2)
,MiniBot2a(8,3)
,MiniBot2b(8,4)
,ClawOpen(8, 8)
,ClawClose(8,7)
,LimitClaw(7)
,LimitArm(13)
{
// myRobot.SetExpiration(0.1);
GetWatchdog().SetEnabled(false);
GetWatchdog().SetExpiration(1);
compressor.Start();
debug("Waiting to init CAN");
Wait(2);
Dlf = new CANJaguar(6,CANJaguar::kSpeed);
Dlb = new CANJaguar(3,CANJaguar::kSpeed);
Drf = new CANJaguar(7,CANJaguar::kSpeed);
Drb = new CANJaguar(2,CANJaguar::kSpeed);
arm1 = new CANJaguar(5);
arm1_sec = new CANJaguar(8);
arm2 = new CANJaguar(4);
EncArm.SetDistancePerPulse(.00025);
EncClaw.SetDistancePerPulse(.00025);
EncClaw.SetReverseDirection(false);
EncArm.SetReverseDirection(true);
EncArm.Reset();
EncClaw.Reset();
updateCAN.Start((int)this);
//cameraTask.Start((int)this);
EncArm.Start();
EncClaw.Start();
debug("done initing");
}
virtual void RobotInit() {
CommandBase::init();
mainCompressor = new Compressor(COMPRESSOR_PRESSURE_SWITCH, COMPRESSOR_RELAY);// COMPRESSOR_RELAY);
autonomousCommand = new cmdAutonomousScheduler(); //DEFINE COMMANDS HERE
mainCompressor->Start();
CommandBase::loaderSubsystem->LowerLoader();
CommandBase::winchSubsystem->Retract();
}
void TeleopPeriodic()
{
comp599->Start();
while(IsOperatorControl())
{
teleDrive();
}
}
virtual void RobotInit()
{
CommandBase::init();
SmartDashboard::init();
autonomousCommand = new AutonomousCommandGroup();
compressor = new Compressor(COMPRESSOR_SWITCH, COMPRESSOR_RELAY);
compressor->Start();
}
void RobotInit(void)
{
/* Once the start function is called no further programming
* is required. However if needed the Stop() function can be
* called.
* */
airCompressor->Start();
}
void RobotDemo::toggleCompressor(){
if(compressor->Enabled()){
compressor->Stop();
}
else{
compressor->Start();
}
}
/**
* Initialization code for test mode should go here.
*
* Use this method for initialization code which will be called each time
* the robot enters test mode.
*/
void RA14Robot::TestInit() {
myCam->Disable();
myCompressor->Start();
Config::LoadFromFile("config.txt");
Config::Dump();
myCamera->Set(Relay::kForward); // turn on light
}
void TeleopInit()
{
step = 0;
drive->setLinVelocity(0);
drive->setTurnSpeed(0, false);
drive->drive();
comp599->Start();
timer->Start();
}
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 AutonomousInit(void) {
printf("Robot autonomous initializing...\n");
GetWatchdog().Feed();
compressor->Start();
kicker->Reset();
printf("Robot autonomous initialization complete.\n");
}
// Code to be run during the remaining 2:20 of the match (after Autonomous())
//
// OperatorControl
// * Calls all the above methods
void OperatorControl()
{
// SAFETY AND SANITY - SET ALL TO ZERO
intake.Set(0.0);
rightWinch.Set(0.0);
leftWinch.Set(0.0);
arm.Set(DoubleSolenoid::kReverse);
/* TODO: Investigate. At least year's (GTR East) competition, we reached the conclusion that disabling this was
* the only way we could get out robot code to work (reliably). Should this be set to false?
*/
robotDrive.SetSafetyEnabled(false);
Timer clock;
int sanity = 0;
int bigSanity = 0;
loading = false;
loaded = winchSwitch.Get();
RegisterButtons();
gamepad.Update();
leftStick.Update();
compressor.Start();
while (IsOperatorControl() && IsEnabled())
{
clock.Start();
HandleDriverInputs();
HandleShooter();
HandleArm();
// HandleEject();
while (!clock.HasPeriodPassed(LOOP_PERIOD)); // add an IsEnabled???
clock.Reset();
sanity++;
if (sanity >= 100)
{
bigSanity++;
sanity = 0;
dsLCD->PrintfLine(DriverStationLCD::kUser_Line4, "%d", bigSanity);
}
gamepad.Update();
leftStick.Update();
dsLCD->UpdateLCD();
}
// SAFETY AND SANITY - SET ALL TO ZERO
intake.Set(0.0);
rightWinch.Set(0.0);
leftWinch.Set(0.0);
}
void toggleCompressor(bool start, bool stop)
{
if(start)
{
comp599->Start();
}
else if(stop)
{
comp599->Stop();
}
}
/**
* Initialization code for teleop mode should go here.
*
* Use this method for initialization code which will be called each time
* the robot enters teleop mode.
*/
void RobotDemo::TeleopInit() {
s1Status=true;
s2Status=true;
s3Status=true;
retract(s3,s3Status);
retract(s1,s1Status);
spinnerStatusLeft=true;
spinnerStatusRight=true;
compressor->Start();
return;
}
Anesthesiologist()
{
manipulator = new AnesthesiologistManipulator();
launcher = new AnesthesiologistLauncher(manipulator);
oi = new AnesthesiologistOperatorInterface();
drive = new AnesthesiologistDrive(oi);
comp599 = new Compressor(1, 1, 1, 2);
timer = new Timer();
oi->dashboard->init();
comp599->Start();
}
Medic()
{
drive = new MedicDrive();
manipulator = new MedicManipulator();
oi = new MedicOperatorInterface();
shooter = new MedicShooter();
pid = new MedicPIDOutput();
comp599 = new Compressor(1, 1);//TODO: add real values
comp599->Start();
}
Medic()
{
oi = new MedicOperatorInterface();
drive = new MedicDrive();
comp599 = new Compressor(1, 1, 1, 1);//TODO: check values
// rpmSensor = new Solenoid(PNEUMATICS_24V_SLOT, 5);
// rpmIRSensor = new DigitalInput(1, SHOOTER_WHEEL_IR_CHANNEL);
oi->dashboard->init();
comp599->Start();
timer = new Timer();
}
RobotDemo(void)
{
rightStick = new Joystick(2); // create the joysticks
leftStick = new Joystick(1);
turretStick = new Joystick(3);
ds = DriverStation::GetInstance();
SmartDashboard *Dash = SmartDashboard::GetInstance();
AutonomousContol = new AutonomousControllerClass(ds);
compressor = new Compressor(14,1); //Pressure Switch, Compressor Relay
compressor->Start();
Dash->PutInt("Initialized", 1);
}
void TeleopInit(void) {
printf("Robot teleop initializing...\n");
GetWatchdog().Feed();
compressor->Start();
kicker->SafeReset();
leftDrivetrainEncoder->Start();
rightDrivetrainEncoder->Start();
printf("Robot teleop initialization complete.\n");
}
void AutonomousInit() {
autoDelay = 20 * AUTO_DELAY;
if(!fout.is_open()) {
fout.open("logging.csv");
logheaders();
}
compressor->Start();
autoState = 0;
autoReady = false;
lc->setMode(2);
freshStart = false;
}
void OperatorControl(void)
{
myRobot.SetSafetyEnabled(true);
gamepad.EnableButton(BUTTON_COLLECTOR_FWD);
gamepad.EnableButton(BUTTON_COLLECTOR_REV);
gamepad.EnableButton(BUTTON_SHOOTER);
gamepad.EnableButton(BUTTON_CLAW_1_LOCKED);
gamepad.EnableButton(BUTTON_CLAW_2_LOCKED);
gamepad.EnableButton(BUTTON_CLAW_1_UNLOCKED);
gamepad.EnableButton(BUTTON_CLAW_2_UNLOCKED);
gamepad.EnableButton(BUTTON_STOP_ALL);
gamepad.EnableButton(BUTTON_JOG_FWD);
gamepad.EnableButton(BUTTON_JOG_REV);
stick2.EnableButton(BUTTON_SHIFT);
// Set inital states for all switches and buttons
gamepad.Update();
indexSwitch.Update();
greenClawLockSwitch.Update();
yellowClawLockSwitch.Update();
stick2.Update();
// Set initial states for all pneumatic actuators
shifter.Set(DoubleSolenoid::kReverse);
greenClaw.Set(DoubleSolenoid::kReverse);
yellowClaw.Set(DoubleSolenoid::kReverse);
compressor.Start ();
while (IsOperatorControl())
{
gamepad.Update();
stick2.Update();
indexSwitch.Update();
greenClawLockSwitch.Update();
yellowClawLockSwitch.Update();
HandleCollectorInputs();
HandleDriverInputsManual();
HandleArmInputs();
HandleShooterInputs();
HandleResetButton();
UpdateStatusDisplays();
dsLCD->UpdateLCD();
Wait(0.005); // wait for a motor update time
}
}
void Test(void)
{
compressor->Start();
myRobot.SetSafetyEnabled(true);
GetWatchdog().SetEnabled(true);
GetWatchdog().SetExpiration(1);
GetWatchdog().Feed();
while(IsTest())
{
GetWatchdog().Feed();
Wait(0.1);
}
}
void teleDrive()
{
drive->setLinVelocity(-oi->getDriveJoystick()->GetY(Joystick::kRightHand));
drive->setTurnSpeed(oi->getDriveJoystick()->GetX(Joystick::kRightHand), oi->getDriveJoystickButton(1));
drive->drive();
drive->shift(oi->getDriveJoystickButton(8), oi->getDriveJoystickButton(9));
if(oi->getDriveJoystickButton(6))
{
comp599->Start();
}
else if(oi->getDriveJoystickButton(7))
{
comp599->Stop();
}
}
void OperatorControl()
{
compressor.Start();
while (IsOperatorControl())
{
if(stick.GetRawButton(6)) //press the upper right trigger
{
piston.Set(true);
}
else if(stick.GetRawButton(8)) //press the lower right trigger
{
piston.Set(false);
}
}
compressor.Stop();
}
void TeleopInit() {
//Open logging
if(!fout.is_open()) {
fout.open("logging.csv");
logheaders();
}
cameraLight->Set(Relay::kOff);
myDrive->SetPID(KP,KI,KD);
myDrive->EnablePID();
manualAngle = 0;
compressor->Start();
myDrive->setGyroDrive(true);
lc->setMode(0);
freshStart = false;
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl()
{
comp->Start();
while(IsOperatorControl())
{
if(wasenabled == false && IsEnabled() == true)
{
//arm->initialize();
wasenabled = true;
//Wait(.5);
}
wasenabled = IsEnabled();
d->go();
//r->Set((r->kForward));
//ds->PrintfLine(ds->kUser_Line1, "button 11: %d " ,stick->GetRawButton(11));
//ds->PrintfLine(ds->kUser_Line2, "button 2: %d", stick->GetRawButton(2));
//arm->pickup();
if(stick->GetRawButton(7))
{
armlock->Set(armlock->kForward);
}
else
{
armlock->Set(armlock->kReverse);
}
ds->PrintfLine(ds->kUser_Line2, "button 7: %d", stick->GetRawButton(7));
//arm->shoot();
ds->UpdateLCD();
}
}
