diff --git a/.cproject b/.cproject
deleted file mode 100644
index 09f9db3..0000000
--- a/.cproject
+++ /dev/null
@@ -1,283 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/.gitignore b/.gitignore
deleted file mode 100644
index 3df573f..0000000
--- a/.gitignore
+++ /dev/null
@@ -1 +0,0 @@
-/Debug/
diff --git a/.project b/.project
deleted file mode 100644
index 69c1953..0000000
--- a/.project
+++ /dev/null
@@ -1,28 +0,0 @@
-
-
- Robot2016
-
-
-
-
-
- org.eclipse.cdt.managedbuilder.core.genmakebuilder
- clean,full,incremental,
-
-
-
-
- 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/Makefile b/Makefile
deleted file mode 100644
index 514df4d..0000000
--- a/Makefile
+++ /dev/null
@@ -1,35 +0,0 @@
-LIBS=wpi
-CXX=arm-frc-linux-gnueabi-g++
-override CPPFLAGS +=-std=c++14
-LDFLAGS=-l$(LIBS)
-TEAM=1786
-SSH_OPTIONS=-o UserKnownHostsFile=/dev/null -o StrictHostKeyChecking=no
-SSH_SSHPASS=$(shell command -v sshpass >/dev/null 2>&1 && echo -n "sshpass -p ''")
-SSH_HOST=roborio-1786-frc.local
-
-all: deploy
-
-build: FRCUserProgram
-
-FRCUserProgram: src/Robot.cpp src/Shooter.o
- @echo "Building FRCUserProgram"
- $(CXX) $(CPPFLAGS) $^ -o $@ $(LDFLAGS)
-
-clean:
- rm FRCUserProgram src/Shooter.o
-
-deploy: build
- @echo "Copying FRCUserProgram"
- ssh $(SSH_OPTIONS) lvuser@$(SSH_HOST) 'rm -f /home/lvuser/FRCUserProgram'
- scp $(SSH_OPTIONS) -o "LogLevel QUIET" FRCUserProgram lvuser@$(SSH_HOST):/home/lvuser/FRCUserProgram
- @echo "Restarting FRCUserProgram"
- $(SSH_SSHPASS) ssh $(SSH_OPTIONS) admin@$(SSH_HOST) '. /etc/profile.d/natinst-path.sh; /usr/local/frc/bin/frcKillRobot.sh -t -r'
-
-restart: FRCUserProgram
- echo "Restarting FRCUserProgram"
- $(SSH_SSHPASS) ssh $(SSH_OPTIONS) admin@$(SSH_HOST) '. /etc/profile.d/natinst-path.sh; /usr/local/frc/bin/frcKillRobot.sh -t -r'
-
-stop:
- @echo "Restarting FRCUserProgram"
- $(SSH_SSHPASS) ssh $(SSH_OPTIONS) admin@$(SSH_HOST) '. /etc/profile.d/natinst-path.sh; /usr/local/frc/bin/frcKillRobot.sh -t'y
-
diff --git a/build.properties b/build.properties
deleted file mode 100644
index f51b8ca..0000000
--- a/build.properties
+++ /dev/null
@@ -1,11 +0,0 @@
-# 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
deleted file mode 100644
index 82e7940..0000000
--- a/build.xml
+++ /dev/null
@@ -1,28 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/src/Robot.cpp b/src/Robot.cpp
deleted file mode 100644
index 9454ca2..0000000
--- a/src/Robot.cpp
+++ /dev/null
@@ -1,587 +0,0 @@
-#include "WPILib.h"
-#include "Shooter.h"
-#include
-#include
-#include
-#include
-#include
-#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 5 // Button 3 for Raising Ramp
-#define RAMP_LOWER 3 // Button 4 to lower ramp.
-#define UNJAM 11
-#define GIM_UP 7
-#define GIM_DOWN 9
-#define GIM_ZERO 10
-#define GIM_SHOOT 8
-
-#define GIMB_DEFAULT 768 //Default position of the gimble (*1000)
-#define GIMB_DELTA 2 //Amount to increment by
-
-#define ARMS_SCALE 0.75
-
-#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
-
-#ifndef LOG
-#define LOG(X) logA << X; logB << X; logC << X //for writing data to the csv file
-#endif
-
-class Robot: public IterativeRobot
-{
-private:
- TalonSRX left_drive, right_drive;
- CANTalon shooter1, shooter2,
- ramp,
- arms;
- RobotDrive drive;
- Shooter shooter;
- Joystick driver_stick, operator_stick;
- Timer auto_clock;
- Servo gimbs;
- int gimba; // angle of the gimble as a value of from 0 to 1000
-
- // 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;
-
- enum auto_states
- {
- START,
- DRIVING_FORWARD,
- STOP
- };
-
- auto_states auto_status;
-
- LiveWindow *lw = LiveWindow::GetInstance();
- SendableChooser *chooser;
- const std::string autoNameDefault = "1.7 Seconds (Slightly Longer)";
- const std::string autoNameShort = "1.5 Seconds (Short, good on the moat)";
- const std::string autoNameOne = "1 Seconds";
- const std::string autoNameDisable = "Disable Autonomous (set time to 0)";
- std::string autoSelected;
-
- void LogCSVData()
- {
- static PowerDistributionPanel pdp; // preparing to read from the pdp
- static std::vector motors;
-
- static std::ofstream logA, logB, logC;
- timeval tm;
-
- SmartDashboard::PutBoolean("log A", logA.is_open());
- SmartDashboard::PutBoolean("log B", logB.is_open());
- SmartDashboard::PutBoolean("log C", logC.is_open());
-
- SmartDashboard::PutBoolean("Ramp Limit F", !ramp.IsFwdLimitSwitchClosed());
- SmartDashboard::PutBoolean("Ramp Limit R", !ramp.IsRevLimitSwitchClosed());
- SmartDashboard::PutBoolean("Arms Limit F", !arms.IsFwdLimitSwitchClosed());
- SmartDashboard::PutBoolean("Arms Limit R", !arms.IsRevLimitSwitchClosed());
-
- SmartDashboard::PutNumber("angle of camera", gimba);
-
- if (!logA.is_open() && !logB.is_open() && !logC.is_open())
- {
- std::fstream logNumFile;
- int logNum;
- logNumFile.open("/home/lvuser/logNum");
- logNumFile >> logNum;
- logNum++;
- logNumFile.seekp(0);
- logNumFile << logNum;
- // writing to /home/lvuser/logs/[unixtime].log
- logA.open("/media/sda1/logs/log" + std::to_string(logNum) + ".csv");
- std::cerr << (logA.is_open() ? "Opened" : "Failed to open") << "log A." << std::endl;
- logB.open("/media/sdb1/logs/log" + std::to_string(logNum) + ".csv");
- std::cerr << (logB.is_open() ? "Opened" : "Failed to open") << "log B." << std::endl;
- logC.open("/home/lvuser/logs/log" + std::to_string(logNum) + ".csv");
- std::cerr << (logC.is_open() ? "Opened" : "Failed to open") << "log C." << std::endl;
-
- LOG("Time\tpdpInput voltage\tpdpTemperature\tpdpTotal Current\t");
- for (int ii = 0; ii < 16; ii++)
- {
- LOG("pdpChannel " << ii << " current\t");
- }
-
- LOG("tlaunch_spinny Bus Voltage\ttlaunch_spinny Output Current\ttlaunch_spinny Output Voltage\ttlaunch_spinny Temperature");
- motors.push_back(&ramp);
- LOG("\tShooter1 Bus Voltage\tShooter1 Output Current\tShooter1 Output Voltage\tShooter1 Temperature");
- motors.push_back(&shooter1);
- LOG("\tShooter2 Bus Voltage\tShooter2 Output Current\tShooter2 Output Voltage\tShooter2 Temperature");
- motors.push_back(&shooter2);
-
- LOG("\tJoystick X\tJoystick Y\tJoystick Twist");
- LOG("\tAlliance\tLocation\tMatch Time\tFMS Attached\tBrowned Out");
- LOG("\tTestStage");
- LOG(std::endl);
- }
- gettimeofday(&tm, NULL);
- LOG(time(0) << '.' << std::setfill('0') << std::setw(3) << tm.tv_usec/1000);
- // Some general information
- LOG("\t" << pdp.GetVoltage());
- LOG("\t" << pdp.GetTemperature());
- LOG("\t" << pdp.GetTotalCurrent());
- // current on each channel
- for (int ii = 0; ii < 16; ii++)
- {
- LOG("\t" << pdp.GetCurrent(ii));
- }
-
- //Talon Data
- for(int ii = 0; ii < motors.size(); ii++)
- {
- LOG("\t" << motors[ii]->GetBusVoltage());
- LOG("\t" << motors[ii]->GetOutputVoltage());
- LOG("\t" << motors[ii]->GetOutputCurrent());
- LOG("\t" << motors[ii]->GetTemperature());
- }
- //control data
- LOG("\t" << driver_stick.GetX());
- LOG("\t" << driver_stick.GetY());
- LOG("\t" << driver_stick.GetTwist());
-
- //DriverStation Data
- LOG("\t" << DriverStation::GetInstance().GetAlliance());
- LOG("\t" << DriverStation::GetInstance().GetLocation());
- LOG("\t" << DriverStation::GetInstance().GetMatchTime());
- LOG("\t" << DriverStation::GetInstance().IsFMSAttached());
- LOG("\t" << DriverStation::GetInstance().IsSysBrownedOut());
- LOG(std::endl);
- }
-
- /**
- * 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 UpdateDrive()
- {
- // Enable arcade mode if button 7 is pressed, disable if
- // button 8 is pressed
- arcade = (driver_stick.GetRawButton(7) || arcade) && !driver_stick.GetRawButton(8);
- if (arcade)
- {
- drive.ArcadeDrive(driver_stick);
- }
- else if (driver_stick.GetRawButton(THUMB)) //Turning
- {
- float left = driver_stick.GetTwist();
- float right = -driver_stick.GetTwist();
- drive.TankDrive(left, right);
- }
- else //Normal Drive
- {
- 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);
- }
- }
- }
-
-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)
- gimbs(3),
- gimba(GIMB_DEFAULT)
- {
-
- }
-
- void RobotInit()
- {
- chooser = new SendableChooser();
-
- chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
- chooser->AddObject(autoNameShort, (void*)&autoNameShort);
- chooser->AddObject(autoNameOne, (void*)&autoNameOne);
- chooser->AddObject(autoNameDisable, (void*)&autoNameDisable);
- 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;
-
- // Initialize the number so we can get it later
- SmartDashboard::PutNumber("shooting angle", GIMB_DEFAULT);
-
- }
-
- void AutonomousInit()
- {
- autoSelected = *((std::string*)chooser->GetSelected());
- //std::string autoSelected = SmartDashboard::GetString("Auto Selector", autoNameDefault);
- std::cout << "Auto selected: " << autoSelected << std::endl;
-
- auto_status = START;
- }
-
- void AutonomousPeriodic()
- {
- const float drivePower = 1;
- float driveTime = 1.7;
- LogCSVData();
- if (autoSelected == autoNameDisable) driveTime = 0;
- else if (autoSelected == autoNameShort) driveTime = 1.5;
- else if (autoSelected == autoNameOne) driveTime = 1;
- else driveTime = 1.7;
-
- //Default Auto goes here
- switch (auto_status)
- {
- case START:
- {
- auto_clock.Start();
- auto_status = DRIVING_FORWARD;
- break;
- }
- case DRIVING_FORWARD:
- {
- if (auto_clock.Get() > driveTime)
- {
- drive.TankDrive(0.0, 0.0);
- auto_status = STOP;
- }
- else
- {
- drive.TankDrive(drivePower, drivePower);
- }
- break;
- }
- case STOP:
- {
- std::cout << "All done!\n" ;
- break;
- }
- }
- gimbs.Set(gimba/1000.0);
- }
-
- void TeleopInit()
- {
-
- }
-
- void TeleopPeriodic()
- {
- LogCSVData();
- 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_RAISE)
- && !operator_stick.GetRawButton(RAMP_LOWER))
- {
- 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;
- }
-
- /*
- * This is for controlling the gimbal.
- */
- if(operator_stick.GetRawButton(GIM_UP))
- {
- gimba += GIMB_DELTA;
- }
- else if(operator_stick.GetRawButton(GIM_DOWN)){
- gimba -= GIMB_DELTA;
- }
- else if(operator_stick.GetRawButton(GIM_SHOOT))
- {
- gimba = SmartDashboard::GetNumber("shooting angle", GIMB_DEFAULT);
- }
- gimbs.Set(gimba/1000.0);
-
- /*
- * 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;
- }
-
- if(operator_stick.GetRawButton(4))
- {
- arms.Set(-operator_stick.GetY() * ARMS_SCALE);
- }
- else
- {
- 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);
- }
- }
-
- void TestPeriodic()
- {
- const int driveTime = 5; //Drive for 5 seconds
- const float drivePower = 0.5, armPower=.35, rampPower=.4; //Arbitrary powers to test at
- static Timer t;
- enum test_stages {
- INIT,
- ARMS_UP,
- ARMS_DOWN,
- RAMP_UP,
- RAMP_DOWN,
- DRIVE_FORWARD,
- DRIVE_BACKWARD,
- TURN_CW,
- TURN_CCW,
- END};
-
- static enum test_stages test_stage, old_test_stage;
-
- if (old_test_stage != test_stage)
- {
- //Reset timer between stages
- t.Stop();
- t.Reset();
-
- //Wait for a button press between stages
- if (operator_stick.GetRawButton(TRIGGER))
- old_test_stage = test_stage;
- }
- else
- {
- if(!t.Get()) t.Start();
-
- switch (test_stage)
- {
- case INIT:
- {
- break;
- }
-
- case ARMS_UP:
- {
- if (arms.GetForwardLimitOK())
- arms.Set(1);
- else
- {
- arms.Set(0);
- test_stage = ARMS_DOWN;
- }
- break;
- }
-
- case ARMS_DOWN:
- {
- if (arms.GetReverseLimitOK())
- arms.Set(-1);
- else
- {
- arms.Set(0);
- test_stage = RAMP_UP;
- }
- break;
- }
-
- case RAMP_UP:
- {
- if (arms.GetForwardLimitOK())
- ramp.Set(1);
- else
- {
- ramp.Set(0);
- test_stage = RAMP_DOWN;
- }
- break;
- }
-
- case RAMP_DOWN:
- {
- if (arms.GetReverseLimitOK())
- ramp.Set(-1);
- else
- {
- ramp.Set(0);
- test_stage = DRIVE_FORWARD;
- }
- break;
- }
-
- case DRIVE_FORWARD:
- {
- if (t.Get() < driveTime)
- drive.TankDrive(drivePower, drivePower);
- else
- {
- drive.TankDrive(0.0, 0.0);
- test_stage = DRIVE_BACKWARD;
- }
- break;
- }
-
- case DRIVE_BACKWARD:
- {
- if (t.Get() < driveTime)
- drive.TankDrive(-drivePower, -drivePower);
- else
- {
- drive.TankDrive(0.0, 0.0);
- test_stage = TURN_CW;
- }
- break;
- }
-
- case TURN_CW:
- {
- if (t.Get() < driveTime)
- drive.TankDrive(drivePower, -drivePower);
- else
- {
- drive.TankDrive(0.0, 0.0);
- test_stage = TURN_CCW;
- }
- break;
- }
-
- case TURN_CCW:
- {
- if (t.Get() < driveTime)
- drive.TankDrive(-drivePower, drivePower);
- else
- {
- drive.TankDrive(0.0, 0.0);
- test_stage = END;
- }
- break;
- }
-
- case END:
- {
- break;
- }
- }
- }
-
- LogCSVData();
- }
-};
-
-START_ROBOT_CLASS(Robot)
diff --git a/src/Shooter.cpp b/src/Shooter.cpp
deleted file mode 100644
index 5909d48..0000000
--- a/src/Shooter.cpp
+++ /dev/null
@@ -1,151 +0,0 @@
-#include
-#include "Shooter.h"
-
-Shooter::Shooter(CANTalon *s1, CANTalon *s2, CANTalon *r) :
- launcher(s1),
- pickup(s2),
- ramp(r),
- ready(true),
- state(READY)
-{
- // shooterDrive = new RobotDrive(s1, s2);
-}
-
-/**
- * Call this method on TeleopInit so that the ramp is properly
- * set at the beginning of the match.
- */
-
-Shooter::~Shooter()
-{
- delete launcher;
- delete pickup;
- delete ramp;
-}
-
-void Shooter::StopShooter()
-{
- ready = true;
- ramp->Set(0);
- launcher->Set(0);
- pickup->Set(0);
-}
-
-void Shooter::LowerRamp()
-{
- ramp->Set(-RAMP_POWER);
- 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 Shooter::RaiseRamp()
-{
- ramp->Set(RAMP_POWER);
- 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 Shooter::StopRamp()
-{
- ramp->Set(0);
-}
-void Shooter::Shoot()
-{
- if (shotClock.Get() > (SPINUP_TIME + 0.1))
- {
- state = RESETTING;
- }
- switch (state)
- {
- case READY:
- {
- state = SPINNINGUP;
- ramp->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
- {
- //std::cout << "*Goku noises*\n";
- }
- break;
- }
- case LAUNCH:
- {
- ramp->Set(LAUNCH_POWER);
- state = LAUNCHING;
- break;
- }
- case LAUNCHING:
- {
- 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 Shooter::PickUp(bool state /*= true*/)
-{
- pickup->Set((float) (state * PICKUP_POWER));
- launcher->Set((float) (state * PICKUP_POWER * -1));
- //std::cout << "picking up!\n";
-}
-
-/**
- * Call this to run the pickup backwards if the ball gets jammed somehow...
- */
-void Shooter::Unjam()
-{
- pickup->Set(-1 * PICKUP_POWER);
-}
-
-void Shooter::ShootLow()
-{
- pickup->Set(-1);
-}
-
-void Shooter::SetPower(float power)
-{
- pickup->Set(power);
- launcher->Set(power);
- //std::cout << "setting shooter power" << std::endl;
-}
-
-int Shooter::GetState()
-{
- return state;
-}
diff --git a/src/Shooter.h b/src/Shooter.h
deleted file mode 100644
index ad505b6..0000000
--- a/src/Shooter.h
+++ /dev/null
@@ -1,62 +0,0 @@
-/*
- * Shooter.h
- *
- * Created on: Feb 2, 2016
- * Author: Jason
- */
-
-#ifndef SRC_SHOOTER_H_
-#define SRC_SHOOTER_H_
-
-#define PICKUP_POWER 0.75
-#define LAUNCH_POWER 1
-#define SPINUP_TIME 1.5 // seconds.
-#define LAUNCH_TIME 0.5
-#define RAMP_POWER 0.3
-
-class Shooter
-{
-public:
-
- /**
- * Shooter talons and launch-spinny talon.
- * s2 is also for the pickup-mechanism and can be controlled independently.
- *
- */
- enum ShooterState
- {
- READY,
- ON_FIRE,
- SPINNINGUP,
- LAUNCH,
- LAUNCHING,
- RESETTING
- };
-
- Shooter(CANTalon *s1, CANTalon *s2, CANTalon *r);
- virtual ~Shooter();
- void StopShooter();
- void LowerRamp();
- void RaiseRamp();
- void StopRamp();
- void Shoot();
- void PickUp(bool state = true);
- void Unjam();
- void ShootLow();
- void SetPower(float power);
- int GetState();
-
-private:
-
- //RobotDrive *shooterDrive;
- CANTalon *launcher;
- CANTalon *pickup;
- CANTalon *ramp;
- ShooterState state;
-
- Timer shotClock;
- bool ready;
- int fake_position;
-};
-
-#endif /* SRC_SHOOTER_H_ */