Merged feature/command-based to develop

.gitignore

@@ -1,4 +1,4 @@
 *.o
-Makefile
-wpilib
+Debug
 bin
+wpilib

.gitmodules

@@ -1,3 +0,0 @@
-[submodule "hhlib"]
-	path = hhlib
-	url = git@github.com:team2059/hhlib.git

CommandBase.cpp

@@ -0,0 +1,22 @@
+#include "CommandBase.h"
+#include "Subsystems/Drivetrain.h"
+#include "Subsystems/Collector.h"
+#include "Subsystems/Elevator.h"
+#include "Subsystems/BinElevator.h"
+Drivetrain* CommandBase::drivetrain = NULL;
+Collector* CommandBase::collector = NULL;
+Elevator* CommandBase::elevator = NULL;
+BinElevator* CommandBase::binElevator = NULL;
+OI* CommandBase::oi = NULL;
+CommandBase::CommandBase(char const *name) : Command(name) {
+}
+CommandBase::CommandBase() : Command() {
+}
+void CommandBase::init(){
+  drivetrain = new Drivetrain();
+  collector = new Collector();
+  elevator = new Elevator();
+  binElevator = new BinElevator();
+  oi = new OI();
+}
+// vim: ts=2:sw=2:et

CommandBase.h

@@ -0,0 +1,24 @@
+#ifndef COMMAND_BASE_H
+#define COMMAND_BASE_H
+
+#include <string>
+#include "Subsystems/Drivetrain.h"
+#include "Subsystems/Collector.h"
+#include "Subsystems/Elevator.h"
+#include "Subsystems/BinElevator.h"
+#include "OI.h"
+#include "WPILib.h"
+
+class CommandBase: public Command {
+  public:
+    CommandBase(char const *name);
+    CommandBase();
+    static void init();
+    static Drivetrain *drivetrain;
+    static Collector *collector;
+    static Elevator *elevator;
+    static BinElevator *binElevator;
+    static OI *oi;
+};
+#endif
+// vim: ts=2:sw=2:et

Commands/Autonomous/AutoDrive.cpp

@@ -0,0 +1,22 @@
+#include "AutoDrive.h"
+#include "../../DentRobot.h"
+// Drive for a short while then stop. Just for testing
+AutoDrive::AutoDrive() : Command("AutoDrive"){
+  Requires(DentRobot::drivetrain);
+  SetTimeout(0.5);
+}
+void AutoDrive::Initialize(){
+}
+void AutoDrive::Execute(){
+  //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
+  DentRobot::drivetrain->DriveMecanum(0.0,-.75,0.0,0.9,0.0);
+}
+bool AutoDrive::IsFinished(){
+  return IsTimedOut();
+}
+void AutoDrive::End(){
+}
+void AutoDrive::Interrupted(){
+  End();
+}
+// vim: ts=2:sw=2:et

Commands/Autonomous/AutoDrive.h

@@ -0,0 +1,19 @@
+#ifndef AUTODRIVE_H
+#define AUTODRIVE_H
+
+#include "Commands/Command.h"
+#include "../../CommandBase.h"
+#include "../../DentRobot.h"
+#include "WPILib.h"
+
+class AutoDrive: public Command{
+  public:
+    AutoDrive();
+    void Initialize();
+    void Execute();
+    bool IsFinished();
+    void End();
+    void Interrupted();
+};
+#endif
+// vim: ts=2:sw=2:et

Commands/Autonomous/Autonomous.cpp

@@ -0,0 +1,22 @@
+#include "Autonomous.h"
+#include "../../DentRobot.h"
+#include "../Elevator/Raise.h"
+#include "../Elevator/Lower.h"
+#include "AutoDrive.h"
+#include "Turn.h"
+#include "../Collector/CloseCollector.h"
+#include "../Collector/OpenCollector.h"
+#include "../Collector/CollectTote.h"
+Autonomous::Autonomous(){
+  //AddSequential(new Raise());
+  //AddSequential(new Lower());
+  //AddSequential(new AutoDrive());
+  AddSequential(new Raise());
+  AddSequential(new Lower());
+  AddSequential(new Turn());
+  AddParallel(new AutoDrive());
+  AddParallel(new CloseCollector());
+  AddParallel(new CollectTote());
+  AddSequential(new Turn());
+}
+// vim: ts=2:sw=2:et

Commands/Autonomous/Autonomous.h

@@ -0,0 +1,14 @@
+#ifndef AUTONOMOUS_H
+#define AUTONOMOUS_H
+
+#include "Commands/CommandGroup.h"
+#include "../../CommandBase.h"
+#include "../../DentRobot.h"
+#include "WPILib.h"
+
+class Autonomous: public CommandGroup{
+  public:
+    Autonomous();
+};
+#endif
+// vim: ts=2:sw=2:et

Commands/Autonomous/Turn.cpp

@@ -0,0 +1,22 @@
+#include "Turn.h"
+#include "../../DentRobot.h"
+// Drive for a short while then stop. Just for testing
+Turn::Turn() : Command("Turn"){
+  Requires(DentRobot::drivetrain);
+  SetTimeout(0.25);
+}
+void Turn::Initialize(){
+}
+void Turn::Execute(){
+  //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
+  DentRobot::drivetrain->DriveMecanum(0.0,0.0,0.5,0.9,0.0);
+}
+bool Turn::IsFinished(){
+  return IsTimedOut();
+}
+void Turn::End(){
+}
+void Turn::Interrupted(){
+  End();
+}
+// vim: ts=2:sw=2:et

Commands/Autonomous/Turn.h

@@ -0,0 +1,19 @@
+#ifndef TURN_H
+#define TURN_H
+
+#include "Commands/Command.h"
+#include "../../CommandBase.h"
+#include "../../DentRobot.h"
+#include "WPILib.h"
+
+class Turn: public Command{
+  public:
+    Turn();
+    void Initialize();
+    void Execute();
+    bool IsFinished();
+    void End();
+    void Interrupted();
+};
+#endif
+// vim: ts=2:sw=2:et

Commands/BinElevator/BinLower.cpp

@@ -0,0 +1,26 @@
+#include "BinLower.h"
+#include "../../DentRobot.h"
+#include "../../OI.h"
+BinLower::BinLower() : Command("BinLower"){
+}
+void BinLower::Initialize(){
+  SetTimeout(2.5);
+}
+void BinLower::Execute(){
+  DentRobot::binElevator->Run(-1.0);
+}
+bool BinLower::IsFinished(){
+  if (!DentRobot::binElevator->GetElevatorBottom()||IsTimedOut()){
+    printf("Robot stoped BinLowering. Sensor based? %d\n", !DentRobot::binElevator->GetElevatorBottom());
+    return true;
+  }else{
+    return false;
+  }
+}
+void BinLower::End(){
+  DentRobot::binElevator->Run(0.0f);
+}
+void BinLower::Interrupted(){
+  End();
+}
+// vim: ts=2:sw=2:et

Commands/BinElevator/BinLower.h

@@ -0,0 +1,17 @@
+#ifndef BINLOWER_H
+#define BINLOWER_H
+
+#include "Commands/Command.h"
+#include "WPILib.h"
+
+class BinLower: public Command{
+  public:
+    BinLower();
+    void Initialize();
+    void Execute();
+    bool IsFinished();
+    void End();
+    void Interrupted();
+};
+#endif
+// vim: ts=2:sw=2:et

Commands/BinElevator/BinRaise.cpp

@@ -0,0 +1,26 @@
+#include "BinRaise.h"
+#include "../../DentRobot.h"
+#include "../../OI.h"
+BinRaise::BinRaise() : Command("BinRaise"){
+}
+void BinRaise::Initialize(){
+  SetTimeout(3.0);
+}
+void BinRaise::Execute(){
+  DentRobot::binElevator->Run(1.0);
+}
+bool BinRaise::IsFinished(){
+  if (!DentRobot::binElevator->GetElevatorTop()||IsTimedOut()){
+    printf("Robot stoped raising. Sensor based? %d\n", !DentRobot::binElevator->GetElevatorTop());
+    return true;
+  }else{
+    return false;
+  }
+}
+void BinRaise::End(){
+  DentRobot::binElevator->Run(0.0f);
+}
+void BinRaise::Interrupted(){
+  End();
+}
+// vim: ts=2:sw=2:et

Commands/BinElevator/BinRaise.h

@@ -0,0 +1,18 @@
+#ifndef BINRAISE_H
+#define BINRAISE_H
+
+#include "Commands/Command.h"
+#include "WPILib.h"
+
+class BinRaise: public Command{
+  public:
+    BinRaise();
+    void Initialize();
+    void Execute();
+    bool IsFinished();
+    void End();
+    void Interrupted();
+};
+
+#endif
+// vim: ts=2:sw=2:et

Commands/Collector/CloseCollector.cpp

@@ -0,0 +1,28 @@
+#include "CloseCollector.h"
+CloseCollector::CloseCollector() : Command("CloseCollector"){
+  Requires(DentRobot::collector);
+}
+void CloseCollector::Initialize(){
+  printf("Initialized collector: 0.5\n");
+  SetTimeout(2.5);
+}
+void CloseCollector::Execute(){
+  //printf("Closing collector: -0.5f\n");
+  DentRobot::collector->MoveArms(-0.5);
+  //DentRobot::collector->MoveArms(-(-DentRobot::oi->GetRightStick()->GetRawAxis(3)+1)/2*.3/.5);
+}
+bool CloseCollector::IsFinished(){
+  if(DentRobot::collector->ArmSensor()||IsTimedOut()){
+    printf("Stopped Closing: %d, %d\n",DentRobot::collector->ArmSensor(), IsTimedOut());
+    return true;
+  }else{
+    return false;
+  }
+}
+void CloseCollector::End(){
+  DentRobot::collector->MoveArms(0.0f);
+}
+void CloseCollector::Interrupted(){
+  End();
+}
+// vim: ts=2:sw=2:et

Commands/Collector/CloseCollector.h

@@ -0,0 +1,20 @@
+#ifndef CLOSECOLLECTOR_H
+#define CLOSECOLLECTOR_H
+
+#include "Commands/Command.h"
+#include "../../CommandBase.h"
+#include "../../DentRobot.h"
+#include "WPILib.h"
+
+class CloseCollector: public Command{
+  public:
+    CloseCollector();
+    void Initialize();
+    void Execute();
+    bool IsFinished();
+    void End();
+    void Interrupted();
+};
+
+#endif
+// vim: ts=2:sw=2:et

Commands/Collector/CollectTote.cpp

@@ -0,0 +1,22 @@
+#include "CollectTote.h"
+CollectTote::CollectTote() : Command("CollectTote"){
+  Requires(DentRobot::collector);
+}
+void CollectTote::Initialize(){
+  printf("Initialized CollectTote\n");
+  SetTimeout(2.0);
+}
+void CollectTote::Execute(){
+  //TODO check this value to move the motors in the right direction
+  DentRobot::collector->MoveRollers(-(-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2);
+}
+bool CollectTote::IsFinished(){
+  return DentRobot::collector->BoxCollected()||IsTimedOut();
+}
+void CollectTote::End(){
+  DentRobot::collector->MoveRollers(0.0);
+}
+void CollectTote::Interrupted(){
+  End();
+}
+// vim: ts=2:sw=2:et

Commands/Collector/CollectTote.h

@@ -0,0 +1,20 @@
+#ifndef COLLECTTOTE_H
+#define COLLECTTOTE_H
+
+#include "Commands/Command.h"
+#include "../../CommandBase.h"
+#include "../../DentRobot.h"
+#include "WPILib.h"
+
+class CollectTote: public Command{
+  public:
+    CollectTote();
+    void Initialize();
+    void Execute();
+    bool IsFinished();
+    void End();
+    void Interrupted();
+};
+
+#endif
+// vim: ts=2:sw=2:et

Commands/Collector/OpenCollector.cpp

@@ -0,0 +1,22 @@
+#include "OpenCollector.h"
+OpenCollector::OpenCollector() : Command("OpenCollector"){
+  Requires(DentRobot::collector);
+}
+void OpenCollector::Initialize(){
+  SetTimeout(0.5);
+}
+void OpenCollector::Execute(){
+  DentRobot::collector->MoveArms(0.35);
+  //DentRobot::collector->MoveArms((-DentRobot::oi->GetRightStick()->GetRawAxis(3)+1)/2*.3/.5);
+}
+bool OpenCollector::IsFinished(){
+  //return DentRobot::collector->ArmSensor();
+  return IsTimedOut();
+}
+void OpenCollector::End(){
+  DentRobot::collector->MoveArms(0.0f);
+}
+void OpenCollector::Interrupted(){
+  End();
+}
+// vim: ts=2:sw=2:et

Commands/Collector/OpenCollector.h

@@ -0,0 +1,20 @@
+#ifndef OPENCOLLECTOR_H
+#define OPENCOLLECTOR_H
+
+#include "Commands/Command.h"
+#include "../../CommandBase.h"
+#include "../../DentRobot.h"
+#include "WPILib.h"
+
+class OpenCollector: public Command{
+  public:
+    OpenCollector();
+    void Initialize();
+    void Execute();
+    bool IsFinished();
+    void End();
+    void Interrupted();
+};
+
+#endif
+// vim: ts=2:sw=2:et

Commands/Collector/ReleaseTote.cpp

@@ -0,0 +1,22 @@
+#include "ReleaseTote.h"
+ReleaseTote::ReleaseTote() : Command("ReleaseTote"){
+  Requires(DentRobot::collector);
+}
+void ReleaseTote::Initialize(){
+  SetTimeout(2.0);
+}
+void ReleaseTote::Execute(){
+  //TODO check this value to move the motors in the right direction
+  // Devide by 2 twice because this speed should be half the collector speed
+  DentRobot::collector->MoveRollers((-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2/2);
+}
+bool ReleaseTote::IsFinished(){
+  return DentRobot::collector->BoxCollected();
+}
+void ReleaseTote::End(){
+  DentRobot::collector->MoveRollers(0.0f);
+}
+void ReleaseTote::Interrupted(){
+  End();
+}
+// vim: ts=2:sw=2:et

Commands/Collector/ReleaseTote.h

@@ -0,0 +1,20 @@
+#ifndef RELEASETOTE_H
+#define RELEASETOTE_H
+
+#include "Commands/Command.h"
+#include "../../CommandBase.h"
+#include "../../DentRobot.h"
+#include "WPILib.h"
+
+class ReleaseTote: public Command{
+  public:
+    ReleaseTote();
+    void Initialize();
+    void Execute();
+    bool IsFinished();
+    void End();
+    void Interrupted();
+};
+
+#endif
+// vim: ts=2:sw=2:et

Commands/Drivetrain/Drive.cpp

@@ -0,0 +1,31 @@
+#include "Drive.h"
+#include "../../DentRobot.h"
+Drive::Drive() : Command("Drive"){
+  Requires(DentRobot::drivetrain);
+}
+void Drive::Initialize(){
+}
+void Drive::Execute(){
+  double x,y,z;
+  //Code to lock the x axis when not holding button 1
+  x = DentRobot::oi->GetLeftStick()->GetRawAxis(0);
+  z = DentRobot::oi->GetLeftStick()->GetRawAxis(2);
+  y = DentRobot::oi->GetLeftStick()->GetRawAxis(1);
+  //if (DentRobot::oi->GetLeftStick()->GetRawButton(1)){
+  //  x=0;
+  //}
+  //if (DentRobot::oi->GetLeftStick()->GetRawButton(2)){
+  //  y=0;
+  //}
+  //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
+  DentRobot::drivetrain->DriveMecanum(x,y,z,0.9,0);
+}
+bool Drive::IsFinished(){
+  return false;
+}
+void Drive::End(){
+}
+void Drive::Interrupted(){
+  End();
+}
+// vim: ts=2:sw=2:et

Commands/Drivetrain/Drive.h

@@ -0,0 +1,19 @@
+#ifndef DRIVE_H
+#define DRIVE_H
+
+#include "Commands/Command.h"
+#include "../../CommandBase.h"
+#include "../../DentRobot.h"
+#include "WPILib.h"
+
+class Drive: public Command{
+  public:
+    Drive();
+    void Initialize();
+    void Execute();
+    bool IsFinished();
+    void End();
+    void Interrupted();
+};
+#endif
+// vim: ts=2:sw=2:et

Commands/Elevator/Lower.cpp

@@ -0,0 +1,26 @@
+#include "Lower.h"
+#include "../../DentRobot.h"
+#include "../../OI.h"
+Lower::Lower() : Command("Lower"){
+}
+void Lower::Initialize(){
+  SetTimeout(2.5);
+}
+void Lower::Execute(){
+  DentRobot::elevator->Run(-1.0);
+}
+bool Lower::IsFinished(){
+  if (!DentRobot::elevator->GetElevatorBottom()||IsTimedOut()){
+    printf("Robot stoped lowering. Sensor based? %d\n", !DentRobot::elevator->GetElevatorBottom());
+    return true;
+  }else{
+    return false;
+  }
+}
+void Lower::End(){
+  DentRobot::elevator->Run(0.0f);
+}
+void Lower::Interrupted(){
+  End();
+}
+// vim: ts=2:sw=2:et

Commands/Elevator/Lower.h

@@ -0,0 +1,17 @@
+#ifndef LOWER_H
+#define LOWER_H
+
+#include "Commands/Command.h"
+#include "WPILib.h"
+
+class Lower: public Command{
+  public:
+    Lower();
+    void Initialize();
+    void Execute();
+    bool IsFinished();
+    void End();
+    void Interrupted();
+};
+#endif
+// vim: ts=2:sw=2:et

Commands/Elevator/Raise.cpp

@@ -0,0 +1,26 @@
+#include "Raise.h"
+#include "../../DentRobot.h"
+#include "../../OI.h"
+Raise::Raise() : Command("Raise"){
+}
+void Raise::Initialize(){
+  SetTimeout(3.0);
+}
+void Raise::Execute(){
+  DentRobot::elevator->Run(1.0);
+}
+bool Raise::IsFinished(){
+  if (!DentRobot::elevator->GetElevatorTop()||IsTimedOut()){
+    printf("Robot stoped raising. Sensor based? %d\n", !DentRobot::elevator->GetElevatorTop());
+    return true;
+  }else{
+    return false;
+  }
+}
+void Raise::End(){
+  DentRobot::elevator->Run(0.0f);
+}
+void Raise::Interrupted(){
+  End();
+}
+// vim: ts=2:sw=2:et

Commands/Elevator/Raise.h

@@ -0,0 +1,18 @@
+#ifndef RAISE_H
+#define RAISE_H
+
+#include "Commands/Command.h"
+#include "WPILib.h"
+
+class Raise: public Command{
+  public:
+    Raise();
+    void Initialize();
+    void Execute();
+    bool IsFinished();
+    void End();
+    void Interrupted();
+};
+
+#endif
+// vim: ts=2:sw=2:et

DentRobot.cpp

@@ -0,0 +1,45 @@
+#include "DentRobot.h"
+#include "Commands/Autonomous/Autonomous.h"
+OI* DentRobot::oi=NULL;
+Collector* DentRobot::collector=NULL;
+Drivetrain* DentRobot::drivetrain=NULL;
+Elevator* DentRobot::elevator=NULL;
+BinElevator* DentRobot::binElevator=NULL;
+CommandGroup* DentRobot::aut=NULL;
+DentRobot::DentRobot(){
+  oi=new OI();
+  collector=new Collector();
+  drivetrain=new Drivetrain();
+  elevator=new Elevator();
+  binElevator=new BinElevator();
+  aut=new Autonomous();
+  CameraServer::GetInstance()->SetQuality(25);
+  CameraServer::GetInstance()->StartAutomaticCapture("cam0");
+  printf("Initialized");
+}
+void DentRobot::RobotInit(){
+  SmartDashboard::PutNumber("CodeVersion",0.001);
+}
+void DentRobot::DisabledPeriodic(){
+  Scheduler::GetInstance()->Run();
+}
+void DentRobot::AutonomousInit(){
+  if(aut != NULL){
+    aut->Start();
+  }
+}
+void DentRobot::AutonomousPeriodic(){
+  Scheduler::GetInstance()->Run();
+}
+void DentRobot::TeleopInit(){
+  //if (aut != NULL){
+  //  aut->Cancel();
+  //}
+}
+void DentRobot::TeleopPeriodic(){
+  Scheduler::GetInstance()->Run();
+}
+void DentRobot::TestPeriodic(){
+}
+START_ROBOT_CLASS(DentRobot);
+// vim: ts=2:sw=2:et

DentRobot.h

@@ -0,0 +1,30 @@
+#ifndef DENTROBOT_H
+#define DENTROBOT_H
+#include "WPILib.h"
+#include "OI.h"
+#include "Subsystems/Elevator.h"
+#include "Subsystems/BinElevator.h"
+#include "Subsystems/Drivetrain.h"
+#include "Subsystems/Collector.h"
+#include "Commands/Autonomous/Autonomous.h"
+class DentRobot: public IterativeRobot {
+  private:
+    Command *driveCommand = NULL;
+  public:
+    DentRobot();
+    static OI* oi;
+    static Collector* collector;
+    static Drivetrain* drivetrain;
+    static Elevator* elevator;
+    static BinElevator* binElevator;
+    static CommandGroup* aut;
+    void RobotInit();
+    void DisabledPeriodic();
+    void AutonomousInit();
+    void AutonomousPeriodic();
+    void TeleopInit();
+    void TeleopPeriodic();
+    void TestPeriodic();
+};
+#endif
+// vim: ts=2:sw=2:et

Makefile

@@ -0,0 +1,30 @@
+REMOTEIP=10.20.59.2
+CC=arm-frc-linux-gnueabi-g++
+CFLAGS=-std=c++11 -O0 -g3 -Wall -c -fmessage-length=0
+LDFLAGS=-Wl,-rpath,/opt/GenICam_v2_3/bin/Linux_armv7-a
+SOURCES=$(shell find -type f -name "*.cpp")
+OBJECTS=$(SOURCES:.cpp=.o)
+WPILIB=/var/frc/wpilib
+EXEC=bin/FRCUserProgram
+CLEANSER=rm -r
+
+all : $(OBJECTS)
+	$(CC) -L$(WPILIB)/lib $(LDFLAGS) -o $(EXEC) $(OBJECTS) -lwpi
+
+%.o : %.cpp
+	$(CC) $(CFLAGS) -I$(WPILIB)/include $^ -o $@
+
+clean:
+	$(CLEANSER) $(OBJECTS) bin/FRCUserProgram
+
+deploy:
+	@cat bin/FRCUserProgram | ssh admin@$(REMOTEIP) 'cat > /home/lvuser/FRCUserProgram2&&rm /home/lvuser/FRCUserProgram;mv /home/lvuser/FRCUserProgram2 /home/lvuser/FRCUserProgram&&. /etc/profile.d/natinst-path.sh;chmod a+x /home/lvuser/FRCUserProgram'
+
+debug:
+	@cat bin/FRCUserProgram | ssh admin@$(REMOTEIP) 'cat > /home/lvuser/FRCUserProgram2&&rm /home/lvuser/FRCUserProgram;mv /home/lvuser/FRCUserProgram2 /home/lvuser/FRCUserProgram&&. /etc/profile.d/natinst-path.sh;chmod a+x /home/lvuser/FRCUserProgram;/home/lvuser/run.sh'
+
+putkey:
+	@test -d ~/.ssh||mkdir ~/.ssh;test -f ~/.ssh/id_rsa||ssh-keygen -t rsa -f ~/.ssh/id_rsa -b 4096 -q -N '';cat ~/.ssh/id_rsa.pub|ssh -v admin@$(REMOTEIP) 'cat >> /tmp/key;mkdir -p ~/.ssh;cat /tmp/key >> ~/.ssh/authorized_keys;rm /tmp/key'
+
+updatemakefile:
+	@curl -s https://raw.githubusercontent.com/int0x191f2/nameless/master/configure.sh | sh

OI.cpp

@@ -0,0 +1,48 @@
+#include "OI.h"
+#include "Commands/Elevator/Lower.h"
+#include "Commands/Elevator/Raise.h"
+#include "Commands/Collector/OpenCollector.h"
+#include "Commands/Collector/CloseCollector.h"
+#include "Commands/Collector/CollectTote.h"
+#include "Commands/Collector/ReleaseTote.h"
+
+OI::OI() {
+  // Joysticks
+  leftStick=new Joystick(0);
+  rightStick=new Joystick(1);
+  // Collector
+  JoystickButton *right1=new JoystickButton(rightStick, 1);
+  JoystickButton *right2=new JoystickButton(rightStick, 2);
+  JoystickButton *left1=new JoystickButton(leftStick, 1);
+  JoystickButton *left2=new JoystickButton(leftStick, 2);
+  right1->WhileHeld(new CloseCollector());
+  right2->WhileHeld(new OpenCollector());
+  left1->WhileHeld(new CollectTote());
+  left2->WhileHeld(new ReleaseTote());
+  // Elevator
+  Raise* raise=new Raise();
+  Lower* lower=new Lower();
+  JoystickButton *right3=new JoystickButton(rightStick, 3);
+  JoystickButton *right4=new JoystickButton(rightStick, 4);
+  JoystickButton *right5=new JoystickButton(rightStick, 5);
+  JoystickButton *right6=new JoystickButton(rightStick, 6);
+  right3->WhenPressed(lower);
+  right4->WhenPressed(lower);
+  right3->CancelWhenPressed(raise);
+  right4->CancelWhenPressed(raise);
+  right5->WhenPressed(raise);
+  right6->WhenPressed(raise);
+  right5->CancelWhenPressed(lower);
+  right6->CancelWhenPressed(lower);
+  // Cancel
+  JoystickButton *right12=new JoystickButton(rightStick, 12);
+  right12->CancelWhenPressed(raise);
+  right12->CancelWhenPressed(lower);
+}
+Joystick* OI::GetRightStick(){
+  return rightStick;
+}
+Joystick* OI::GetLeftStick(){
+  return leftStick;
+}
+// vim: ts=2:sw=2:et

OI.h

@@ -0,0 +1,16 @@
+#ifndef OI_H
+#define OI_H
+
+#include "WPILib.h"
+
+class OI
+{
+  private:
+    Joystick *leftStick, *rightStick;
+  public:
+    OI();
+    Joystick* GetRightStick();
+    Joystick* GetLeftStick();
+};
+#endif
+// vim: ts=2:sw=2:et

RobotMap.h

@@ -0,0 +1,37 @@
+#ifndef ROBOTMAP_H
+#define ROBOTMAP_H
+
+#include "WPILib.h"
+
+// Elevator
+#define ELEVATOR_CAN 1
+#define ELEVATOR_BOTTOM_DIO 0
+#define ELEVATOR_COLELCT_TOTE_DIO 1
+#define ELEVATOR_READY_TOTE_DIO 2
+#define ELEVATOR_TOP_DIO 5
+#define ELEVATOR_ENCODERA 0
+#define ELEVATOR_ENCODERB 1
+
+// BinElevator
+#define BINELEVATOR_CAN 11
+#define BINELEVATOR_BOTTOM_DIO 6
+#define BINELEVATOR_COLELCT_BIN_DIO 7
+#define BINELEVATOR_TOP_DIO 8
+#define BINELEVATOR_ENCODERA 2
+#define BINELEVATOR_ENCODERB 3
+
+// Drivetrain
+#define DRIVE_FRONT_LEFT_CAN 2
+#define DRIVE_BACK_LEFT_CAN 3
+#define DRIVE_FRONT_RIGHT_CAN 4
+#define DRIVE_BACK_RIGHT_CAN 5
+
+// Collector
+#define COLLECTOR_WINDOW_LEFT_CAN 6
+#define COLLECTOR_WINDOW_RIGHT_CAN 7
+#define COLLECTOR_LEFT_CAN 8
+#define COLLECTOR_BOTTOM_CAN 10
+#define COLLECTOR_RIGHT_CAN 9
+
+#endif
+// vim: ts=2:sw=2:et

Subsystems/BinElevator.cpp

@@ -0,0 +1,26 @@
+#include "BinElevator.h"
+#include "../RobotMap.h"
+BinElevator::BinElevator(){
+  motor=new CANTalon(BINELEVATOR_CAN);
+  elevatorEncoder=new Encoder(BINELEVATOR_ENCODERA,BINELEVATOR_ENCODERB,false);
+  elevatorBottom=new DigitalInput(BINELEVATOR_BOTTOM_DIO);
+  elevatorTop=new DigitalInput(BINELEVATOR_TOP_DIO);
+}
+void BinElevator::InitDefaultCommand(){
+}
+void BinElevator::Run(double power){
+  motor->Set(power);
+}
+void BinElevator::ResetEncoder(){
+  elevatorEncoder->Reset();
+}
+double BinElevator::GetHeight(){
+  return elevatorEncoder->Get();
+}
+bool BinElevator::GetElevatorBottom(){
+  return elevatorBottom->Get();
+}
+bool BinElevator::GetElevatorTop(){
+  return elevatorTop->Get();
+}
+// vim: ts=2:sw=2:et

Subsystems/BinElevator.h

@@ -0,0 +1,22 @@
+#ifndef BINELEVATOR_H
+#define BINELEVATOR_H
+
+#include "WPILib.h"
+#include "Commands/PIDSubsystem.h"
+class BinElevator{
+  private:
+    CANTalon *motor;
+    Encoder *elevatorEncoder;
+    static constexpr double kP_real=4, kI_real=.0f, kP_simulation=18, kI_simulation=.2;
+    DigitalInput *elevatorBottom, *elevatorTop;
+  public:
+    BinElevator();
+    void InitDefaultCommand();
+    void Run(double);
+    void ResetEncoder();
+    double GetHeight();
+    bool GetElevatorTop();
+    bool GetElevatorBottom();
+};
+#endif
+// vim: ts=2:sw=2:et

Subsystems/Collector.cpp

@@ -0,0 +1,30 @@
+#include "Collector.h"
+#include "../RobotMap.h"
+
+Collector::Collector() : Subsystem("Collector") {
+  windowMotorLeft=new CANTalon(COLLECTOR_WINDOW_LEFT_CAN);
+  windowMotorRight=new CANTalon(COLLECTOR_WINDOW_RIGHT_CAN);
+  collectorMotorLeft=new CANTalon(COLLECTOR_LEFT_CAN);
+  collectorMotorBottom=new CANTalon(COLLECTOR_BOTTOM_CAN);
+  collectorMotorRight=new CANTalon(COLLECTOR_RIGHT_CAN);
+}
+void Collector::InitDefaultCommand() {
+}
+void Collector::MoveArms(double a){
+  windowMotorLeft->Set(a);
+  windowMotorRight->Set(-a);
+}
+void Collector::MoveRollers(double a){
+  collectorMotorLeft->Set(a);
+  collectorMotorBottom->Set(a);
+  collectorMotorRight->Set(-a);
+}
+bool Collector::ArmSensor(){
+  // TODO: include limit switch code
+  return false;
+}
+bool Collector::BoxCollected(){
+  return false;
+  //return boxSwitch->Get();
+}
+// vim: ts=2:sw=2:et

Subsystems/Collector.h

@@ -0,0 +1,18 @@
+#ifndef COLLECTOR_H
+#define COLLECTOR_H
+
+#include "WPILib.h"
+class Collector: public Subsystem
+{
+  private:
+    CANTalon *windowMotorLeft, *windowMotorRight, *collectorMotorLeft, *collectorMotorBottom, *collectorMotorRight;
+  public:
+    Collector();
+    void InitDefaultCommand();
+    void MoveArms(double);
+    void MoveRollers(double);
+    bool ArmSensor();
+    bool BoxCollected();
+};
+#endif
+// vim: ts=2:sw=2:et

Subsystems/Drivetrain.cpp

@@ -0,0 +1,23 @@
+#include "Drivetrain.h"
+#include "../RobotMap.h"
+#include "../Commands/Drivetrain/Drive.h"
+
+Drivetrain::Drivetrain() : Subsystem("Drivetrain"){
+  rightFront = new CANTalon(DRIVE_FRONT_RIGHT_CAN);
+  leftFront = new CANTalon(DRIVE_FRONT_LEFT_CAN);
+  rightRear = new CANTalon(DRIVE_BACK_RIGHT_CAN);
+  leftRear = new CANTalon(DRIVE_BACK_LEFT_CAN);
+}
+void Drivetrain::InitDefaultCommand(){
+  SetDefaultCommand(new Drive());
+}
+void Drivetrain::DriveMecanum(float x, float y, float z, float sensitivity, float gyro){
+  double correctY = (sensitivity*(pow(y,3))+(1-sensitivity)*y);
+  double correctX = -(sensitivity*(pow(x,3))+(1-sensitivity)*x);
+  double correctZ = -z *.5;
+  rightFront->Set((-correctX + correctY - correctZ));
+  leftFront->Set((correctX + correctY + correctZ)*-1);
+  rightRear->Set((correctX + correctY - correctZ));
+  leftRear->Set((-correctX + correctY + correctZ)*-1);
+}
+// vim: ts=2:sw=2:et

Subsystems/Drivetrain.h

@@ -0,0 +1,16 @@
+#ifndef DRIVETRAIN_H
+#define DRIVETRAIN_H
+
+#include "WPILib.h"
+class Drivetrain: public Subsystem{
+  private:
+    CANTalon *rightFront, *leftFront, *rightRear, *leftRear;
+    RobotDrive *drive;
+  public:
+    Drivetrain();
+    void InitDefaultCommand();
+    void DriveMecanum(float,float,float,float,float);
+    void DriveArcade(float, float);
+};
+#endif
+// vim: ts=2:sw=2:et

Subsystems/Elevator.cpp

@@ -0,0 +1,26 @@
+#include "Elevator.h"
+#include "../RobotMap.h"
+Elevator::Elevator(){
+  motor=new CANTalon(ELEVATOR_CAN);
+  elevatorEncoder=new Encoder(ELEVATOR_ENCODERA,ELEVATOR_ENCODERB,false);
+  elevatorBottom=new DigitalInput(ELEVATOR_BOTTOM_DIO);
+  elevatorTop=new DigitalInput(ELEVATOR_TOP_DIO);
+}
+void Elevator::InitDefaultCommand(){
+}
+void Elevator::Run(double power){
+  motor->Set(power);
+}
+void Elevator::ResetEncoder(){
+  elevatorEncoder->Reset();
+}
+double Elevator::GetHeight(){
+  return elevatorEncoder->Get();
+}
+bool Elevator::GetElevatorBottom(){
+  return elevatorBottom->Get();
+}
+bool Elevator::GetElevatorTop(){
+  return elevatorTop->Get();
+}
+// vim: ts=2:sw=2:et

Subsystems/Elevator.h

@@ -0,0 +1,22 @@
+#ifndef ELEVATOR_H
+#define ELEVATOR_H
+
+#include "WPILib.h"
+#include "Commands/PIDSubsystem.h"
+class Elevator{
+  private:
+    CANTalon *motor;
+    Encoder *elevatorEncoder;
+    static constexpr double kP_real=4, kI_real=.0f, kP_simulation=18, kI_simulation=.2;
+    DigitalInput *elevatorBottom, *elevatorTop;
+  public:
+    Elevator();
+    void InitDefaultCommand();
+    void Run(double);
+    void ResetEncoder();
+    double GetHeight();
+    bool GetElevatorTop();
+    bool GetElevatorBottom();
+};
+#endif
+// vim: ts=2:sw=2:et

classes/Collector.cpp

@@ -1,22 +0,0 @@
-#include "Collector.h"
-DentCollector::DentCollector(int fl, int fr, int rl, int rr){
-  collectorLeft = new Talon(fl);
-  collectorRight = new Talon(fr);
-  raiserLeft = new Talon(rl);
-  raiserRight = new Talon(rr);
-}
-void DentCollector::Collect(float power){
-  collectorLeft->Set(power);
-  collectorRight->Set(power);
-}
-void DentCollector::Raise(float power){
-  raiserLeft->Set(power);
-  raiserRight->Set(power);
-}
-void DentCollector::Rest(){
-  raiserLeft->Set(0);
-  raiserRight->Set(0);
-  collectorLeft->Set(0);
-  collectorRight->Set(0);
-}
-// vim: ts=2:sw=2:et

classes/Collector.h

@@ -1,15 +0,0 @@
-#ifndef __COLLECTOR_H__
-#include <WPILib.h>
-#define __COLLECTOR_H__
-//#include ""
-class DentCollector{
-  private:
-    Talon *collectorLeft, *collectorRight, *raiserLeft, *raiserRight;
-  public:
-    DentCollector(int, int, int, int);
-    void Collect(float);
-    void Raise(float);
-    void Rest();
-};
-#endif
-// vim: ts=2:sw=2:et

hhlib

@@ -1 +0,0 @@
-Subproject commit 787733b3d4204d061620c7906af0cf4777c47e34