void AutonomousType4() {
SmartDashboard::PutString("STATUS:", "STARTING AUTO 4");
//Lift, turn, drive
chainLift.SetSpeed(0.5);
while (midPoint.Get() && maxUp.Get()) {
}
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Polar(0, 0, 0.3);
if (WaitF(2.5))
return;
robotDrive.MecanumDrive_Polar(0.25, 0, 0);
if (WaitF(5.6))
return;
robotDrive.MecanumDrive_Polar(0, 0, 0.3);
if (WaitF(2))
return;
robotDrive.MecanumDrive_Polar(0, 0, 0);
//chainLift.SetSpeed(-0.4);
//while (maxDown.Get() && IsAutonomous()) {
//}
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Polar(0, 0, 0);
SmartDashboard::PutString("STATUS:", "AUTO 4 COMPLETE");
}
//Choose which auto to use
void AutonomousInit() {
chainLift.SetSpeed(0);
canGrabber.SetSpeed(0);
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
SmartDashboard::PutString("STATUS:", "STARTING AUTO");
robotDrive.SetSafetyEnabled(false);
chainLift.SetSafetyEnabled(false);
SmartDashboard::PutBoolean("Auto switch A: ", autoSwitch1.Get());
SmartDashboard::PutBoolean("Auto switch B: ", autoSwitch2.Get());
//Select auto type
if (autoSwitch1.Get()) {
if (autoSwitch2.Get())
AutonomousType4();
else
//1 on 2 grab n back
AutonomousType8();
} else {
if (autoSwitch2.Get())
//1 off, 2 on: grab n turn
AutonomousType10();
else {
SmartAutoPicker();
}
//Do Nothing
}
}
//Grab first two and turn to go right
void AutonomousType10() {
SmartDashboard::PutString("STATUS:", "STARTING AUTO 10");
robotDrive.MecanumDrive_Cartesian(0, -0.2, 0);
if (WaitF(1.2))
return;
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(0.5);
while (IsAutonomous() && maxUp.Get() && midPoint.Get()) {
}
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Cartesian(0, 0.4, 0);
if (WaitF(1.6))
return;
robotDrive.MecanumDrive_Polar(0, 0, 0.3);
if (WaitF(2.6))
return;
robotDrive.MecanumDrive_Cartesian(0, -0.4, 0);
if (WaitF(1))
return;
robotDrive.MecanumDrive_Polar(0, 0, -0.3);
if (WaitF(2.6))
return;
robotDrive.MecanumDrive_Cartesian(0, -0.4, 0);
if (WaitF(1.6))
return;
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
SmartDashboard::PutString("STATUS:", "AUTO 10 COMPLETE");
}
void RobotDemo::dumb_climber_code()
{
//printf("Top:%i Bottom:%i " , top_claw_limit_switch->Get() , bottom_claw_limit_switch->Get());
if (drive_stick_prim ->GetRawButton(climber_hold_up))
{
if (top_claw_limit_switch->Get() == 1)
{
climbing_motor->Set(0.0);
//printf("STOPPED\n");
}
else
{
climbing_motor->Set(1);
//printf("GOING UP\n");
}
} // not climber hold up
else if (drive_stick_prim->GetRawButton(climber_hold_down))
{
if (bottom_claw_limit_switch->Get() == 0)
{
climbing_motor->Set(0.0);
//printf("STOPED\n");
}
else
{
climbing_motor->Set(-1);
//printf("GOING DOWN\n");
}
} // hold down button
else
{ // no js buttons pushed
climbing_motor->Set(0.0);
}
}
/**
* 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(void)
{
Victor armMotor(5); // create arm motor instance
while (IsOperatorControl())
{
GetWatchdog().Feed();
// determine if tank or arcade mode; default with no jumper is for tank drive
if (ds->GetDigitalIn(ARCADE_MODE) == 0) {
myRobot->TankDrive(leftStick, rightStick); // drive with tank style
} else{
myRobot->ArcadeDrive(rightStick); // drive with arcade style (use right stick)
}
// Control the movement of the arm using the joystick
// Use the "Y" value of the arm joystick to control the movement of the arm
float armStickDirection = armStick->GetY();
// if at a limit and telling the arm to move past the limit, don't drive the motor
if ((armUpperLimit->Get() == 0) && (armStickDirection > 0.0)) {
armStickDirection = 0;
} else if ((armLowerLimit->Get() == 0) && (armStickDirection < 0.0)) {
armStickDirection = 0;
}
// Set the motor value
armMotor.Set(armStickDirection);
}
}
/******************************************************************************
* Function: DisabledPeriodic
*
* Description: Run the functions that are expected during the period when the
* robot is disabled.
******************************************************************************/
void DisabledPeriodic()
{
Scheduler::GetInstance()->Run();
Light1.Set(1);
Light2.Set(1);
while(IsDisabled())
{
UpdateActuatorCmnds(0,0,false,false,false,false,false,false,false,0,0,0,0,0);
ReadAutonSwitch();
UpdateSmartDashboad(Sw1.Get(),
Sw2.Get(),
Sw3.Get(),
Sw4.Get(),
BlSw.Get(),
(double)AutonState,
(double)0,
ballarm.GetDistance(),
gyroOne.GetAngle(),
accel.GetX(),
accel.GetY(),
accel.GetZ(),
(double)0,
(double)0);
wait(kUpdatePeriod);
}
}
/*
* Get the value from a digital input channel.
* Retrieve the value of a single digital input channel from the FPGA.
*
* @param slot The slot the digital input module is plugged into
* @param channel The particular channel this digital input is using
*/
UINT32 GetDigitalInput(UINT32 slot, UINT32 channel)
{
DigitalInput *digIn = AllocateDigitalInput(slot, channel);
if (digIn)
return digIn->Get();
else
return 0;
}
/*
* Get the value from a digital input channel.
* Retrieve the value of a single digital input channel from the FPGA.
*
* @param slot The slot the digital input module is plugged into
* @param channel The particular channel this digital input is using
*/
UINT32 GetDigitalInput(UINT8 moduleNumber, UINT32 channel)
{
DigitalInput *digIn = AllocateDigitalInput(moduleNumber, channel);
if (digIn)
return digIn->Get();
else
return 0;
}
void Autonomous ()
{
DriveTrain.StartDriveTrain();
Shooter.StartShooterAuto();
AutonomousState = 1;
while(IsAutonomous())
{
if (AutonomousSwitch.Get() == 0)
{
if (AutonomousState == 1)
{
DriveTrain.RunDriveTrain(1, 1, 0, 0);
if ((DriveTrain.LeftEncTotal >= 2400) || (DriveTrain.RightEncTotal >= 2400))
{
AutonomousState = 2;
}
}
else if ((AutonomousState == 2) && (HotGoal == true))
{
Shooter.RunShooter(0, 1, 0);
}
}
else if (AutonomousSwitch.Get() == 1)
{
if (AutonomousState == 1)
{
DriveTrain.RunDriveTrain(1, 1, 0, 0);
if ((DriveTrain.LeftEncTotal >= 2400) || (DriveTrain.RightEncTotal >= 2400))
{
AutonomousState = 2;
}
}
else if ((AutonomousState == 2) && (WhichHotGoal != 0))
{
DriveTrain.RunDriveTrain(WhichHotGoal, -WhichHotGoal, 0, 0);
if (((WhichHotGoal == 1) && ((DriveTrain.LeftEncTotal >= 2800) || (DriveTrain.RightEncTotal <= 2000))) || ((WhichHotGoal == -1) && ((DriveTrain.LeftEncTotal <= 2000) || (DriveTrain.RightEncTotal >= 2800))))
{
AutonomousState = 3;
}
}
else if (AutonomousState == 3)
{
Shooter.RunShooter(0, 1, 0);
AutonomousState = 4;
}
else if (AutonomousState == 4)
{
DriveTrain.RunDriveTrain(-WhichHotGoal, WhichHotGoal, 0, 0);
if (((WhichHotGoal == 1) && ((DriveTrain.LeftEncTotal <= 2400) || (DriveTrain.RightEncTotal >= 2400))) || ((WhichHotGoal == -1) && ((DriveTrain.LeftEncTotal >= 2400) || (DriveTrain.RightEncTotal <= 2400))))
{
AutonomousState = 5;
}
}
}
}
}
void shootCatapult()
{
if (logitech.GetRawButton(2) && buttonOne.Get()==0 && buttonTwo.Get()==0)
{
dogSolenoid.Set(DoubleSolenoid::kReverse);
Wait(0.5);
ratchetSolenoid.Set(DoubleSolenoid::kReverse);
Wait(1);
}
}
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++;
}
void loadCatapult()
{
if (buttonOne.Get()==1 && buttonTwo.Get()==1 && dogSolenoid.Get()==DoubleSolenoid::kReverse)
{
dogSolenoid.Set(DoubleSolenoid::kForward);
Wait(0.5);
ratchetSolenoid.Set(DoubleSolenoid::kForward);
Wait(0.5);
catapultMotor.Set(1);
}
}
void autonomousCatapultRelease()
{
if ((leftLimitSwitch.Get()== 0 || rightLimitSwitch.Get()== 0) && winchMotor.Get() == 0)
{
stopDriving(); // Stops the drive so the robot doesn't flip on itself or something
winchMotor.Set(0); // Redundant line for extra safety that can be removed after testing (The winch should already be off)
dogSolenoid.Set(DoubleSolenoid::kReverse); // Brings the pneumatic piston backward to disengage the dog gear
Wait(0.2); // Giving the pistons time to disengage properly
ratchetSolenoid.Set(DoubleSolenoid::kReverse); // Brings the pneumatic piston backward to disengage the ratchet
Wait(5); // Waits 5 seconds after shooting before starting to load the catapult
}
}
//Grab first yellow, back up to auto zone, DON'T DROP
void AutonomousType12() {
SmartDashboard::PutString("STATUS:", "STARTING AUTO 12");
chainLift.SetSpeed(0.5);
while (IsAutonomous() && IsEnabled() && maxUp.Get() && midPoint.Get()) {
}
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Cartesian(0, 0.4, 0);
if (WaitF(3))
return;
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
SmartDashboard::PutString("STATUS:", "AUTO 12 COMPLETE");
}
void AutonomousType3() { //Grab a bin/trash bin, and move forward
SmartDashboard::PutString("STATUS:", "STARTING AUTO 3");
chainLift.SetSpeed(0.5);
while (midPoint.Get() && maxUp.Get()) {
}
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Cartesian(0, -0.75, 0);
if (WaitF(1.75))
return;
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(-0.5);
while (maxDown.Get()) {
}
chainLift.SetSpeed(0);
SmartDashboard::PutString("STATUS:", "AUTO 3 COMPLETE");
}
bool UpdateState(
float powerLevel, //typically 1.0 for forward -1.0 for backward
double targetPosition,
HookSwitch hookSwitch=NoHookSwitch)
{
if (powerLevel < 0.0 && !lowerLimitSwitch->Get()){
if (motorController) {
motorController->Disable();
}
motor->Set(0.0);
encoder->Reset();
encoder->Start();
return true;
}
double position = encoder->GetDistance();
if (motorController) {
if (!motorController->IsEnabled())
motorController->Enable();
motorController->SetSetpoint(targetPosition);
} else {
if (position < targetPosition-ClimberCloseTolerance) {
motor->Set(1.0);
} else if (position > targetPosition+ClimberCloseTolerance){
motor->Set(-1.0);
}
}
if ( (hookSwitch == UpperHookSwitch && !upperHookSwitch->Get())||
(hookSwitch == LowerHookSwitch && !lowerHookSwitch->Get())||
(hookSwitch == NoHookSwitch &&
targetPosition -ClimberCloseTolerance <= position && position <= targetPosition + ClimberCloseTolerance ) ) {
if (motorController) {
motorController->Disable();
}
motor->Set(0.0);
encoder->Start();
return true;
} else if (hookSwitch != NoHookSwitch &&
( (powerLevel > 0 && position > targetPosition) ||
(powerLevel < 0 && position < targetPosition) ) ) {
robot->AbortClimb("Grab did not occur when expected");
return false;
} // else if (hookSwitch != NoHookSwitch && Check motor power draw)
// abort here if looking for switch and started drawing a lot of power
return false;
}
void Test()
{
DriverStationLCD *screen = DriverStationLCD::GetInstance();
while (IsTest())
{
if ((buttonOne.Get()) == 1)
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Button1_Not Pressed!");
}
else
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Button1_Pressed!");
}
if ((buttonTwo.Get()) == 1)
{
screen -> PrintfLine(DriverStationLCD::kUser_Line2,"Button2_Not Pressed!");
}
else
{
screen -> PrintfLine(DriverStationLCD::kUser_Line2,"Button2_Pressed!");
}
if ((togglebuttonOne.Get()) == 0)
{
screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Left Is_On!");
}
else
{
screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Left Is_Off!");
}
if ((togglebuttonTwo.Get()) == 0)
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Right Is_On!");
}
else
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Right Is_Off!");
}
if (buttonOne.Get() == 0)
{
emergencyCompressor.Set(Relay::kOff);
}
#if 0
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"ButtonOneIs_%d", buttonOne.Get());
screen -> PrintfLine(DriverStationLCD::kUser_Line2,"toggle_%d", buttonTwo.Get());
screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Button_%d", togglebuttonOne.Get());
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"toggle_%d", togglebuttonTwo.Get());
#endif
Wait(0.005);
screen -> UpdateLCD();
}
}
void TeleopPeriodic(void)
{
Scheduler::GetInstance()->Run();
myDrive_all->TankDrive(drivestick_left->GetY(), drivestick_right->GetY());
ABCheck = Xbox_Button_A->Get(); // Gets the value of the A button on the xbox
BBCheck = Xbox_Button_B->Get(); // Gets the value of the B button on the xbox
if (ABCheck == 1) // If A button is pressed
{
FirstSolenoid->Set(DoubleSolenoid::kForward); // Make piston extend
}
if (BBCheck == 1) // If B button is pressed
{
FirstSolenoid->Set(DoubleSolenoid::kReverse); // Make piston retract
}
if ((ABCheck == 0) && (BBCheck == 0)) // If A and B buttons are not pressed
{
FirstSolenoid->Set(DoubleSolenoid::kOff); // Make piston stay where it is
}
XBCheck = Xbox_Button_X->Get(); // Gets value of the X button on the xbox
YBCheck = Xbox_Button_Y->Get(); // Get value of the Y button on the xbox
if (XBCheck == 1)//If X button is pressed
{
if (limitSwitch->Get() == 0) // If limitswitch is pressed
{
intake_Motor->Set(0); // Stop test motor
}
if (limitSwitch->Get() == 1) // If limitswitch is not pressed
{
intake_Motor->Set(1); // Test motor goes forward at 0.5 speed
}
}
if (YBCheck == 1) // If Y button is pressed
{
intake_Motor->Set(-1); // Test motor goes backward at -0.5 speed
}
if ((XBCheck == 0) && (YBCheck == 0)) // If X and Y buttons are not pressed
{
intake_Motor->Set(0); // Test motor stops
}
}
void AutonomousType2() { //Pick tote and bin, move to auto zone
SmartDashboard::PutString("STATUS:", "STARTING AUTO 2");
chainLift.SetSpeed(0.5);
while (midPoint.Get() && maxUp.Get()) {
}
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Polar(0.3, 0, 0);
if (WaitF(1.6))
return;
robotDrive.MecanumDrive_Polar(0, 0, 0);
chainLift.SetSpeed(-0.2);
if (WaitF(0.8))
return;
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Polar(-0.3, 0, 0);
if (WaitF(1.6))
return;
robotDrive.MecanumDrive_Polar(0, 0, 0);
chainLift.SetSpeed(-0.3);
while (maxDown.Get()) {
}
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Polar(0.2, 0, 0);
if (WaitF(2))
return;
robotDrive.MecanumDrive_Polar(0, 0, 0);
chainLift.SetSpeed(0.4);
while (midPoint.Get() && maxUp.Get()) {
}
chainLift.SetSpeed(0);
//turn 90 deg
robotDrive.MecanumDrive_Polar(0, 0, -0.3);
if (WaitF(4))
return;
robotDrive.MecanumDrive_Polar(0.5, 0, 0);
if (WaitF(2.5))
return;
robotDrive.MecanumDrive_Polar(0, 0, 0);
chainLift.SetSpeed(-0.4);
while (maxDown.Get() && IsAutonomous()) {
}
chainLift.SetSpeed(0);
SmartDashboard::PutString("STATUS:", "AUTO 2 COMPLETE");
}
void initRobot () {
cerr << "running init\n";
Dlf->EnableControl(0);
Dlb->EnableControl(0);
Drf->EnableControl(0);
Drb->EnableControl(0);
arm1->EnableControl();
arm1_sec->EnableControl();
arm2->EnableControl();
Dlf->ConfigEncoderCodesPerRev(250);
Dlf->SetPID(1,0,0);
Dlb->ConfigEncoderCodesPerRev(250);
Dlb->SetPID(1,0,0);
Drf->ConfigEncoderCodesPerRev(250);
Drf->SetPID(1,0,0);
Drb->ConfigEncoderCodesPerRev(250);
Drb->SetPID(1,0,0);
Wait(.1);
if(robotInted==false) {
int count=220;
arm2->Set(-.3);
while(count-->0 && LimitClaw.Get() == 1) Wait(.005);
arm2->Set(.15);
while(count-->0 && LimitClaw.Get() == 0) Wait(.005);
arm2->Set(0);
if(count>0)
EncClaw.Reset();
arm1->Set(-.3);
arm1_sec->Set(-.3);
while(count-->0 && LimitArm.Get() == 1) Wait(.005);
arm1->Set(.5);
arm1_sec->Set(.5);
while(count-->0 && LimitArm.Get() == 0) Wait(.005);
if(count>0)
EncArm.Reset();
arm1->Set(0);
arm1_sec->Set(0);
robotInted = true;
}
}
void TestPeriodic()
{
lw->Run();
if (launcherSensor->Get()) {
printf("on\n");
} else {
printf("off\n");
}
}
void ClimbPeriodic() {
displayCount++;
// This is the distance in inches the climber must be initially raised
static const float PullupPostion = -5.0f;
log->info("current state is: %s\n",ClimbStateString(climbState));
// depending on state, cont
switch (climbState) {
case NotInitialized: {
leftClimber->encoder->Start();
rightClimber->encoder->Start();
bar = LowerBar;
climber = NULL;
setClimbState(InitialGrab);
break; }
//when they set up the robot they have to set the climbers 6" above the bar
case InitialGrab: {
// Pulling both arms down enough to latch (low power consumption)
// We have several options here:
// 1. Move a specific distance and assume it is right
// 2. Move until the power goes up, and then check the encoders to see if the distance
// is plausible
// 3. Move until a limit HookSwitch inside the grip for each climber trips
bool leftDone = leftClimber->UpdateState(-1.0, PullupPostion);
bool rightDone = rightClimber->UpdateState(-1.0, PullupPostion);
startingState = false;
// moving climbers into initial position
if (leftDone && rightDone)
setClimbState(Abort);
break; }
case Abort: {
return;
}
default:
log->info("unexpected climber state!");
log->print();
}
log->info("LR %f %f",
leftClimber->encoder->GetDistance(),
rightClimber->encoder->GetDistance());
log->info("LRL %d %d %d %d %d %d %d",
leftClimber->lowerLimitSwitch->Get(),
leftClimber->lowerHookSwitch->Get(),
leftClimber->upperHookSwitch->Get(),
rightClimber->lowerLimitSwitch->Get(),
rightClimber->lowerHookSwitch->Get(),
rightClimber->upperHookSwitch->Get(),
loaderSwitch->Get());
log->print();
}
virtual void TeleopPeriodic() {
rightDrive->SetSpeed(-(Driver->GetRawAxis(2)));
leftDrive->SetSpeed((Driver->GetRawAxis(5)));
shooterFWD->SetSpeed(-(Operator->GetRawAxis(2)));
shooterRear->SetSpeed(-(Operator->GetRawAxis(2)));
//shoioter angle
if(Operator->GetRawButton(5))
{
cout<<"Relay 1 forward"<<endl;
shooterAngle->Set(Relay::kForward);
}
if(Operator->GetRawButton(6))
{
cout<<"Relay 1 Reverse"<<endl;
shooterAngle->Set(Relay::kReverse);
}
//Fire button
if(Operator->GetRawButton(1))
{
cout<<"Relay 1 forward"<<endl;
shooterFire->Set(Relay::kForward);
}
if(Operator->GetRawButton(2))
{
cout<<"Relay 1 Reverse"<<endl;
shooterFire->Set(Relay::kReverse);
}
if(CompressorSwitch->Get() == 0){
CompressorRelay->Set(Relay::kForward);
}else{
CompressorRelay->Set(Relay::kOff);
}
//if(canPDP == 0){
// cout << "NULL" << endl;
//}else{
//canPDP->GetVoltage(Voltage) ;
//cout << "0" << endl;
//}
}
/** Function to retrieve the user inputs
* Inputs (For all user input modes):
* * Potentiometers
* * Limit switches
* * the requested state for the robot (from the joystick buttons)
* For manual mode only:
* * The intake and ejection wheel speeds - based on the operator joystick throttle
* * The arm position - based on the operator joystick Y axis
*/
void GetUserInputs(RobotStates_t &requestedState)
{
float throttleRaw = 0.0;
//Read potentiometer goes from -5 to 28
potentiometerValueRight = armPotentiometer1.GetValue();
//potentiometerValueLeft = armPotentiometer2.GetValue();
ballCaughtLimitSwitch1 = limitSwitch1.Get(); //TODO 0 is no ball 1 is ball
//Read Joystick state buttons
requestedState = ReadJoystickButtons();
throttleRaw = DeadZone(Operator_Control_Stick.GetTwist(), EJWHEEL_MM_DEADZONE_VAL);
manualModeThrottlePosition = (throttleRaw * -1); //invert throttle value to match the joystick
manualModeArmPosition = DeadZone(Operator_Control_Stick.GetY(), ARM_MM_DEADZONE_VAL);
}
void Autonomous()
{
//float tiltAngleRad = drive->NavX->GetPitch() * (M_PIl/180);
while(IsAutonomous() && IsEnabled()){
drive->CalibrateNavX();
//if(drive->NavX->GetPitch() < 25 && drive->NavX->GetPitch() > -25){
if(auto2tote1Can->Get() == 0){//DIO 9
auton->Run2Tote1CanAuto(drive, lifter, pivotPiston);
}
else if(auto1Tote1Can->Get() == 0){//DIO 8
auton->Run1Tote1CanAuto(drive, lifter, pivotPiston);
}
else if(auto3ToteStack->Get() == 0){//DIO 7
auton->Run3Tote1CanAuto(drive, lifter, pivotPiston);
}
else if(autoDriveFwd->Get() == 0){//DIO 6
auton->RunDriveForward(drive);
}
else if(autoFastCan->Get() == 0){//DIO 5
auton->RunFast1Can(drive, lifter);
}
else{//Not Plugged In
auton->RunNothing(drive, lifter);
}
/*}
else if(drive->NavX->GetPitch() > 25){
drive->AutonDriveStraight(false, tiltAngleRad);
}
else if(drive->NavX->GetPitch() < -25){
drive->AutonDriveStraight(false, tiltAngleRad);
}
else{
auton->RunNothing(drive, lifter);
}*/
//printf("Angle: %f\n", drive->NavX->GetYaw());
}
}
void RobotDemo::printfs()
{
//printf("OUT LOOP\n");
if (cycle_counter >= 70)
{
printf("RPS Back:%f RPS Front:%f %f ", RPS_back, RPS_front,
desired_RPS_control);
printf("LS:%i %i ", top_claw_limit_switch->Get(),
bottom_claw_limit_switch->Get());
//printf("Climb:%f " , climbing_motor->Get());
//printf("Shooters:%f %f ", shooter_motor_front ->Get(),
//shooter_motor_back ->Get());
if (shooter_angle_1->Get())
{
printf("Up ");
}
else
{
printf("Down ");
}
if (arcadedrive)
{
printf("Arcade ");
}
else
{
printf("Tank ");
}
printf("\n");
cycle_counter = 0;
}
cycle_counter++;
}
/********************************** AUTONOMOUS MODE ******************************/
void AutonomousInit(void)
{
defaultSteeringGain = -0.4; // default value for steering gain
previousValue = 0;
oldTimeInSeconds = -1;
// set the straight vs forked path variables as read from the DS digital
// inputs or the I/O Setup panel on the driver station.
straightLine = StraightLineSwitch->Get();
stopTime = (straightLine) ? 2.0 : 4.0;
goLeft = GoLeftSwitch->Get();
atCross=false;
printf("StraightLine: %d\n", straightLine);
printf("GoingLeft: %d\n", goLeft);
// set up timer for 8 second max driving time and use the timer to
// pick values from the power profile arrays
autotimer->Start();
autotimer->Reset();
}
// WHen robot is enabled
void init() {
log->info("initializing");
log->print();
climber = NULL;
leftDriveEncoder->Reset();
leftDriveEncoder->SetDistancePerPulse(DriveDistancePerPulse);
leftDriveEncoder->Start();
rightDriveEncoder->Reset();
rightDriveEncoder->SetDistancePerPulse(DriveDistancePerPulse);
rightDriveEncoder->Start();
//leftClimber->motorController->Disable();
leftClimber->encoder->Reset();
leftClimber->encoder->Start();
//rightClimber->motorController->Disable();
rightClimber->encoder->Reset();
rightClimber->encoder->Start();
bcdValue = bcd->value();
loadSwitchOldState = loaderSwitch->Get();
#if 0
bool leftDone = false;
bool rightDone = false;
// Only do this for some BCD values?
while (!leftDone || !rightDone){
if (!leftDone)
leftDone = leftClimber->UpdateState(-1.0, -30);
if (!rightDone)
rightDone = rightClimber->UpdateState(-1.0, -30);
log->info("Wait: Ll Rl: %d %d",
leftClimber->lowerLimitSwitch->Get(),
rightClimber->lowerLimitSwitch->Get());
log->print();
}
#endif
climbState = NotInitialized;
cameraElevateAngle =
(cameraElevateMotor->GetMaxAngle()-cameraElevateMotor->GetMinAngle()) * 2/3;
cameraPivotAngle = 0;
cameraPivotMotor->SetAngle(cameraPivotAngle);
cameraElevateMotor->SetAngle(cameraElevateAngle);
loading = false;
loaderDisengageDetected = false;
//This is a rough guess of motor power it should be based on voltage
shooterMotorVolts = 8.0; // volts as a fraction of 12V
loadCount = 0;
}
void ReadDistance() {
//// digitalWrite(trigPin, LOW); Arduino code.
// triggerPin->Set(0);
// Code to pulse. We are using pulse instead.
// distTimer->Reset(); // delay of 2 microseconds
// while(distTimer->HasPeriodPassed(0.000002) == 0) {
// ;
// }
//
//// digitalWrite(trigPin, HIGH);
// triggerPin->Set(1);
//
// distTimer->Reset(); // delay of 10 microseconds
// while(distTimer->HasPeriodPassed(0.000010) == 0) {
// ;
// }
triggerPin->Pulse(0.000010);
//// digitalWrite(trigPin, LOW); Arduino code.
// triggerPin->Set(0);
// duration = pulseIn(echoPin, HIGH); Arduino code.
duration = 0;
distTimer = 0;
while(echoPin->Get() == 0 && distTimer<1000) {
// duration = distTimer->Get();
distTimer++;
duration++;
}
//Calculate the distance (in cm) based on the speed of sound.
distance = duration/58.2;
if (distance >= maximumRange || distance <= minimumRange){
/* Send a negative number to computer to indicate "out of range" */
val = -1;
}
else {
/* Send the distance to the computer to indicate successful reading. */
val = distance;
}
SmartDashboard::PutNumber("Distance is:", val);
}
void TeleopPeriodic()
{
SmartDashboard::PutNumber("joystickX",stick.GetX());
SmartDashboard::PutNumber("joystickY",stick.GetY());
//SmartDashboard::PutBoolean("f*****g buttons", stick.GetRawButton(1));
//SmartDashboard::PutNumber("potentiometer voltage", pot.GetVoltage());
SmartDashboard::PutBoolean("infra",infra.Get());
SmartDashboard::PutNumber("accelX",accel.GetX());
SmartDashboard::PutNumber("accelY",accel.GetY());
SmartDashboard::PutNumber("accelZ",accel.GetZ());
servo.Set(
trueMap(stick.GetX(), 1, -1, 1, 0) // trueMap allows use of entire joystick
);
SmartDashboard::PutNumber("servo", servo.Get());
jag1.Set(stick.GetY());
jag2.Set(stick.GetY());
//tal1.Set(stick.GetY());
SmartDashboard::PutNumber("jag1", jag1.Get());
SmartDashboard::PutNumber("jag2", jag2.Get());
/*SmartDashboard::PutNumber("encpos", enc.Get());
SmartDashboard::PutNumber("encspd", enc.GetRate());*/
if (stick.GetRawButton(1) && !actuatePressed) {
pistonVal=!pistonVal;
piston.Set(pistonVal ? DoubleSolenoid::kForward : DoubleSolenoid::kReverse);
actuatePressed = true;
}
else if (!stick.GetRawButton(1))
actuatePressed = false;
SmartDashboard::PutBoolean("piston forward", piston.Get() == DoubleSolenoid::kForward);
}
