diff --git a/src/Robot.cpp b/src/Robot.cpp index 5e9beaf..8d96f0b 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -8,14 +8,20 @@ #include #include #include +#include #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. +#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.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 @@ -25,14 +31,15 @@ class Robot: public IterativeRobot { - Talon left_drive, right_drive; + TalonSRX left_drive, right_drive; CANTalon shooter1, shooter2, - ramp; + ramp, + arms; RobotDrive drive; Shooter shooter; Joystick driver_stick, operator_stick; //DriverStation DriverStation::GetInstance(); - float power; + //float power; public: Robot(): left_drive(0), // Left DriveTrain Talons plug into PWM channel 1 with a Y-splitter @@ -40,13 +47,13 @@ public: 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) + operator_stick(1) // left stick (driver) { - //DriverStation::GetInstance().GetInstance(); } @@ -54,6 +61,11 @@ 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; + float shooter_power; LiveWindow *lw = LiveWindow::GetInstance(); SendableChooser *chooser; @@ -81,12 +93,18 @@ private: 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); + //ramp.SetInverted(true); inverting = false; + pickupRunning = false; + ramping = false; + shooting = false; + unjamming = false; + arming = false; + shooter_power = 0; } @@ -102,9 +120,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; @@ -128,44 +143,89 @@ private: void TeleopInit() { - shooter.CalibrateRamp(); - shooter.SetRamp(Shoot); // start in the full up position! - power = 0; + } void TeleopPeriodic() { LogCSVData(); - std::cout << "Ramp position: "<< ramp.GetEncPosition() << std::endl; - drive.ArcadeDrive(&driver_stick, true); + 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); + //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."; + //ramp.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."; + //ramp.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(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(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 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(THUMB) && !inverting) { + std::cout << "Inverting Drive Train."; left_drive.SetInverted(!left_drive.GetInverted()); right_drive.SetInverted(!right_drive.GetInverted()); inverting = true; @@ -174,14 +234,59 @@ private: { inverting = false; } - - if(((1.0 - operator_stick.GetThrottle()) / 2.0) > power + 0.005||((1.0 - operator_stick.GetThrottle()) / 2.0) < power -0.005) + + + /* + * 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)) { - power = (1.0 - operator_stick.GetThrottle()) / 2.0; - shooter.SetPower(power); + 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(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); + } + + // 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() { diff --git a/src/Shooter.h b/src/Shooter.h index ccb3c9b..82e4ace 100644 --- a/src/Shooter.h +++ b/src/Shooter.h @@ -8,89 +8,156 @@ #ifndef SRC_SHOOTER_H_ #define SRC_SHOOTER_H_ -#define PICKUP_POWER 0.8 -#define RAMP_LOWER_DURATION 2 //Rotations. +#define PICKUP_POWER 0.75 +#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) { + 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; + 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() { + virtual ~Shooter() + { delete launcher; delete pickup; delete ramp; } - - void PickUp(bool state = true) { - pickup->Set((float) (state * PICKUP_POWER)); - std::cout << "picking up!\n"; + + void StopShooter() + { + ready = true; + ramp->Set(0); + launcher->Set(0); + pickup->Set(0); } - void SetRamp(RampState state) { - // TODO: - // Move the Ramp to the set position. + void LowerRamp() + { + ramp->Set(-0.5); + if(ramp->Limits::kReverseLimit){ + SmartDashboard::PutNumber("ramp", 2); //going to put a circlar dial to show where the ramp could be + } else { + SmartDashboard::PutNumber("ramp", 1); + } + } + + void RaiseRamp() + { + ramp->Set(0.5); + if(ramp->Limits::kForwardLimit){ + SmartDashboard::PutNumber("ramp", 0); //going to put a circlar dial to show where the ramp could be + } else { + SmartDashboard::PutNumber("ramp", 1); + } + } + + void StopRamp() + { + ramp->Set(0); + } + 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: + case READY: { - if (ramp->GetForwardLimitOK()) + state = SPINNINGUP; + ramp->Set(-1); + launcher->Set(PICKUP_POWER); + pickup->Set(PICKUP_POWER); + shotClock.Reset(); + shotClock.Start(); + break; + } + case SPINNINGUP: + { + if (shotClock.Get() > SPINUP_TIME) { - ramp->Set(1); + state = LAUNCH; + shotClock.Reset(); + shotClock.Start(); } else { - ramp->Set(0); + std::cout << "*Goku noises*\n"; } + break; } - case Half: + case LAUNCH: { - //yeah, put something here when you get the encoder working. - std::cout << "Hey! Didja install an encoder? Because this is placeholder that Aidan would have removed if you had installed one and told him about it.\n"; + ramp->Set(1); + state = LAUNCHING; + break; } - case Down: + case LAUNCHING: { - //see half. - std::cout << "Hey! Didja install an encoder? Because this is placeholder that Aidan would have removed if you had installed one and told him about it.\n"; + if (shotClock.Get() > LAUNCH_TIME) + { + + state = RESETTING; + } + break; + } + case RESETTING: + { + ramp->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 * -0.75)); + //ramp->Set(-1.0*PICKUP_POWER); + //std::cout << "picking up!\n"; } /** @@ -98,24 +165,31 @@ public: */ void Unjam() { - pickup->Set(-1 * PICKUP_POWER); + pickup->Set(PICKUP_POWER * -0.75); } - void SetPower(float power) { + void ShootLow() + { + pickup->Set(-1); + } + + void SetPower(float power) + { pickup->Set(power); launcher->Set(power); - std::cout << "setting shooter power" << std::endl; + //std::cout << "setting shooter power" << std::endl; } private: - RobotDrive *shooterDrive; + //RobotDrive *shooterDrive; CANTalon *launcher; CANTalon *pickup; CANTalon *ramp; - - RampState rampState; - RampState targetState; - RampState previousState; + ShooterState state; + + Timer shotClock; + bool ready; + int fake_position; }; #endif /* SRC_SHOOTER_H_ */