Merge branch 'shooter'
This commit is contained in:
Aidan Ferguson
commit
17bd06f9c2
132
src/Robot.cpp
132
src/Robot.cpp
@ -8,6 +8,11 @@
|
||||
#define THUMB 2 // Thumb button number
|
||||
#define RAMP_RAISE 3 // Button 3 for Raising Ramp
|
||||
#define RAMP_LOWER 4 // Button 4 to lower ramp.
|
||||
#define UNJAM 11
|
||||
|
||||
#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
|
||||
|
||||
@ -15,22 +20,20 @@ class Robot: public IterativeRobot
|
||||
{
|
||||
TalonSRX left_drive, right_drive;
|
||||
CANTalon shooter1, shooter2,
|
||||
ramp;
|
||||
launch_spinny;
|
||||
RobotDrive drive;
|
||||
Shooter shooter;
|
||||
Joystick driver_stick, operator_stick;
|
||||
Counter ramp_height;
|
||||
public:
|
||||
Robot():
|
||||
left_drive(0), // Left DriveTrain Talons plug into PWM channel 1 with a Y-spliter
|
||||
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(10), // shooter drive 1
|
||||
shooter2(11), // shooter drive 2
|
||||
ramp(12),
|
||||
shooter1(11), // shooter drive 1
|
||||
shooter2(10), // shooter drive 2
|
||||
launch_spinny(12),
|
||||
drive(&left_drive, &right_drive),
|
||||
ramp_height(),
|
||||
shooter( // initialize Shooter object.
|
||||
&shooter1, &shooter2, &ramp, &ramp_height),
|
||||
&shooter1, &shooter2, &launch_spinny),
|
||||
driver_stick(0), // right stick (operator)
|
||||
operator_stick(1) // left stick (driver)
|
||||
{
|
||||
@ -41,6 +44,10 @@ 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;
|
||||
float shooter_power;
|
||||
|
||||
LiveWindow *lw = LiveWindow::GetInstance();
|
||||
SendableChooser *chooser;
|
||||
@ -54,13 +61,17 @@ private:
|
||||
chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
|
||||
chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
|
||||
SmartDashboard::PutData("Auto Modes", chooser);
|
||||
ramp_height.SetUpSource(1);
|
||||
ramp.Enable();
|
||||
shooter1.Enable();
|
||||
shooter2.Enable();
|
||||
left_drive.SetInverted(true);
|
||||
right_drive.SetInverted(true);
|
||||
//launch_spinny.SetInverted(true);
|
||||
inverting = false;
|
||||
pickupRunning = false;
|
||||
ramping = false;
|
||||
shooting = false;
|
||||
unjamming = false;
|
||||
shooter_power = 0;
|
||||
|
||||
}
|
||||
|
||||
@ -76,9 +87,6 @@ private:
|
||||
*/
|
||||
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;
|
||||
@ -101,41 +109,84 @@ private:
|
||||
|
||||
void TeleopInit()
|
||||
{
|
||||
shooter.CalibrateRamp();
|
||||
shooter.SetRamp(Shoot); // start in the full up position!
|
||||
|
||||
}
|
||||
|
||||
void TeleopPeriodic()
|
||||
{
|
||||
//std::cout << "Ramp position: "<< launch_spinny.GetEncPosition() << std::endl;
|
||||
drive.ArcadeDrive(&driver_stick, true);
|
||||
|
||||
//bool rampDoing = false;
|
||||
// This is shit code for testing. Replace it with real code.
|
||||
if(operator_stick.GetRawButton(RAMP_RAISE))
|
||||
if(!ramping && operator_stick.GetRawButton(RAMP_RAISE))
|
||||
{
|
||||
ramp.Set(1);
|
||||
std::cout << "Raising Ramp.";
|
||||
//launch_spinny.Set(1);
|
||||
shooter.RaiseRamp();
|
||||
ramping =true;
|
||||
}
|
||||
else if(operator_stick.GetRawButton(RAMP_LOWER))
|
||||
else if(!ramping && operator_stick.GetRawButton(RAMP_LOWER))
|
||||
{
|
||||
ramp.Set(-1);
|
||||
std::cout << "Lowering Ramp.";
|
||||
//launch_spinny.Set(-1);
|
||||
shooter.LowerRamp();
|
||||
ramping = true;
|
||||
}
|
||||
else
|
||||
/*else if(!ramping && operator_stick.GetRawButton(TRIGGER))
|
||||
{
|
||||
ramp.Set(0);
|
||||
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(UNJAM))
|
||||
{
|
||||
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 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;
|
||||
}
|
||||
else if(pickupRunning)
|
||||
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)
|
||||
{
|
||||
std::cout << "Inverting Drive Train.";
|
||||
left_drive.SetInverted(!left_drive.GetInverted());
|
||||
right_drive.SetInverted(!right_drive.GetInverted());
|
||||
inverting = true;
|
||||
@ -145,8 +196,41 @@ private:
|
||||
inverting = false;
|
||||
}
|
||||
|
||||
float power = (1.0 - operator_stick.GetThrottle()) / 2.0;
|
||||
shooter.SetPower(power);
|
||||
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
if(operator_stick.GetRawButton(TRIGGER))
|
||||
{
|
||||
shooting = true;
|
||||
shooter.Shoot();
|
||||
}
|
||||
else if(shooting && !operator_stick.GetRawButton(TRIGGER))
|
||||
{
|
||||
shooting = false;
|
||||
shooter.StopShooter();
|
||||
}
|
||||
|
||||
// This code will become obsolete after the Shooter logic is complete.
|
||||
float opThrottle = SaneThrottle(operator_stick.GetThrottle());
|
||||
|
||||
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()
|
||||
|
||||
183
src/Shooter.h
183
src/Shooter.h
@ -9,77 +9,151 @@
|
||||
#define SRC_SHOOTER_H_
|
||||
|
||||
#define PICKUP_POWER 0.5
|
||||
#define RAMP_LOWER_DURATION 2 //Rotations.
|
||||
#define LAUNCH_POWER 1
|
||||
#define SPINUP_TIME 1.5 // seconds.
|
||||
#define LAUNCH_TIME 0.5
|
||||
|
||||
|
||||
/**
|
||||
* You can use the values assigned to each of these values as the number
|
||||
* of rotations to run the motor down from the "Shoot" position.
|
||||
*
|
||||
* This might not be the best way to do it, and also requires that we
|
||||
* figure out how to read the Hall Effect signal from the motor.
|
||||
*/
|
||||
enum RampState {
|
||||
Shoot = 0, // 0 rotations
|
||||
Half = (RAMP_LOWER_DURATION / 2), // 1 rotation?
|
||||
Down = RAMP_LOWER_DURATION, // 2 rotations?
|
||||
Uncalibrated = -1,
|
||||
Transitioning = -2
|
||||
};
|
||||
|
||||
class Shooter {
|
||||
class Shooter
|
||||
{
|
||||
public:
|
||||
|
||||
/**
|
||||
* Shooter talons and ramp talon.
|
||||
* Shooter talons and launch-spinny talon.
|
||||
* s2 is also for the pickup-mechanism and can be controlled independently.
|
||||
*
|
||||
*/
|
||||
Shooter(CANTalon *s1, CANTalon *s2, CANTalon *r, Counter *h) {
|
||||
shooterDrive = new RobotDrive(s1, s2);
|
||||
enum ShooterState
|
||||
{
|
||||
READY,
|
||||
ON_FIRE,
|
||||
SPINNINGUP,
|
||||
LAUNCH,
|
||||
LAUNCHING,
|
||||
RESETTING
|
||||
};
|
||||
|
||||
Shooter(CANTalon *s1, CANTalon *s2, CANTalon *r)
|
||||
{
|
||||
// shooterDrive = new RobotDrive(s1, s2);
|
||||
launcher = s1;
|
||||
pickup = s2;
|
||||
ramp = r;
|
||||
rampState = Uncalibrated;
|
||||
rampHeight = h;
|
||||
launch_spinny = r;
|
||||
ready = true;
|
||||
state = READY;
|
||||
}
|
||||
|
||||
/**
|
||||
* Call this method on TeleopInit so that the ramp is properly
|
||||
* set at the beginning of the match.
|
||||
*/
|
||||
RampState CalibrateRamp() {
|
||||
// TODO:
|
||||
// Raise ramp until limit switch is triggered,
|
||||
// then lower the ramp to its lower limit.
|
||||
|
||||
return Down;
|
||||
}
|
||||
|
||||
virtual ~Shooter() {
|
||||
delete shooterDrive;
|
||||
virtual ~Shooter()
|
||||
{
|
||||
delete launcher;
|
||||
delete pickup;
|
||||
delete ramp;
|
||||
delete rampHeight;
|
||||
delete launch_spinny;
|
||||
}
|
||||
|
||||
void PickUp(bool state = true) {
|
||||
pickup->Set((float) (state * PICKUP_POWER));
|
||||
void StopShooter()
|
||||
{
|
||||
ready = true;
|
||||
launch_spinny->Set(0);
|
||||
launcher->Set(0);
|
||||
pickup->Set(0);
|
||||
}
|
||||
|
||||
void SetRamp(RampState state) {
|
||||
// TODO:
|
||||
// Move the Ramp to the set position.
|
||||
void LowerRamp()
|
||||
{
|
||||
launch_spinny->Set(-1);
|
||||
}
|
||||
|
||||
void RaiseRamp()
|
||||
{
|
||||
launch_spinny->Set(1);
|
||||
}
|
||||
|
||||
void StopRamp()
|
||||
{
|
||||
launch_spinny->Set(0);
|
||||
}
|
||||
|
||||
void BoostRamp()
|
||||
{
|
||||
launch_spinny->Set(1);
|
||||
}
|
||||
|
||||
void Shoot()
|
||||
{
|
||||
// TODO: Shooter Logic should go as follows:
|
||||
|
||||
/*
|
||||
Assuming a ball is held in the shooter. When the trigger is pulled,
|
||||
the launch-spinny should be STOPPED. Start a Timer object counting
|
||||
When... too hard to write.. I emailed you a flow chart.
|
||||
*/
|
||||
switch (state)
|
||||
{
|
||||
case Shoot:
|
||||
if (ramp->GetForwardLimitOK())
|
||||
case READY:
|
||||
{
|
||||
ramp->Set(1);
|
||||
state = SPINNINGUP;
|
||||
launch_spinny->Set(-1);
|
||||
launcher->Set(LAUNCH_POWER);
|
||||
pickup->Set(LAUNCH_POWER);
|
||||
shotClock.Reset();
|
||||
shotClock.Start();
|
||||
break;
|
||||
}
|
||||
case SPINNINGUP:
|
||||
{
|
||||
if (shotClock.Get() > SPINUP_TIME)
|
||||
{
|
||||
state = LAUNCH;
|
||||
shotClock.Reset();
|
||||
shotClock.Start();
|
||||
} else
|
||||
{
|
||||
ramp->Set(0);
|
||||
rampHeight->Reset();
|
||||
std::cout << "*Goku noises*\n";
|
||||
}
|
||||
break;
|
||||
}
|
||||
case LAUNCH:
|
||||
{
|
||||
launch_spinny->Set(LAUNCH_POWER);
|
||||
state = LAUNCHING;
|
||||
break;
|
||||
}
|
||||
case LAUNCHING:
|
||||
{
|
||||
if (shotClock.Get() > LAUNCH_TIME)
|
||||
{
|
||||
|
||||
state = RESETTING;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case RESETTING:
|
||||
{
|
||||
launch_spinny->Set(0);
|
||||
launcher->Set(0);
|
||||
pickup->Set(0);
|
||||
state = READY;
|
||||
break;
|
||||
}
|
||||
case ON_FIRE:
|
||||
{
|
||||
std::cout << "Something is wrong with the launch sequence.\n";
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void PickUp(bool state = true)
|
||||
{
|
||||
pickup->Set((float) (state * PICKUP_POWER));
|
||||
launcher->Set((float) (state * PICKUP_POWER * -1));
|
||||
//launch_spinny->Set(-1.0*PICKUP_POWER);
|
||||
//std::cout << "picking up!\n";
|
||||
}
|
||||
|
||||
/**
|
||||
@ -90,19 +164,22 @@ public:
|
||||
pickup->Set(-1 * PICKUP_POWER);
|
||||
}
|
||||
|
||||
void SetPower(float power) {
|
||||
shooterDrive->TankDrive(power, -power, false);
|
||||
void SetPower(float power)
|
||||
{
|
||||
pickup->Set(power);
|
||||
launcher->Set(power);
|
||||
//std::cout << "setting shooter power" << std::endl;
|
||||
}
|
||||
private:
|
||||
|
||||
RobotDrive *shooterDrive;
|
||||
//RobotDrive *shooterDrive;
|
||||
CANTalon *launcher;
|
||||
CANTalon *pickup;
|
||||
CANTalon *ramp;
|
||||
Counter *rampHeight;
|
||||
CANTalon *launch_spinny;
|
||||
ShooterState state;
|
||||
|
||||
RampState rampState;
|
||||
RampState targetState;
|
||||
RampState previousState;
|
||||
Timer shotClock;
|
||||
bool ready;
|
||||
};
|
||||
|
||||
#endif /* SRC_SHOOTER_H_ */
|
||||
|
||||
Reference in New Issue
Block a user