Added binElevator (untested)

CommandBase.cpp

@@ -2,9 +2,11 @@
 #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) {
 }
@@ -14,6 +16,7 @@ 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

@@ -5,6 +5,7 @@
 #include "Subsystems/Drivetrain.h"
 #include "Subsystems/Collector.h"
 #include "Subsystems/Elevator.h"
+#include "Subsystems/BinElevator.h"
 #include "OI.h"
 #include "WPILib.h"
 
@@ -16,6 +17,7 @@ class CommandBase: public Command {
     static Drivetrain *drivetrain;
     static Collector *collector;
     static Elevator *elevator;
+    static BinElevator *binElevator;
     static OI *oi;
 };
 #endif

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

@@ -1,12 +1,12 @@
-#ifndef CALIBRATE_H
+#ifndef BINLOWER_H
-#define CALIBRATE_H
+#define BINLOWER_H
 
 #include "Commands/Command.h"
 #include "WPILib.h"
 
-class Calibrate: public Command{
+class BinLower: public Command{
   public:
-    Calibrate();
+    BinLower();
     void Initialize();
     void Execute();
     bool IsFinished();

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/Elevator/Calibrate.cpp

@@ -1,26 +0,0 @@
-#include "Calibrate.h"
-#include "../../DentRobot.h"
-// Lowers elevator until it hits the limit switch then sets the height of the elevator to the height of the limit switches
-Calibrate::Calibrate() : Command("Calibrate"){
-}
-void Calibrate::Initialize(){
-}
-void Calibrate::Execute(){
-  // Lower collector until End()
-  DentRobot::elevator->Run(-0.4f);
-}
-bool Calibrate::IsFinished(){
-  if(DentRobot::elevator->GetElevatorBottom()){
-    DentRobot::elevator->ResetEncoder();
-    DentRobot::elevator->SetOffset(0.99);
-    return true;
-  }
-  return false;
-}
-void Calibrate::End(){
-  DentRobot::elevator->Run(0.0f);
-}
-void Calibrate::Interrupted(){
-  End();
-}
-// vim: ts=2:sw=2:et

DentRobot.cpp

@@ -4,12 +4,14 @@ 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();
   printf("Initialized");
 }

DentRobot.h

@@ -3,6 +3,7 @@
 #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"
@@ -15,6 +16,7 @@ class DentRobot: public IterativeRobot {
     static Collector* collector;
     static Drivetrain* drivetrain;
     static Elevator* elevator;
+    static BinElevator* binElevator;
     static CommandGroup* aut;
     void RobotInit();
     void DisabledPeriodic();

Subsystems/BinElevator.cpp

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

Subsystems/BinElevator.h

@@ -0,0 +1,24 @@
+#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;
+    double offset, height;
+    DigitalInput *elevatorBottom, *elevatorTop;
+  public:
+    BinElevator();
+    void InitDefaultCommand();
+    void Run(double);
+    void SetOffset(double);
+    void ResetEncoder();
+    double GetHeight();
+    bool GetElevatorTop();
+    bool GetElevatorBottom();
+};
+#endif
+// vim: ts=2:sw=2:et