333 lines
8.5 KiB
C++
333 lines
8.5 KiB
C++
#include "WPILib.h"
|
|
#include "Shooter.h"
|
|
#include <math.h>
|
|
|
|
#ifndef BUTTON_LAYOUT
|
|
#define BUTTON_LAYOUT
|
|
|
|
#define TRIGGER 1 // Trigger button number
|
|
#define THUMB 2 // Thumb button number
|
|
#define RAMP_RAISE 5 // Button 3 for Raising Ramp
|
|
#define RAMP_LOWER 3 // Button 4 to lower ramp.
|
|
#define UNJAM 11
|
|
|
|
#define DEADZONE_RADIUS 0.05 // 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.
|
|
|
|
#define TURN_FACTOR 1.5 // Left(x,y) = y*(1 + TF*x) : x < 0
|
|
// = y : x >= 0
|
|
// Right(x,y) = y : x < 0
|
|
// = y*(1 - TF*x) : x >= 0
|
|
|
|
#endif // BUTTON_LAYOUT
|
|
|
|
class Robot: public IterativeRobot
|
|
{
|
|
TalonSRX left_drive, right_drive;
|
|
CANTalon shooter1, shooter2,
|
|
ramp,
|
|
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
|
|
ramp(12),
|
|
arms(13),
|
|
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;
|
|
bool ramping;
|
|
bool shooting;
|
|
bool unjamming;
|
|
bool arming;
|
|
bool arcade;
|
|
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);
|
|
//ramp.SetInverted(true);
|
|
inverting = false;
|
|
pickupRunning = false;
|
|
ramping = false;
|
|
shooting = false;
|
|
unjamming = false;
|
|
arming = false;
|
|
shooter_power = 0;
|
|
arcade = 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()
|
|
{
|
|
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()
|
|
{
|
|
|
|
}
|
|
|
|
void TeleopInit()
|
|
{
|
|
|
|
}
|
|
|
|
void TeleopPeriodic()
|
|
{
|
|
std::cout << "arm encoder position: " << arms.GetEncPosition() << std::endl;
|
|
std::cout << "arm encoder velocity: " << arms.GetEncVel() << std::endl;
|
|
drive.ArcadeDrive(-driver_stick.GetY(), -driver_stick.GetX()*0.75);
|
|
if(driver_stick.GetRawButton(7))
|
|
{
|
|
arcade = true;
|
|
}
|
|
if(driver_stick.GetRawButton(8))
|
|
{
|
|
arcade = false;
|
|
}
|
|
if (arcade)
|
|
{
|
|
drive.ArcadeDrive(driver_stick);
|
|
}
|
|
else
|
|
{
|
|
if (driver_stick.GetRawButton(THUMB))
|
|
{
|
|
float left = driver_stick.GetTwist();
|
|
float right = -driver_stick.GetTwist();
|
|
drive.TankDrive(left, right);
|
|
}
|
|
else
|
|
{
|
|
UpdateDrive();
|
|
}
|
|
}
|
|
//bool rampDoing = false;
|
|
// This is shit code for testing. Replace it with real code.
|
|
if(!ramping && operator_stick.GetRawButton(RAMP_RAISE))
|
|
{
|
|
std::cout << "Raising Ramp.";
|
|
//ramp.Set(1);
|
|
shooter.RaiseRamp();
|
|
ramping =true;
|
|
}
|
|
else if(!ramping && operator_stick.GetRawButton(RAMP_LOWER))
|
|
{
|
|
std::cout << "Lowering Ramp.";
|
|
//ramp.Set(-1);
|
|
shooter.LowerRamp();
|
|
ramping = true;
|
|
}
|
|
/*else if(!ramping && operator_stick.GetRawButton(TRIGGER))
|
|
{
|
|
shooter.BoostRamp();
|
|
ramping = true;
|
|
}*/
|
|
else if(ramping
|
|
&& !operator_stick.GetRawButton(RAMP_LOWER)
|
|
&& !operator_stick.GetRawButton(RAMP_RAISE))
|
|
{
|
|
std::cout << "Stopping Ramp.";
|
|
shooter.StopRamp();
|
|
ramping = false;
|
|
}
|
|
|
|
|
|
if(!unjamming && operator_stick.GetRawButton(UNJAM))
|
|
{
|
|
unjamming = true;
|
|
shooter.Unjam();
|
|
}
|
|
else if(!unjamming && operator_stick.GetRawButton(TRIGGER))
|
|
{
|
|
shooter.ShootLow();
|
|
unjamming = true;
|
|
}
|
|
else if(unjamming && !operator_stick.GetRawButton(UNJAM) && !operator_stick.GetRawButton(TRIGGER))
|
|
{
|
|
shooter.PickUp(false);
|
|
unjamming = false;
|
|
}
|
|
/*
|
|
* 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 iteration 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;
|
|
}
|
|
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 iteration 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(TRIGGER) && !inverting)
|
|
{
|
|
std::cout << "Inverting Drive Train.";
|
|
left_drive.SetInverted(!left_drive.GetInverted());
|
|
right_drive.SetInverted(!right_drive.GetInverted());
|
|
inverting = true;
|
|
}
|
|
else if(!driver_stick.GetRawButton(TRIGGER))
|
|
{
|
|
inverting = false;
|
|
}
|
|
|
|
|
|
/*
|
|
* Unlike the previous actions, this method does need to be called every iteration 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.
|
|
*/
|
|
/*if(operator_stick.GetRawButton(TRIGGER))
|
|
{
|
|
shooting = true;
|
|
shooter.Shoot();
|
|
}
|
|
else if(shooting && !operator_stick.GetRawButton(TRIGGER))
|
|
{
|
|
shooting = false;
|
|
shooter.StopShooter();
|
|
}*/
|
|
|
|
if(!arming && driver_stick.GetRawButton(RAMP_RAISE))
|
|
{
|
|
arming = true;
|
|
arms.Set(0.5);
|
|
}
|
|
else if(!arming && driver_stick.GetRawButton(RAMP_LOWER))
|
|
{
|
|
arming = true;
|
|
arms.Set(-0.5);
|
|
}
|
|
else if(arming && !driver_stick.GetRawButton(RAMP_RAISE) && !driver_stick.GetRawButton(RAMP_LOWER))
|
|
{
|
|
arming = false;
|
|
arms.Set(0);
|
|
}
|
|
|
|
// This code will become obsolete after the Shooter logic is complete.
|
|
float opThrottle = SaneThrottle(operator_stick.GetThrottle());
|
|
|
|
if(!pickupRunning && !unjamming)
|
|
{
|
|
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();
|
|
}
|
|
|
|
void SimpleDrive()
|
|
{
|
|
float x = -driver_stick.GetX();
|
|
float y = -driver_stick.GetY();
|
|
float left = 0;
|
|
float right = 0;
|
|
|
|
if (x > 0)
|
|
{
|
|
right = y;
|
|
left = (1- x*TURN_FACTOR)*y ;
|
|
}
|
|
else
|
|
{
|
|
left = y;
|
|
right = (1+x*TURN_FACTOR)*y;
|
|
}
|
|
|
|
drive.TankDrive(left, right);
|
|
}
|
|
|
|
void UpdateDrive()
|
|
{
|
|
float x = -driver_stick.GetX();
|
|
float y = -driver_stick.GetY();
|
|
if (x > 0)
|
|
{
|
|
float right = y * SaneThrottle(driver_stick.GetThrottle());
|
|
float left = (1-x)*y * SaneThrottle(driver_stick.GetThrottle());
|
|
drive.TankDrive(left, right);
|
|
}
|
|
else
|
|
{
|
|
float left = y * SaneThrottle(driver_stick.GetThrottle());
|
|
float right = (1+x)*y * SaneThrottle(driver_stick.GetThrottle());
|
|
drive.TankDrive(left, right);
|
|
}
|
|
}
|
|
};
|
|
|
|
START_ROBOT_CLASS(Robot)
|