commit 01c462c0c6ca04685b1a608d2bcef0c237df729d Author: Jason Date: Tue Feb 9 16:23:45 2016 -0500 Added new Robot project using PWM drive train... Much simpler... Signed-off-by: Jason diff --git a/.cproject b/.cproject new file mode 100644 index 0000000..09f9db3 --- /dev/null +++ b/.cproject @@ -0,0 +1,283 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..3df573f --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +/Debug/ diff --git a/.project b/.project new file mode 100644 index 0000000..a419617 --- /dev/null +++ b/.project @@ -0,0 +1,27 @@ + + + Robot2016 + + + + + + org.eclipse.cdt.managedbuilder.core.genmakebuilder + + + + + org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder + full,incremental, + + + + + + org.eclipse.cdt.core.cnature + org.eclipse.cdt.core.ccnature + org.eclipse.cdt.managedbuilder.core.managedBuildNature + org.eclipse.cdt.managedbuilder.core.ScannerConfigNature + edu.wpi.first.wpilib.plugins.core.nature.FRCProjectNature + + diff --git a/.settings/org.eclipse.cdt.managedbuilder.core.prefs b/.settings/org.eclipse.cdt.managedbuilder.core.prefs new file mode 100644 index 0000000..a3c3365 --- /dev/null +++ b/.settings/org.eclipse.cdt.managedbuilder.core.prefs @@ -0,0 +1,13 @@ +eclipse.preferences.version=1 +environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.cross.exe.debug.1104744751/CPATH/delimiter=; +environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.cross.exe.debug.1104744751/CPATH/operation=remove +environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.cross.exe.debug.1104744751/CPLUS_INCLUDE_PATH/delimiter=; +environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.cross.exe.debug.1104744751/CPLUS_INCLUDE_PATH/operation=remove +environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.cross.exe.debug.1104744751/C_INCLUDE_PATH/delimiter=; +environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.cross.exe.debug.1104744751/C_INCLUDE_PATH/operation=remove +environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.cross.exe.debug.1104744751/append=true +environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.cross.exe.debug.1104744751/appendContributed=true +environment/buildEnvironmentLibrary/cdt.managedbuild.config.gnu.cross.exe.debug.1104744751/LIBRARY_PATH/delimiter=; +environment/buildEnvironmentLibrary/cdt.managedbuild.config.gnu.cross.exe.debug.1104744751/LIBRARY_PATH/operation=remove +environment/buildEnvironmentLibrary/cdt.managedbuild.config.gnu.cross.exe.debug.1104744751/append=true +environment/buildEnvironmentLibrary/cdt.managedbuild.config.gnu.cross.exe.debug.1104744751/appendContributed=true diff --git a/build.properties b/build.properties new file mode 100644 index 0000000..f51b8ca --- /dev/null +++ b/build.properties @@ -0,0 +1,11 @@ +# Build information +out=FRCUserProgram +src.dir=src +build.dir=build +out.exe=Debug/${out} + +# Simulation +simulation.world.file=/usr/share/frcsim/worlds/GearsBotDemo.world + +# Use the current C++ library by default +cpp-version=current diff --git a/build.xml b/build.xml new file mode 100644 index 0000000..82e7940 --- /dev/null +++ b/build.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Robot.cpp b/src/Robot.cpp new file mode 100644 index 0000000..a2093bc --- /dev/null +++ b/src/Robot.cpp @@ -0,0 +1,127 @@ +#include "../wpilib/WPILib.h" +#include "Shooter.h" + +#ifndef BUTTON_LAYOUT +#define BUTTON_LAYOUT + +#define TRIGGER 1 // I have no idea what the right button number is... +#define THUMB 2 + +#endif // BUTTON_LAYOUT + +class Robot: public IterativeRobot +{ + Talon left_drive; + Talon right_drive; + CANTalon shooter1; + CANTalon shooter2; + CANTalon ramp; + RobotDrive drive; + Shooter shooter; + Joystick driver_stick, operator_stick; +public: + Robot(): + left_drive(1), // Left DriveTrain Talons plug into PWM channel 1 with a Y-spliter + right_drive(2), // Right DriveTrain Talons plug // left wheel 2 + shooter1(10), // shooter drive 1 + shooter2(11), // 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: + 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); + } + + + /** + * 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() + { + if(autoSelected == autoNameCustom){ + //Custom Auto goes here + } else { + //Default Auto goes here + } + } + + void TeleopInit() + { + + } + + void TeleopPeriodic() + { + drive.ArcadeDrive(&driver_stick); + + if(operator_stick.GetRawButton(3)) + { + ramp.Set(1); + } + else if(operator_stick.GetRawButton(4)) + { + ramp.Set(-1); + } + else + { + ramp.Set(0); + } + + if(operator_stick.GetRawButton(THUMB)) + { + shooter.PickUp(); + } + else + { + shooter.PickUp(false); + } + + + float power = (1.0 - operator_stick.GetThrottle()) / 2.0; + shooter.SetPower(power); + } + + void TestPeriodic() + { + lw->Run(); + } +}; + +START_ROBOT_CLASS(Robot) diff --git a/src/Shooter.h b/src/Shooter.h new file mode 100644 index 0000000..6971715 --- /dev/null +++ b/src/Shooter.h @@ -0,0 +1,90 @@ +/* + * Shooter.h + * + * Created on: Feb 2, 2016 + * Author: Jason + */ + +#ifndef SRC_SHOOTER_H_ +#define SRC_SHOOTER_H_ + +#define PICKUP_POWER 0.5 +#define RAMP_LOWER_DURATION 2 //Rotations. + + +/** + * 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 = 1, // 1 rotation? + Down = 2, // 2 rotations? + Uncalibrated = -1 +}; + +class Shooter { +public: + /** + * Shooter talons and ramp talon. + * s2 is also for the pickup-mechanism and can be controlled independently. + * + */ + Shooter(CANTalon *s1, CANTalon *s2, CANTalon *r) { + shooterDrive = new RobotDrive(s1, s2); + pickup = s2; + ramp = r; + rampState = Uncalibrated; + } + + /** + * 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; + delete pickup; + delete ramp; + } + + void PickUp(bool state = true) { + pickup->Set((float) (state * PICKUP_POWER)); + } + + void SetRamp(RampState state) { + // TODO: + // Move the Ramp to the set position. + } + + /** + * Call this to run the pickup backwards if the ball gets jammed somehow... + */ + void Unjam() + { + pickup->Set(-1 * PICKUP_POWER); + } + + void SetPower(float power) { + shooterDrive->TankDrive(power, -power, false); + } +private: + + RobotDrive *shooterDrive; + CANTalon *pickup; + CANTalon *ramp; + + RampState rampState; +}; + +#endif /* SRC_SHOOTER_H_ */