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
2016-02-10 20:43:09 -05:00

169 lines
4.2 KiB
C++

#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.
#endif // BUTTON_LAYOUT
class Robot: public IterativeRobot
{
Talon left_drive, right_drive;
CANTalon shooter1, shooter2,
ramp;
RobotDrive drive;
Shooter shooter;
Joystick driver_stick, operator_stick;
float power;
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
ramp(12),
drive(&left_drive, &right_drive),
shooter( // initialize Shooter object.
&shooter1, &shooter2, &ramp),
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;
LiveWindow *lw = LiveWindow::GetInstance();
SendableChooser *chooser;
const std::string autoNameDefault = "Default";
const std::string autoNameCustom = "My Auto";
std::string autoSelected;
void LogData()
{
static PowerDistributionPanel pdp;
static DriverStation* ds = DriverStation::GetInstance();
}
void RobotInit()
{
chooser = new SendableChooser();
chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
SmartDashboard::PutData("Auto Modes", chooser);
ramp.Enable();
shooter1.Enable();
shooter2.Enable();
left_drive.SetInverted(true);
right_drive.SetInverted(true);
inverting = false;
}
/**
* 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()
{
shooter.CalibrateRamp();
shooter.SetRamp(Shoot);
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()
{
shooter.CalibrateRamp();
shooter.SetRamp(Shoot); // start in the full up position!
power = 0;
}
void TeleopPeriodic()
{
std::cout << "Ramp position: "<< ramp.GetEncPosition() << std::endl;
drive.ArcadeDrive(&driver_stick, true);
// This is shit code for testing. Replace it with real code.
if(operator_stick.GetRawButton(RAMP_RAISE))
{
ramp.Set(1);
}
else if(operator_stick.GetRawButton(RAMP_LOWER))
{
ramp.Set(-1);
}
else
{
ramp.Set(0);
}
if(operator_stick.GetRawButton(THUMB) && !pickupRunning)
{
shooter.PickUp();
pickupRunning = true;
}
else if(pickupRunning)
{
shooter.PickUp(false);
pickupRunning = false;
}
if(driver_stick.GetRawButton(THUMB) && !inverting)
{
left_drive.SetInverted(!left_drive.GetInverted());
right_drive.SetInverted(!right_drive.GetInverted());
inverting = true;
}
else if(!driver_stick.GetRawButton(THUMB))
{
inverting = false;
}
if(((1.0 - operator_stick.GetThrottle()) / 2.0) > power + 0.005||((1.0 - operator_stick.GetThrottle()) / 2.0) < power -0.005)
{
power = (1.0 - operator_stick.GetThrottle()) / 2.0;
shooter.SetPower(power);
}
}
void TestPeriodic()
{
lw->Run();
}
};
START_ROBOT_CLASS(Robot)