Merge branch 'develop'

Commands/Autonomous/AutoDrive.cpp

@@ -1,20 +1,32 @@
 #include "AutoDrive.h"
 #include "../../DentRobot.h"
 // Drive for a short while then stop. Just for testing
-AutoDrive::AutoDrive() : Command("AutoDrive"){
+AutoDrive::AutoDrive(double wait) : Command("AutoDrive"){
   Requires(DentRobot::drivetrain);
-  SetTimeout(0.5);
+  SetTimeout(wait);
+  power=5;
+}
+AutoDrive::AutoDrive(double wait, double p) : Command("AutoDrive"){
+  Requires(DentRobot::drivetrain);
+  SetTimeout(wait);
+  power=p;
 }
 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);
+  //if(power==NULL){
+  if(power==5){
+    DentRobot::drivetrain->DriveMecanum(0.0,-.75,0.0,0.9,0.0);
+  }else{
+    DentRobot::drivetrain->DriveMecanum(0.0,power,0.0,0.9,0.0);
+  }
 }
 bool AutoDrive::IsFinished(){
   return IsTimedOut();
 }
 void AutoDrive::End(){
+  DentRobot::drivetrain->DriveMecanum(0.0,0.0,0.0,0.9,0.0);
 }
 void AutoDrive::Interrupted(){
   End();

Commands/Autonomous/AutoDrive.h

@@ -7,8 +7,11 @@
 #include "WPILib.h"
 
 class AutoDrive: public Command{
+  private:
+    double power;
   public:
-    AutoDrive();
+    AutoDrive(double);
+    AutoDrive(double, double);
     void Initialize();
     void Execute();
     bool IsFinished();

Commands/Autonomous/Autonomous.cpp

@@ -4,19 +4,46 @@
 #include "../Elevator/Lower.h"
 #include "AutoDrive.h"
 #include "Turn.h"
-#include "../Collector/CloseCollector.h"
-#include "../Collector/OpenCollector.h"
+#include "../Collector/RollIn.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());
+Autonomous::Autonomous(int seq){
+  //SmartDashboard::GetNumber("Auto Wait Time");
+  switch(seq){
+    case 0:
+      // Just for testing
+      AddSequential(new CollectTote());
+      AddSequential(new Turn());
+      //AddSequential(new Raise());
+      //AddSequential(new Lower());
+      //AddSequential(new Turn());
+      //AddParallel(new AutoDrive(0.5));
+      //AddSequential(new RollIn());
+      //AddSequential(new Turn());
+      break;
+    case 1:
+      // Drive forward a bit, turn around, collect tote then bin
+      AddSequential(new AutoDrive(0.5));
+      AddSequential(new Turn());
+      AddSequential(new Turn());
+      AddSequential(new RollIn());
+      AddSequential(new Raise());
+      break;
+    case 2:
+      // Drive forward a bit, turn around, collect tote then bin
+      AddParallel(new Raise());
+      AddParallel(new AutoDrive(0.5));
+      AddSequential(new Turn());
+      AddSequential(new Turn());
+      AddSequential(new RollIn());
+      break;
+    case 3:
+      // Wait a desigated value, drive to Auto Zone (TM)
+      //Wait(SmartDashboard::GetNumber("Auto Wait Time"));
+      AddSequential(new AutoDrive(2.0));
+      break;
+    default:
+      printf("Invalid seq: %d\n", seq);
+      break;
+  }
 }
 // vim: ts=2:sw=2:et

Commands/Autonomous/Autonomous.h

@@ -8,7 +8,7 @@
 
 class Autonomous: public CommandGroup{
   public:
-    Autonomous();
+    Autonomous(int);
 };
 #endif
 // vim: ts=2:sw=2:et

Commands/Autonomous/Turn.cpp

@@ -3,18 +3,20 @@
 // Drive for a short while then stop. Just for testing
 Turn::Turn() : Command("Turn"){
   Requires(DentRobot::drivetrain);
-  SetTimeout(0.25);
+  SetTimeout(0.85);
 }
 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);
+  DentRobot::drivetrain->DriveMecanum(0.0,0.0,1.0,0.9,0.0);
 }
 bool Turn::IsFinished(){
   return IsTimedOut();
 }
 void Turn::End(){
+  // Stop driving
+  DentRobot::drivetrain->DriveMecanum(0.0,0.0,0.0,0.9,0.0);
 }
 void Turn::Interrupted(){
   End();

Commands/BinElevator/BinLower.cpp

@@ -10,7 +10,7 @@ void BinLower::Execute(){
   DentRobot::binElevator->Run(-1.0);
 }
 bool BinLower::IsFinished(){
-  if (!DentRobot::binElevator->GetElevatorBottom()||IsTimedOut()){
+  if (/*!DentRobot::binElevator->GetElevatorBottom()||*/IsTimedOut()){
     printf("Robot stoped BinLowering. Sensor based? %d\n", !DentRobot::binElevator->GetElevatorBottom());
     return true;
   }else{

Commands/BinElevator/BinRaise.cpp

@@ -10,7 +10,7 @@ void BinRaise::Execute(){
   DentRobot::binElevator->Run(1.0);
 }
 bool BinRaise::IsFinished(){
-  if (!DentRobot::binElevator->GetElevatorTop()||IsTimedOut()){
+  if (/*!DentRobot::binElevator->GetElevatorTop()||*/IsTimedOut()){
     printf("Robot stoped raising. Sensor based? %d\n", !DentRobot::binElevator->GetElevatorTop());
     return true;
   }else{

Commands/Collector/CloseCollector.cpp

@@ -1,28 +0,0 @@
-#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/CollectTote.cpp

@@ -1,22 +1,9 @@
 #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();
+#include "../../DentRobot.h"
+#include "../Autonomous/AutoDrive.h"
+#include "RollIn.h"
+CollectTote::CollectTote(){
+  AddParallel(new AutoDrive(1.0, -0.75));
+  AddSequential(new RollIn());
 }
 // vim: ts=2:sw=2:et

Commands/Collector/CollectTote.h

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

Commands/Collector/OpenCollector.cpp

@@ -1,22 +0,0 @@
-#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/ReleaseTote.cpp

@@ -1,22 +1,9 @@
 #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();
+#include "../../DentRobot.h"
+#include "RollOut.h"
+#include "../Autonomous/AutoDrive.h"
+ReleaseTote::ReleaseTote(){
+  AddParallel(new RollOut());
+  AddParallel(new AutoDrive(0.5, 0.75));
 }
 // vim: ts=2:sw=2:et

Commands/Collector/ReleaseTote.h

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

Commands/Collector/RollIn.cpp

@@ -0,0 +1,26 @@
+#include "RollIn.h"
+RollIn::RollIn() : Command("RollIn"){
+}
+void RollIn::Initialize(){
+  printf("Initialized RollIn\n");
+  SetTimeout(2.0);
+}
+void RollIn::Execute(){
+  double throttle=DentRobot::oi->GetLeftThrottle();
+  double cvt=throttle*DentRobot::collector->GetSonarDistance()/0.4;
+  if(cvt>=1.0){
+    DentRobot::collector->MoveRollers(1.0);
+  }else{
+    DentRobot::collector->MoveRollers(cvt);
+  }
+}
+bool RollIn::IsFinished(){
+  return IsTimedOut();
+}
+void RollIn::End(){
+  DentRobot::collector->MoveRollers(0.0);
+}
+void RollIn::Interrupted(){
+  End();
+}
+// vim: ts=2:sw=2:et

Commands/Collector/RollIn.h

@@ -1,14 +1,14 @@
-#ifndef OPENCOLLECTOR_H
-#define OPENCOLLECTOR_H
+#ifndef ROLLIN_H
+#define ROLLIN_H
 
 #include "Commands/Command.h"
 #include "../../CommandBase.h"
 #include "../../DentRobot.h"
 #include "WPILib.h"
 
-class OpenCollector: public Command{
+class RollIn: public Command{
   public:
-    OpenCollector();
+    RollIn();
     void Initialize();
     void Execute();
     bool IsFinished();

Commands/Collector/RollOut.cpp

@@ -0,0 +1,22 @@
+#include "RollOut.h"
+RollOut::RollOut() : Command("RollOut"){
+  Requires(DentRobot::collector);
+}
+void RollOut::Initialize(){
+  SetTimeout(2.0);
+}
+void RollOut::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->GetLeftThrottle());
+}
+bool RollOut::IsFinished(){
+  return IsTimedOut();
+}
+void RollOut::End(){
+  DentRobot::collector->MoveRollers(0.0f);
+}
+void RollOut::Interrupted(){
+  End();
+}
+// vim: ts=2:sw=2:et

Commands/Collector/RollOut.h

@@ -1,14 +1,14 @@
-#ifndef CLOSECOLLECTOR_H
-#define CLOSECOLLECTOR_H
+#ifndef ROLLOUT_H
+#define ROLLOUT_H
 
 #include "Commands/Command.h"
 #include "../../CommandBase.h"
 #include "../../DentRobot.h"
 #include "WPILib.h"
 
-class CloseCollector: public Command{
+class RollOut: public Command{
   public:
-    CloseCollector();
+    RollOut();
     void Initialize();
     void Execute();
     bool IsFinished();

Commands/Drivetrain/Drive.cpp

@@ -21,7 +21,7 @@ void Drive::Execute(){
   DentRobot::drivetrain->DriveMecanum(x,y,z,0.9,0);
 }
 bool Drive::IsFinished(){
-  return false;
+  return IsTimedOut();
 }
 void Drive::End(){
 }

Commands/Elevator/Raise.cpp

@@ -10,14 +10,26 @@ 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());
+  //if(!DentRobot::elevator->GetElevatorMiddle()){
+  //  DentRobot::elevator->stoppedAtSensor=true;
+  //}
+  //if ((DentRobot::elevator->stoppedAtSensor)){
+  //  printf("Stopped at the middle sensor\n");
+  //  DentRobot::elevator->stoppedAtSensor=false;
+  //  return true;
+  //}else if (!DentRobot::elevator->GetElevatorTop()) {
+  if (!DentRobot::elevator->GetElevatorTop()||!DentRobot::elevator->GetElevatorMiddle()||IsTimedOut()){
+    printf("Robot stopped raising. Sensor based? %d\n", !DentRobot::elevator->GetElevatorTop()||!DentRobot::elevator->GetElevatorMiddle());
     return true;
   }else{
     return false;
   }
 }
 void Raise::End(){
+  // If the elevator is at the top
+  if(DentRobot::elevator->GetElevatorTop()){
+    DentRobot::elevator->SetUseEncoder(true);
+  }
   DentRobot::elevator->Run(0.0f);
 }
 void Raise::Interrupted(){

DentRobot.cpp

@@ -1,4 +1,5 @@
 #include "DentRobot.h"
+#include "OI.h"
 #include "Commands/Autonomous/Autonomous.h"
 OI* DentRobot::oi=NULL;
 Collector* DentRobot::collector=NULL;
@@ -12,13 +13,15 @@ DentRobot::DentRobot(){
   drivetrain=new Drivetrain();
   elevator=new Elevator();
   binElevator=new BinElevator();
-  aut=new Autonomous();
+  aut=new Autonomous(0);
   CameraServer::GetInstance()->SetQuality(25);
   CameraServer::GetInstance()->StartAutomaticCapture("cam0");
-  printf("Initialized");
+  //SmartDashboard::PutNumber("Auto Wait Time", 1.0);
+  //SmartDashboard::PutNumber("Auto Sequence", 0);
+  printf("Initialized\n");
 }
 void DentRobot::RobotInit(){
-  SmartDashboard::PutNumber("CodeVersion",0.001);
+  //SmartDashboard::PutNumber("CodeVersion",1.0);
 }
 void DentRobot::DisabledPeriodic(){
   Scheduler::GetInstance()->Run();
@@ -32,12 +35,17 @@ void DentRobot::AutonomousPeriodic(){
   Scheduler::GetInstance()->Run();
 }
 void DentRobot::TeleopInit(){
-  //if (aut != NULL){
-  //  aut->Cancel();
-  //}
+  if (aut != NULL){
+    aut->Cancel();
+  }
 }
 void DentRobot::TeleopPeriodic(){
   Scheduler::GetInstance()->Run();
+  // TODO: Calibrate 1.0 to the height we want the elevator to automatically raise
+  if(elevator->GetUseEncoder()&&elevator->GetHeight()<=-1.0){
+    // Raise the elevator if it dips below elevatorTop
+    oi->raise->Start();
+  }
 }
 void DentRobot::TestPeriodic(){
 }

Makefile

@@ -9,6 +9,7 @@ EXEC=bin/FRCUserProgram
 CLEANSER=rm -r
 
 all : $(OBJECTS)
+	if [ ! -d bin ];then mkdir bin; fi
 	$(CC) -L$(WPILIB)/lib $(LDFLAGS) -o $(EXEC) $(OBJECTS) -lwpi
 
 %.o : %.cpp
@@ -17,10 +18,10 @@ all : $(OBJECTS)
 clean:
 	$(CLEANSER) $(OBJECTS) bin/FRCUserProgram
 
-deploy:
+deploy: all
 	@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:
+debug: all
 	@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:

OI.cpp

@@ -1,43 +1,44 @@
 #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"
+#include "Commands/Collector/RollIn.h"
+#include "Commands/Collector/RollOut.h"
+#include "Commands/BinElevator/BinLower.h"
+#include "Commands/BinElevator/BinRaise.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());
+  left1->WhileHeld(new RollIn());
+  left2->WhileHeld(new RollOut());
   // Elevator
-  Raise* raise=new Raise();
-  Lower* lower=new Lower();
-  JoystickButton *right3=new JoystickButton(rightStick, 3);
+  raise=new Raise();
+  lower=new Lower();
   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);
+  // BinElevator
+  JoystickButton *right3=new JoystickButton(rightStick, 3);
+  JoystickButton *right5=new JoystickButton(rightStick, 5);
+  binRaise=new BinRaise();
+  binLower=new BinLower();
+  right3->WhenPressed(binLower);
+  right3->CancelWhenPressed(binRaise);
+  right5->WhenPressed(binRaise);
+  right5->CancelWhenPressed(binLower);
   // Cancel
   JoystickButton *right12=new JoystickButton(rightStick, 12);
   right12->CancelWhenPressed(raise);
   right12->CancelWhenPressed(lower);
+  right12->CancelWhenPressed(binRaise);
+  right12->CancelWhenPressed(binLower);
 }
 Joystick* OI::GetRightStick(){
   return rightStick;
@@ -45,4 +46,10 @@ Joystick* OI::GetRightStick(){
 Joystick* OI::GetLeftStick(){
   return leftStick;
 }
+double OI::GetRightThrottle(){
+  return (-rightStick->GetRawAxis(3)+1.0)/2;
+}
+double OI::GetLeftThrottle(){
+  return (-leftStick->GetRawAxis(3)+1.0)/2;
+}
 // vim: ts=2:sw=2:et

OI.h

@@ -2,6 +2,7 @@
 #define OI_H
 
 #include "WPILib.h"
+#include "Commands/Command.h"
 
 class OI
 {
@@ -9,8 +10,11 @@ class OI
     Joystick *leftStick, *rightStick;
   public:
     OI();
+    Command *raise, *lower, *binLower, *binRaise;
     Joystick* GetRightStick();
     Joystick* GetLeftStick();
+    double GetLeftThrottle();
+    double GetRightThrottle();
 };
 #endif
 // vim: ts=2:sw=2:et

RobotMap.h

@@ -5,20 +5,19 @@
 
 // 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
+#define ELEVATOR_BOTTOM_DIO 9
+#define ELEVATOR_MIDDLE_DIO 8
+#define ELEVATOR_TOP_DIO 7
+#define ELEVATOR_ENCODERA 4
+#define ELEVATOR_ENCODERB 9
 
 // 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
+#define BINELEVATOR_ENCODERA 10
+#define BINELEVATOR_ENCODERB 11
 
 // Drivetrain
 #define DRIVE_FRONT_LEFT_CAN 2
@@ -27,11 +26,11 @@
 #define DRIVE_BACK_RIGHT_CAN 5
 
 // Collector
-#define COLLECTOR_WINDOW_LEFT_CAN 6
-#define COLLECTOR_WINDOW_RIGHT_CAN 7
+#define COLLECTOR_RAMP_CAN 7
 #define COLLECTOR_LEFT_CAN 8
 #define COLLECTOR_BOTTOM_CAN 10
 #define COLLECTOR_RIGHT_CAN 9
+#define COLLECTOR_SONAR_ANALOG 3
 
 #endif
 // vim: ts=2:sw=2:et

Subsystems/Collector.cpp

@@ -1,30 +1,23 @@
 #include "Collector.h"
 #include "../RobotMap.h"
 
-Collector::Collector() : Subsystem("Collector") {
-  windowMotorLeft=new CANTalon(COLLECTOR_WINDOW_LEFT_CAN);
-  windowMotorRight=new CANTalon(COLLECTOR_WINDOW_RIGHT_CAN);
+Collector::Collector() : Subsystem("Collector"){
   collectorMotorLeft=new CANTalon(COLLECTOR_LEFT_CAN);
   collectorMotorBottom=new CANTalon(COLLECTOR_BOTTOM_CAN);
+  collectorMotorRamp=new CANTalon(COLLECTOR_RAMP_CAN);
   collectorMotorRight=new CANTalon(COLLECTOR_RIGHT_CAN);
+  sonarAnalog=new AnalogInput(COLLECTOR_SONAR_ANALOG);
 }
-void Collector::InitDefaultCommand() {
-}
-void Collector::MoveArms(double a){
-  windowMotorLeft->Set(a);
-  windowMotorRight->Set(-a);
+void Collector::InitDefaultCommand(){
 }
 void Collector::MoveRollers(double a){
   collectorMotorLeft->Set(a);
-  collectorMotorBottom->Set(a);
+  collectorMotorBottom->Set(-a);
+  collectorMotorRamp->Set(a);
   collectorMotorRight->Set(-a);
+  printf("Roller power: %f\n",a);
 }
-bool Collector::ArmSensor(){
-  // TODO: include limit switch code
-  return false;
-}
-bool Collector::BoxCollected(){
-  return false;
-  //return boxSwitch->Get();
+double Collector::GetSonarDistance(){
+  return sonarAnalog->GetAverageVoltage();
 }
 // vim: ts=2:sw=2:et

Subsystems/Collector.h

@@ -5,14 +5,14 @@
 class Collector: public Subsystem
 {
   private:
-    CANTalon *windowMotorLeft, *windowMotorRight, *collectorMotorLeft, *collectorMotorBottom, *collectorMotorRight;
+    CANTalon *collectorMotorLeft, *collectorMotorBottom, *collectorMotorRamp, *collectorMotorRight;
+    AnalogInput *sonarAnalog;
+    DigitalOutput *sonarDigital;
   public:
     Collector();
     void InitDefaultCommand();
-    void MoveArms(double);
     void MoveRollers(double);
-    bool ArmSensor();
-    bool BoxCollected();
+    double GetSonarDistance();
 };
 #endif
 // vim: ts=2:sw=2:et

Subsystems/Elevator.cpp

@@ -4,11 +4,19 @@ Elevator::Elevator(){
   motor=new CANTalon(ELEVATOR_CAN);
   elevatorEncoder=new Encoder(ELEVATOR_ENCODERA,ELEVATOR_ENCODERB,false);
   elevatorBottom=new DigitalInput(ELEVATOR_BOTTOM_DIO);
+  elevatorMiddle=new DigitalInput(ELEVATOR_MIDDLE_DIO);
   elevatorTop=new DigitalInput(ELEVATOR_TOP_DIO);
+  // Checks if the elevator is drifting
+  useEncoder=false;
+  stoppedAtSensor=false;
 }
 void Elevator::InitDefaultCommand(){
 }
 void Elevator::Run(double power){
+  // If we're not telling it to stop
+  if(power != 0.0){
+    SetUseEncoder(false);
+  }
   motor->Set(power);
 }
 void Elevator::ResetEncoder(){
@@ -20,7 +28,16 @@ double Elevator::GetHeight(){
 bool Elevator::GetElevatorBottom(){
   return elevatorBottom->Get();
 }
+bool Elevator::GetElevatorMiddle(){
+  return elevatorMiddle->Get();
+}
 bool Elevator::GetElevatorTop(){
   return elevatorTop->Get();
 }
+void Elevator::SetUseEncoder(bool param){
+  useEncoder=param;
+}
+bool Elevator::GetUseEncoder(){
+  return useEncoder;
+}
 // vim: ts=2:sw=2:et

Subsystems/Elevator.h

@@ -8,15 +8,20 @@ class Elevator{
     CANTalon *motor;
     Encoder *elevatorEncoder;
     static constexpr double kP_real=4, kI_real=.0f, kP_simulation=18, kI_simulation=.2;
-    DigitalInput *elevatorBottom, *elevatorTop;
+    DigitalInput *elevatorBottom, *elevatorMiddle, *elevatorTop;
+    bool useEncoder;
   public:
     Elevator();
+    bool stoppedAtSensor;
     void InitDefaultCommand();
     void Run(double);
     void ResetEncoder();
     double GetHeight();
     bool GetElevatorTop();
+    bool GetElevatorMiddle();
     bool GetElevatorBottom();
+    void SetUseEncoder(bool);
+    bool GetUseEncoder();
 };
 #endif
 // vim: ts=2:sw=2:et