This repository has been archived on 2020-09-21. You can view files and clone it, but cannot push or open issues or pull requests.
FRC2016/src/Robot.cpp

264 lines
7.2 KiB
C++
Raw Normal View History

#include "WPILib.h"
#include "Shooter.h"
#ifndef BUTTON_LAYOUT
#define BUTTON_LAYOUT
#define TRIGGER 1 // Trigger button number
#define THUMB 2 // Thumb button number
#define RAMP_RAISE 3 // Button 3 for Raising Ramp
#define RAMP_LOWER 4 // Button 4 to lower ramp.
2016-02-13 12:00:28 -05:00
#define UNJAM 11
2016-02-13 11:26:07 -05:00
#define DEADZONE_RADIUS 0.01 // Deadzone Radius prevents tiny twitches in the joystick's value from
// affecting the robot. Use this for cleaning up drive train and shooter.
// Also used for detecting changes in an axis' value.
#endif // BUTTON_LAYOUT
class Robot: public IterativeRobot
{
2016-02-09 19:47:17 -05:00
Talon left_drive, right_drive;
CANTalon shooter1, shooter2,
2016-02-13 16:44:57 -05:00
launch_spinny,
arms;
RobotDrive drive;
Shooter shooter;
Joystick driver_stick, operator_stick;
public:
Robot():
left_drive(0), // Left DriveTrain Talons plug into PWM channel 1 with a Y-splitter
right_drive(1), // Right DriveTrain Talons plug // left wheel 2
shooter1(11), // shooter drive 1
shooter2(10), // shooter drive 2
2016-02-11 17:17:42 -05:00
launch_spinny(12),
2016-02-13 16:44:57 -05:00
arms(13),
drive(&left_drive, &right_drive),
shooter( // initialize Shooter object.
2016-02-11 17:17:42 -05:00
&shooter1, &shooter2, &launch_spinny),
driver_stick(0), // right stick (operator)
operator_stick(1) // left stick (driver)
{
}
private:
// instance variables
bool pickupRunning; // don't want to spam the Talon with set messages. Toggle the pickup when a button is pressed or released.
bool inverting;
bool ramping;
bool shooting;
2016-02-13 12:00:28 -05:00
bool unjamming;
2016-02-13 16:44:57 -05:00
bool arming;
float shooter_power;
LiveWindow *lw = LiveWindow::GetInstance();
SendableChooser *chooser;
const std::string autoNameDefault = "Default";
const std::string autoNameCustom = "My Auto";
std::string autoSelected;
void RobotInit()
{
chooser = new SendableChooser();
chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
SmartDashboard::PutData("Auto Modes", chooser);
shooter1.Enable();
shooter2.Enable();
left_drive.SetInverted(true);
right_drive.SetInverted(true);
//launch_spinny.SetInverted(true);
inverting = false;
pickupRunning = false;
ramping = false;
shooting = false;
2016-02-13 12:00:28 -05:00
unjamming = false;
2016-02-13 16:44:57 -05:00
arming = false;
2016-02-11 16:51:13 -05:00
shooter_power = 0;
}
/**
* 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
}
}
void AutonomousPeriodic()
{
if(autoSelected == autoNameCustom){
//Custom Auto goes here
} else {
//Default Auto goes here
}
}
void TeleopInit()
{
2016-02-11 16:51:13 -05:00
}
void TeleopPeriodic()
{
std::cout << "arm encoder position: " << arms.GetEncPosition() << std::endl;
std::cout << "arm encoder velocity: " << arms.GetEncVel() << std::endl;
drive.ArcadeDrive(&driver_stick, true);
//bool rampDoing = false;
// This is shit code for testing. Replace it with real code.
if(!ramping && operator_stick.GetRawButton(RAMP_RAISE))
{
2016-02-13 11:26:07 -05:00
std::cout << "Raising Ramp.";
2016-02-12 19:37:36 -05:00
//launch_spinny.Set(1);
shooter.RaiseRamp();
ramping =true;
}
else if(!ramping && operator_stick.GetRawButton(RAMP_LOWER))
{
2016-02-13 11:26:07 -05:00
std::cout << "Lowering Ramp.";
2016-02-12 19:37:36 -05:00
//launch_spinny.Set(-1);
shooter.LowerRamp();
ramping = true;
2016-02-12 19:37:36 -05:00
}
2016-02-13 11:26:07 -05:00
/*else if(!ramping && operator_stick.GetRawButton(TRIGGER))
2016-02-12 19:37:36 -05:00
{
shooter.BoostRamp();
ramping = true;
2016-02-13 11:26:07 -05:00
}*/
2016-02-13 12:00:28 -05:00
else if(ramping
&& !operator_stick.GetRawButton(RAMP_LOWER)
&& !operator_stick.GetRawButton(RAMP_RAISE))
{
2016-02-13 11:26:07 -05:00
std::cout << "Stopping Ramp.";
shooter.StopRamp();
ramping = false;
}
2016-02-12 19:37:36 -05:00
2016-02-13 12:00:28 -05:00
if(!unjamming && operator_stick.GetRawButton(UNJAM))
{
unjamming = true;
shooter.Unjam();
}
else if(unjamming && !operator_stick.GetRawButton(UNJAM))
{
shooter.PickUp(false);
unjamming = false;
}
2016-02-12 19:37:36 -05:00
/*
* Run the Shooter only while the THUMB button is held down on the operator stick.
* the 'pickupRunning' boolean is there to prevent the shooter from calling PickUp
* every itteration of the TeleopPeriodic method (once every 10ms!)
* The pickup is disabled when the thumb button is released, but the code still
* has 'pickupRunning' as true.
*/
if(operator_stick.GetRawButton(THUMB) && !pickupRunning)
{
shooter.PickUp();
pickupRunning = true;
}
2016-02-12 21:17:45 -05:00
else if(!operator_stick.GetRawButton(THUMB) && pickupRunning)
{
shooter.PickUp(false);
pickupRunning = false;
}
/*
* The 'inverting' variable is used to make sure that the drive train isn't getting
* inverted every itteration of the TeleopPeriodic method while the button is held down.
* This is important because the TeleopPeriodic method executes something like once every 10ms.
* Thus, this if-else if pair make the button a toggle.
*/
if(driver_stick.GetRawButton(THUMB) && !inverting)
{
2016-02-13 12:09:08 -05:00
std::cout << "Inverting Drive Train.";
left_drive.SetInverted(!left_drive.GetInverted());
right_drive.SetInverted(!right_drive.GetInverted());
inverting = true;
}
else if(!driver_stick.GetRawButton(THUMB))
{
inverting = false;
}
/*
* Unlike the previous actions, this method does need to be called every itteration of
* TeleopPeriodic. This is because the logic running this operation needs to be checked
* Every time the method is called. This cannot be a loop in the Shoot method because
* that would lock the robot every time the trigger is hit.
*/
2016-02-13 11:26:07 -05:00
if(operator_stick.GetRawButton(TRIGGER))
{
shooting = true;
shooter.Shoot();
}
2016-02-13 12:00:28 -05:00
else if(shooting && !operator_stick.GetRawButton(TRIGGER))
{
shooting = false;
shooter.StopShooter();
}
2016-02-13 16:47:57 -05:00
if(!arming && driver_stick.GetRawButton(RAMP_RAISE))
{
arming = true;
arms.Set(1);
}
else if(!arming && driver_stick.GetRawButton(RAMP_LOWER))
{
arming = true;
arms.Set(-1);
}
else if(arming && !driver_stick.GetRawButton(RAMP_RAISE) && !driver_stick.GetRawButton(RAMP_LOWER))
{
arming = false;
arms.Set(0);
}
2016-02-13 16:44:57 -05:00
// This code will become obsolete after the Shooter logic is complete.
float opThrottle = SaneThrottle(operator_stick.GetThrottle());
2016-02-12 21:17:45 -05:00
if(!pickupRunning && ( opThrottle > shooter_power + DEADZONE_RADIUS
|| opThrottle < shooter_power - DEADZONE_RADIUS))
{
shooter.SetPower(opThrottle);
}
}
/**
* Takes the gross raw throttle input from joystick and returns a
* value between 0.0-1.0 (no negative values)
*/
float SaneThrottle(float rawThrottle)
{
return ((1.0 - rawThrottle) / 2.0);
}
void TestPeriodic()
{
lw->Run();
}
};
START_ROBOT_CLASS(Robot)