void Drive (float speed, int dist)
{
leftDriveEncoder.Reset();
leftDriveEncoder.Start();
int reading = 0;
dist = abs(dist);
// The encoder.Reset() method seems not to set Get() values back to zero,
// so we use a variable to capture the initial value.
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "initial=%d\n", leftDriveEncoder.Get());
dsLCD->UpdateLCD();
// Start moving the robot
robotDrive.Drive(speed, 0.0);
while ((IsAutonomous()) && (reading <= dist))
{
reading = abs(leftDriveEncoder.Get());
dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "reading=%d\n", reading);
dsLCD->UpdateLCD();
}
robotDrive.Drive(0.0, 0.0);
leftDriveEncoder.Stop();
}
void OperatorControl(void) {
GetWatchdog().SetEnabled(true);
dsLCD->Clear();
dsLCD->UpdateLCD();
driveControl.initialize();
pneumaticsControl->initialize();
shooterControl->initialize();
while (IsOperatorControl() && IsEnabled()) {
GetWatchdog().Feed();
driveControl.run();
shooterControl->run();
dsLCD->UpdateLCD();
Wait(0.005); // wait for a motor update time
}
pneumaticsControl->disable();
}
void OperatorControl()
{
// Loop counter to ensure that the program is running (debug helper
// that can be removed when things get more stable)
int sanity, bigSanity = 0;
gamepad.Update();
while (IsOperatorControl() && IsEnabled())
{
controls = Controls::GetInstance();
controls->SetSpeed(LEFT_DRIVE_PWM, -1.0 * gamepad.GetRightY());
controls->SetSpeed(RIGHT_DRIVE_PWM, -1.0 * gamepad.GetRightY());
gamepad.Update();
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 Test Fix");
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "Teleop Mode");
dsLCD->PrintfLine(DriverStationLCD::kUser_Line6, "bigSanity: %d", sanity);
dsLCD->UpdateLCD();
sanity++;
if (0 == sanity % 20)
{
bigSanity++;
}
Wait(0.05); // wait for a motor update time
}
}
void TeleopPeriodic(void)
{
//Drive code
leftFrontDrive->Set(-1 * leftStick->GetY());
rightFrontDrive->Set(rightStick->GetY());
leftRearDrive->Set(-1 * leftStick->GetY());
rightRearDrive->Set(rightStick->GetY());
//airCompressor->Start();
// Simple Button Drive Forward
if(rightStick->GetRawButton(3))
{
leftFrontDrive->Set(-1);
rightFrontDrive->Set(1);
leftRearDrive->Set(-1);
rightRearDrive->Set(1);
}
// Simple Button Drive Backwards
if(rightStick->GetRawButton(2))
{
leftFrontDrive->Set(1);
rightFrontDrive->Set(-1);
leftRearDrive->Set(1);
rightRearDrive->Set(-1);
}
// Code to print to the user messages box in the Driver Station
LCD->PrintfLine(DriverStationLCD::kUser_Line1, "Hello World");
LCD->Printf(DriverStationLCD::kUser_Line2,1,"Y Left: %f", leftStick->GetY());
LCD->Printf(DriverStationLCD::kUser_Line3,1,"Y Right: %f", rightStick->GetY());
LCD->UpdateLCD();
Wait(0.2);
}
RobotDemo(void):
myRobot(LEFT_DRIVE_PWM, RIGHT_DRIVE_PWM), // these must be initialized in the same order
stick(1), // as they are declared above.
stick2(2),
gamepad(3),
collectorMotor(PICKUP_PWM),
indexerMotor(INDEX_PWM),
shooterMotor(SHOOTER_PWM),
armMotor (ARM_PWM),
shifter(SHIFTER_A,SHIFTER_B),
greenClaw(CLAW_1_LOCKED, CLAW_1_UNLOCKED),
yellowClaw(CLAW_2_LOCKED, CLAW_2_UNLOCKED),
potentiometer(ARM_ROTATION_POT),
indexSwitch(INDEXER_SW),
compressor(COMPRESSOR_PRESSURE_SW, COMPRESSOR_SPIKE)
{
m_collectorMotorRunning = false;
m_shooterMotorRunning = false;
dsLCD = DriverStationLCD::GetInstance();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 " NAME);
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, __DATE__ " "__TIME__);
dsLCD->UpdateLCD();
myRobot.SetExpiration(0.1);
shifter.Set(DoubleSolenoid::kReverse);
}
DriverStationLCDTextExample()
{
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
control = new LCDControl();
dsLCD->UpdateLCD();
}
void Autonomous(void)
{
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 Test Fix");
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "Autonomous Mode");
dsLCD->UpdateLCD();
}
// Runs during test mode
// Test
// *
void Test()
{
shifters.Set(DoubleSolenoid::kForward);
leftDriveEncoder.Start();
leftDriveEncoder.Reset();
int start = leftDriveEncoder.Get();
while (IsTest()) {
if (rightStick.GetRawButton(7)) {
robotDrive.ArcadeDrive(rightStick.GetY(), -rightStick.GetX());
}
else {
robotDrive.ArcadeDrive(rightStick.GetY()/2, -rightStick.GetX()/2);
}
if (gamepad.GetEvent(4) == kEventClosed) {
start = leftDriveEncoder.Get();
}
dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "lde: %d", leftDriveEncoder.Get() - start);
dsLCD->UpdateLCD();
gamepad.Update();
}
}
void output (void)
{
REDRUM;
if (IsAutonomous())
driverOut->PrintfLine(DriverStationLCD::kUser_Line1, "blaarag");
else if (IsOperatorControl())
{
REDRUM;
}
outputCounter++;
if (outputCounter % 30 == 0){
REDRUM;
driverOut->PrintfLine(DriverStationLCD::kUser_Line2, "Top Shooter RPM: %f",(float)LTop.GetSpeed());
driverOut->PrintfLine(DriverStationLCD::kUser_Line3, "Bot Shooter RPM: %f",(float)LBot.GetSpeed());
// driverOut->PrintfLine(DriverStationLCD::kUser_Line6, "Pilot Z-Axis: %f",pilot.GetZ());
}
if (outputCounter % 60 == 0){
REDRUM;
driverOut->PrintfLine(DriverStationLCD::kUser_Line4, "Top CANJag Temp: %f Celcius",LTop.GetTemperature()*(9/5) + 32);
driverOut->PrintfLine(DriverStationLCD::kUser_Line5, "Bot CANJag Temp: %f Celcius",LBot.GetTemperature()*(9/5) + 32);
outputCounter = 1;
}
driverOut->UpdateLCD();
}//nom nom nom
void LcdDisplaySubsystem::UpdateLines()
{
DriverStationLCD *lcd = DriverStationLCD::GetInstance();
lcd->PrintfLine(DriverStationLCD::kUser_Line1, "Tilt angle: %f", CommandBase::shooterTiltSubsystem->GetAngle());
lcd->UpdateLCD();
}
void AutonomousStateMachine() {
pneumaticsControl->initialize();
shooterControl->initializeAuto();
driveControl.initializeAuto();
enum AutoState {
AutoDrive, AutoSetup, AutoShoot
};
AutoState autoState;
autoState = AutoDrive;
bool feederState = false;
bool hasFired = false;
Timer feeder;
while (IsAutonomous() && IsEnabled()) {
GetWatchdog().Feed();
switch (autoState) {//Start of Case Machine
case AutoDrive://Drives the robot to set Destination
bool atDestination = driveControl.autoPIDDrive2();
if (atDestination) {//If at Destination, Transition to Shooting Setup
autoState = AutoSetup;
}
break;
case AutoSetup://Starts the ballgrabber motors to place the ball and extends ballgrabber
if (!pneumaticsControl->ballGrabberIsExtended()) {//extends ballgrabber if not already extended
pneumaticsControl->ballGrabberExtend();
}
if (!feederState) {//Started the feeder timer once
feederState = true;
feeder.Start();
autoState = AutoShoot;
}
break;
case AutoShoot://Shoots the ball
if (feeder.Get() < 2.0) {//Runs ballgrabber for 2.0 Seconds at most
shooterControl->feed(true);
} else if (feeder.Get() >= 2.0) {//Transition to Shooting
shooterControl->feed(false);
feeder.Stop();
}
if (pneumaticsControl->ballGrabberIsExtended() && !hasFired) {
shooterControl->autoShoot();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1,
"The robot is(should be) shooting");
if (shooterControl->doneAutoFire()) {//Returns true only after shoot is done firing
hasFired = true;
}
} else if (hasFired) {//runs only after shoot is done
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1,
"AutoMode Finished");
}
break;
}
dsLCD->UpdateLCD();
}
}
void RobotInit(void)
{
DriverStationLCD *dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
dsLCD->UpdateLCD();
//blnShift = true;
}
void Machine :: TeleopInit()
{
DriverStationLCD *lcd = DriverStationLCD::GetInstance();
lcd->PrintfLine(DriverStationLCD::kUser_Line2, "Ben is here");
lcd->UpdateLCD();
drive.enableVoltageControl();
//drive.enableSpeedControl();
}
void Zed::updateDriverStation()
{
DriverStationLCD* lcd = DriverStationLCD::GetInstance();
lcd->Clear();
lcd->PrintfLine(DriverStationLCD::kUser_Line1, 0, "Shooter Speed: %f",
shooterSpeed);
lcd->UpdateLCD();
}
void DriverstationMessages::SendTextLines()
{
DriverStationLCD *lcd =DriverStationLCD::GetInstance();
for(size_t i = 0; i < LENGTH(text);i++){
lcd->Printf((DriverStationLCD::Line)i, 1, "%s", (char *)text[i]);
}
lcd->UpdateLCD();
}
RobotDemo(void) :
signal(3), signalControlVoltage(7) {
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "SonarTest");
dsLCD->UpdateLCD();
}
RobotDemo(void):
myRobot(1, 2), // these must be initialized in the same order
stick(1) // as they are declared above.
{
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "XboxController2");
dsLCD->UpdateLCD();
myRobot.SetExpiration(0.1);
}
Hohenheim(void) {
driverStation = DriverStation::GetInstance();
dsLCD = DriverStationLCD::GetInstance();
pneumaticsControl = PneumaticsControl::getInstance();
shooterControl = ShooterControl::getInstance();
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "Hohenheim 2014 V 3.1");
dsLCD->UpdateLCD();
GetWatchdog().SetEnabled(false);
}
/**
* Runs the motors under driver control with either tank or arcade steering selected
* by a jumper in DS Digin 0. Also an arm will operate based on a joystick Y-axis.
*/
void OperatorControl()
{
control->initialize();
while (IsOperatorControl())
{
control->run();
dsLCD->UpdateLCD();
Wait(0.005);
}
}
void RobotDemo::DriverLCD()
{
if (cycle_counter >= 50)
{
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "RPS Back:%f ",
RPS_back);
dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "RPS Front:%f ",
RPS_front);
dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "RPS DRPS:%f ",
desired_RPS_control);
#if 0
if (shooter_fire_piston_A->Get())
{
dsLCD->Printf(DriverStationLCD::kUser_Line4, 1,"Fire ");
}
else
{
dsLCD->Printf(DriverStationLCD::kUser_Line4, 1,"Retracting... ");
}
#endif
//dsLCD->Printf(DriverStationLCD::kUser_Line4, 1,"TopLS:%i BotLS:%i ", top_claw_limit_switch->Get(),
// bottom_claw_limit_switch ->Get());
if (top_claw_limit_switch->Get())
{
dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "!TOP");
}
else if (!bottom_claw_limit_switch->Get())
{
dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "!BOTTOM");
}
else
{
dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "Neither");
}
if (shooter_angle_1->Get())
{
dsLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Up ");
}
else
{
dsLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Down ");
}
if (arcadedrive)
{
dsLCD->Printf(DriverStationLCD::kUser_Line6, 1, "Arcade ");
}
else
{
dsLCD->Printf(DriverStationLCD::kUser_Line6, 1, "Tank ");
}
dsLCD->UpdateLCD();
//cycle_counter = 0;
}
//cycle_counter++;
}
// 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);
}
/**
* Drive left & right motors for 2 seconds, enabled by a jumper (jumper
* must be in for autonomous to operate).
*/
void Autonomous()
{
control->initializeAuto();
while (IsAutonomous())
{
control->runAuto();
dsLCD->UpdateLCD();
Wait(0.005);
}
}
void
DriverMessages::SendTextLines()
{
DriverStationLCD *lcd =DriverStationLCD::GetInstance();
for(int i = 0; i < 3;i++){
lcd->Printf((DriverStationLCD::Line)i, 1, "%s", (char *)lineText[i]);
}
lcd->UpdateLCD();
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void) {
while (IsOperatorControl()) {
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "Voltage: %f",
signal.GetVoltage());
dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "CVoltage: %f",
signalControlVoltage.GetVoltage());
dsLCD->UpdateLCD();
Wait(0.005); // wait for a motor update time
}
}
void DashBoardInput() {
int i = 0;
GetWatchdog().SetEnabled(true);
while (IsAutonomous() && IsEnabled()) {
i++;
GetWatchdog().Feed();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "%f, %i",
driverStation->GetAnalogIn(1), i);
dsLCD->UpdateLCD();
}
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
myRobot.SetSafetyEnabled(true);
while (IsOperatorControl())
{
myRobot.ArcadeDrive(stick.getAxisLeftY(), stick.getAxisLeftX()); // drive with arcade style (use left stick)
dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "Move: %f4", stick.getAxisLeftY());
dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "Rotate: %f4", stick.getAxisLeftX());
dsLCD->UpdateLCD();
Wait(0.005); // wait for a motor update time
}
}
void DriveThenShootAuto() {
//initizlizes all parts of robot
pneumaticsControl->initialize();
shooterControl->initializeAuto();
driveControl.initializeAuto();
bool destinationPrevious = false;
bool autoShot = false; //true if autoShoot
//creates a timer for the ball grabber motors
Timer feeding;
bool started = false;
while (IsAutonomous() && IsEnabled()) {
GetWatchdog().Feed();
//drives forward to shooting point
bool atDestination = destinationPrevious || driveControl.autoPIDDrive2(); //autoDrive returns true when robot reached it's goal
if (atDestination) {
// The robot has reached the destination on the floor and is now ready to open and shoot
if (!started) {
started = true;
destinationPrevious = true;
//starts feeding-timer controls feeder motors so the ball doesn't get stuck
feeding.Start();
}
pneumaticsControl->ballGrabberExtend();
//waits for feeding to be done
if (feeding.Get() < 2.0) {//3.0 was
shooterControl->feed(true);
} else if (feeding.Get() >= 2.0) { // 3.0 was
shooterControl->feed(false);
feeding.Stop();
}
if (pneumaticsControl->ballGrabberIsExtended() && !autoShot) {
shooterControl->autoShoot();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1,
"The robot is(should be) shooting");
if (shooterControl->doneAutoFire()) {//works only after shoot is done firing
autoShot = true;
}
} else if (autoShot) {//runs only after shoot is done
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1,
"AutoMode Finished");
}
}
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "Feeder Time: %f",
feeding.Get());
dsLCD->UpdateLCD();
}
}
SebastianRobot(void) {
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "Sebastian V24.2");
dsLCD->UpdateLCD();
GetWatchdog().SetEnabled(false);
led0 = new Relay(2);
led1 = new Relay(3);
flashCount = 0;
led0->Set(Relay::kOff);
led1->Set(Relay::kOff);
isFlashing=false;
}
//should move this to helper function
void robot::feed()
{
DriverStationLCD *lcd = DriverStationLCD::GetInstance();
lcd->Clear();
float th = gyro.getangle();
a = qmod(th * dt + a, -180, 180);
lcd->PrintfLine(DriverStationLCD::kUser_Line1, "BUILD: %i", BUILD);
lcd->PrintfLine(DriverStationLCD::kUser_Line2, "%f", gyrob.GetAngle());
lcd->PrintfLine(DriverStationLCD::kUser_Line3, "%f", arma.GetVoltage());
lcd->PrintfLine(DriverStationLCD::kUser_Line4, "%f", armb.GetVoltage());
//lcd->PrintfLine(DriverStationLCD::kUser_Line5, "%f", aa);
lcd->UpdateLCD();
}
RobotDemo(void)
{
motor = new Jaguar(9);
stick = new Joystick(1);
compressor = new Compressor(1, 1);
valve = new Solenoid(8);
// Construct the dashboard sender object used to send hardware state
// to the driver station
// dds = new DashboardDataSender();
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Plyboy test code: 6:46PM 1/2/2012");
dsLCD->UpdateLCD();
}
