diff --git a/Robot2016/src/Robot.cpp b/Robot2016/src/Robot.cpp index 3813ab6..5e25294 100644 --- a/Robot2016/src/Robot.cpp +++ b/Robot2016/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,24 +20,22 @@ 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) + 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; @@ -144,9 +195,42 @@ private: { 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. + */ + 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); + } + } - float power = (1.0 - operator_stick.GetThrottle()) / 2.0; - shooter.SetPower(power); + /** + * 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() diff --git a/Robot2016/src/Shooter.h b/Robot2016/src/Shooter.h index 8b7a833..c8980f5 100644 --- a/Robot2016/src/Shooter.h +++ b/Robot2016/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 StopShooter() + { + ready = true; + launch_spinny->Set(0); + launcher->Set(0); + pickup->Set(0); } - void PickUp(bool state = true) { - pickup->Set((float) (state * PICKUP_POWER)); + void LowerRamp() + { + launch_spinny->Set(-1); } - void SetRamp(RampState state) { - // TODO: - // Move the Ramp to the set position. + 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); - } else + state = SPINNINGUP; + launch_spinny->Set(-1); + launcher->Set(LAUNCH_POWER); + pickup->Set(LAUNCH_POWER); + shotClock.Reset(); + shotClock.Start(); + break; + } + case SPINNINGUP: { - ramp->Set(0); - rampHeight->Reset(); + if (shotClock.Get() > SPINUP_TIME) + { + state = LAUNCH; + shotClock.Reset(); + shotClock.Start(); + } else + { + 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; - - RampState rampState; - RampState targetState; - RampState previousState; + CANTalon *launch_spinny; + ShooterState state; + + Timer shotClock; + bool ready; }; #endif /* SRC_SHOOTER_H_ */