Added a lot of collector code, needs fine tuning and calibration

CommandBase.cpp

@@ -5,7 +5,10 @@
 #include "Subsystems/DIO.h"
 #include "Subsystems/AirCompressor.h"
 #include "Commands/Drive.h"
-#include "Commands/Collect.h"
+#include "Commands/CloseCollector.h"
+#include "Commands/OpenCollector.h"
+#include "Commands/CollectTote.h"
+#include "Commands/ReleaseTote.h"
 #include "Commands/Eject.h"
 #include "Commands/Raise.h"
 #include "Commands/Lower.h"

Commands/CloseCollector.cpp

@@ -0,0 +1,16 @@
+#include "CloseCollector.h"
+CloseCollector::CloseCollector() : Command("CloseCollector"){
+    Requires(DentRobot::collector);
+}
+void CloseCollector::Initialize(){
+}
+void CloseCollector::Execute(){
+    DentRobot::collector->MoveArms(.1);
+}
+bool CloseCollector::IsFinished(){
+    return DentRobot::collector->ArmsDoneMoving();
+}
+void CloseCollector::End(){
+}
+void CloseCollector::Interrupted(){
+}

Commands/CloseCollector.h

@@ -0,0 +1,19 @@
+#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

Commands/Collect.cpp

@@ -1,14 +0,0 @@
-#include "Collect.h"
-Collect::Collect() : Command("Collect"){
-}
-void Collect::Initialize(){
-}
-void Collect::Execute(){
-}
-bool Collect::IsFinished(){
-  return false;
-}
-void Collect::End(){
-}
-void Collect::Interrupted(){
-}

Commands/CollectTote.cpp

@@ -0,0 +1,17 @@
+#include "CollectTote.h"
+CollectTote::CollectTote() : Command("CollectTote"){
+    Requires(DentRobot::collector);
+}
+void CollectTote::Initialize(){
+}
+void CollectTote::Execute(){
+    //TODO check this value to move the motors in the right direction
+    DentRobot::collector->MoveRollers(-1);
+}
+bool CollectTote::IsFinished(){
+    return DentRobot::collector->BoxCollected();
+}
+void CollectTote::End(){
+}
+void CollectTote::Interrupted(){
+}

Commands/CollectTote.h

@@ -1,12 +1,14 @@
-#ifndef COLLECT_H
-#define COLLECT_H
+#ifndef COLLECTTOTE_H
+#define COLLECTTOTE_H
 
 #include "Commands/Command.h"
+#include "../CommandBase.h"
+#include "../DentRobot.h"
 #include "WPILib.h"
 
-class Collect: public Command{
+class CollectTote: public Command{
   public:
-    Collect();
+    CollectTote();
     void Initialize();
     void Execute();
     bool IsFinished();

Commands/Eject.cpp

@@ -6,7 +6,6 @@ Eject::Eject() : Command("Eject"){
 void Eject::Initialize(){
 }
 void Eject::Execute(){
-  DentRobot::collector->Set(DentRobot::oi->GetLeftStick()->GetThrottle());
 }
 bool Eject::IsFinished(){
   return false;

Commands/OpenCollector.cpp

@@ -0,0 +1,17 @@
+#include "OpenCollector.h"
+OpenCollector::OpenCollector() : Command("OpenCollector"){
+    Requires(DentRobot::collector);
+}
+void OpenCollector::Initialize(){
+}
+void OpenCollector::Execute(){
+    //TODO check this value to move the motors in the right direction
+    DentRobot::collector->MoveArms(-.1);
+}
+bool OpenCollector::IsFinished(){
+    return DentRobot::collector->ArmsDoneMoving();
+}
+void OpenCollector::End(){
+}
+void OpenCollector::Interrupted(){
+}

Commands/OpenCollector.h

@@ -0,0 +1,19 @@
+#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

Commands/ReleaseTote.cpp

@@ -0,0 +1,17 @@
+#include "ReleaseTote.h"
+ReleaseTote::ReleaseTote() : Command("ReleaseTote"){
+    Requires(DentRobot::collector);
+}
+void ReleaseTote::Initialize(){
+}
+void ReleaseTote::Execute(){
+    //TODO check this value to move the motors in the right direction
+    DentRobot::collector->MoveRollers(1);
+}
+bool ReleaseTote::IsFinished(){
+    return DentRobot::collector->BoxCollected();
+}
+void ReleaseTote::End(){
+}
+void ReleaseTote::Interrupted(){
+}

Commands/ReleaseTote.h

@@ -0,0 +1,19 @@
+#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

OI.cpp

@@ -1,18 +1,29 @@
 #include "OI.h"
 #include "Commands/Lower.h"
 #include "Commands/Raise.h"
-#include "Commands/Collect.h"
 #include "Commands/Eject.h"
+#include "Commands/OpenCollector.h"
+#include "Commands/CloseCollector.h"
+#include "Commands/CollectTote.h"
+#include "Commands/ReleaseTote.h"
 #include "Commands/Compressor/StartCompressing.h"
 #include "Commands/Compressor/StopCompressing.h"
 
 OI::OI() {
   leftStick=new Joystick(0);
   rightStick=new Joystick(1);
+  //TODO name these buttons to their functions rather to their number
   JoystickButton *left10=new JoystickButton(leftStick, 10);
   JoystickButton *left11=new JoystickButton(leftStick, 11);
+  JoystickButton *right1=new JoystickButton(rightStick, 1);
+  JoystickButton *right2=new JoystickButton(rightStick, 2);
+  JoystickButton *right3=new JoystickButton(rightStick, 3);
+  JoystickButton *right4=new JoystickButton(rightStick, 4);
+  right1->WhenPressed(new OpenCollector());
+  right2->WhenPressed(new CloseCollector());
+  right3->WhenPressed(new CollectTote());
+  right4->WhenPressed(new ReleaseTote());
   left10->WhenPressed(new Eject());
-  left11->WhenPressed(new Collect());
 }
 Joystick* OI::GetRightStick(){
   return rightStick;

Subsystems/Collector.cpp

@@ -2,12 +2,32 @@
 #include "../RobotMap.h"
 
 Collector::Collector() : Subsystem("Collector") {
-  motor1=new Talon(0);
-  motor2=new Talon(1);
+  windowMotorLeft=new CANTalon(50);
+  windowMotorRight=new CANTalon(51);
+  collectorMotorLeft=new CANTalon(52);
+  collectorMotorRight=new CANTalon(53);
+  boxSwitch=new DigitalInput(9);
 }
 void Collector::InitDefaultCommand() {
 }
-void Collector::Set(float power){
-  motor1->Set(power);
-  motor2->Set(power);
+void Collector::MoveArms(float a){
+    windowMotorLeft->Set(a);
+    windowMotorRight->Set(-a);
+    a++;
+}
+void Collector::MoveRollers(float a){
+    collectorMotorLeft->Set(a);
+    collectorMotorRight->Set(-a);
+    r++;
+}
+bool Collector::ArmsDoneMoving(){
+    //TODO calibrate these values or find a sensor to use
+    if(a>=200){
+        return true;
+    }else{
+        return false;
+    }
+}
+bool Collector::BoxCollected(){
+    return boxSwitch->Get();
 }

Subsystems/Collector.h

@@ -5,10 +5,15 @@
 class Collector: public Subsystem
 {
   private:
-    Talon *motor1, *motor2;
+      CANTalon *windowMotorLeft, *windowMotorRight, *collectorMotorLeft, *collectorMotorRight;
+      DigitalInput *boxSwitch;
+      int a,r;
   public:
     Collector();
     void InitDefaultCommand();
-    void Set(float);
+    void MoveArms(float);
+    void MoveRollers(float);
+    bool ArmsDoneMoving();
+    bool BoxCollected();
 };
 #endif

hhlib

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