Removed HH* files, changed auto, implemented encoder (untested)

Commands/Autonomous/Autonomous.cpp

@@ -8,11 +8,10 @@
 #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());
+  AddParallel(new OpenCollector());
+  AddParallel(new CloseCollector());
   AddSequential(new Turn());
   AddParallel(new AutoDrive());
   AddParallel(new CloseCollector());

Commands/Elevator/Raise.cpp

@@ -18,6 +18,10 @@ bool Raise::IsFinished(){
   }
 }
 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;
@@ -32,12 +33,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(){
 }

HHBase.cpp

@@ -1,28 +0,0 @@
-#include "HHBase.h"
-#include <iostream>
-#include <fstream>
-#include <stdexcept>
-#include <map>
-HHBase::HHBase():
-  hhbot(new HHRobot()){
-    printf("Done\n");
-  }
-void HHBase::RobotInit(){
-}
-void HHBase::DisabledInit(){}
-void HHBase::AutonomousInit(){
-}
-void HHBase::TeleopInit(){
-}
-void HHBase::DisabledContinuous(){}
-void HHBase::AutonomousContinuous(){}
-void HHBase::TeleopContinuous(){}
-void HHBase::DisabledPeriodic(){}
-void HHBase::AutonomousPeriodic(){
-}
-void HHBase::TeleopPeriodic(){
-  hhbot->Handler();
-}
-void HHBase::Test(){}
-START_ROBOT_CLASS(HHBase);
-// vim: ts=2:sw=2:et

HHBase.h

@@ -1,26 +0,0 @@
-#ifndef __HH_BASE_H__
-#define __HH_BASE_H__
-#include <WPILib.h>
-#include <string>
-#include "HHRobot.h"
-//Because this is the first header to be included, classes need to be declared here
-class HHRobot;
-class HHBase : public IterativeRobot{
-  private:
-	HHRobot *hhbot;
-  public:
-    HHBase();
-    void RobotInit();
-    void DisabledInit();
-    void AutonomousInit();
-    void TeleopInit();
-    void DisabledContinuous();
-    void AutonomousContinuous();
-    void TeleopContinuous();
-    void DisabledPeriodic();
-    void AutonomousPeriodic();
-    void TeleopPeriodic();
-    void Test();
-};
-#endif
-// vim: ts=2:sw=2:et

HHRobot.cpp

@@ -1,46 +0,0 @@
-#include "HHRobot.h"
-#include "HHBase.h"
-HHRobot::HHRobot():
-  hhdrive(new RobotDrive(2,0,3,1)),
-  gyro(new Gyro(1)),
-  collector(new DentCollector(4, 5, 6, 7)),
-  driveStick(new Extreme3dPro(0)){
-    hhdrive->SetExpiration(0.1);
-    hhdrive->SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
-    hhdrive->SetInvertedMotor(RobotDrive::kRearLeftMotor,true);
-  }
-void HHRobot::Init(){
-  printf("Initing\n");
-  printf("Code Version: %f\n",0000.1);
-  gyro->Reset();
-}
-//Main function used to handle periodic tasks on the robot
-void HHRobot::Handler(){
-  const float Kp = 0.3;
-  if(driveStick->GetJoystickButton(1)==1){
-    hhdrive->MecanumDrive_Cartesian(driveStick->GetJoystickAxis("z"), 0, driveStick->GetJoystickAxis("x"));
-  }else if(driveStick->GetJoystickButton(2)==1){
-    hhdrive->MecanumDrive_Cartesian(driveStick->GetJoystickAxis("z"), driveStick->GetJoystickAxis("y"), 0);
-  }else if(driveStick->GetJoystickButton(3)==1){
-    hhdrive->Drive(driveStick->GetJoystickAxis("y"), driveStick->GetJoystickAxis("y")*Kp*-gyro->GetAngle());
-  }else{
-    hhdrive->MecanumDrive_Cartesian(driveStick->GetJoystickAxis("z"), driveStick->GetJoystickAxis("y"), driveStick->GetJoystickAxis("x"));
-  }
-  if(driveStick->GetJoystickButton(11)==1){
-    collector->Collect(driveStick->GetThrottle());
-  }else if(driveStick->GetJoystickButton(12)==1){
-    collector->Collect(-driveStick->GetThrottle());
-  }else if(driveStick->GetJoystickButton(9)==1){
-    collector->Raise(driveStick->GetThrottle());
-  }else if(driveStick->GetJoystickButton(10)==1){
-    collector->Raise(-driveStick->GetThrottle());
-  }else{
-    collector->Rest();
-  }
-  SmartDashboard::PutNumber("hambone1", driveStick->GetThrottle());
-  SmartDashboard::PutNumber("hambone2", driveStick->GetJoystickAxis("joystick"));
-  SmartDashboard::PutNumber("hambone3", driveStick->GetRawJoystickAxis(3));
-  printf("hambone2: %f", driveStick->GetThrottle());
-  Wait(0.005);
-}
-// vim: ts=2:sw=2:et

HHRobot.h

@@ -1,19 +0,0 @@
-#ifndef __ROBOT_H__
-#define __ROBOT_H__
-#include <WPILib.h>
-#include "HHBase.h"
-#include "classes/Collector.h"
-#include "hhlib/input/controller/Joystick.h"
-class HHRobot{
-  private:
-    RobotDrive *hhdrive;
-    Gyro *gyro;
-    DentCollector *collector;
-    Extreme3dPro *driveStick;
-  public:
-    HHRobot();
-    void Init();
-    void Handler();
-};
-#endif
-// vim: ts=2:sw=2:et

OI.cpp

@@ -20,8 +20,8 @@ OI::OI() {
   left1->WhileHeld(new CollectTote());
   left2->WhileHeld(new ReleaseTote());
   // Elevator
-  Raise* raise=new Raise();
-  Lower* lower=new Lower();
+  raise=new Raise();
+  lower=new Lower();
   JoystickButton *right3=new JoystickButton(rightStick, 3);
   JoystickButton *right4=new JoystickButton(rightStick, 4);
   JoystickButton *right5=new JoystickButton(rightStick, 5);

OI.h

@@ -2,6 +2,7 @@
 #define OI_H
 
 #include "WPILib.h"
+#include "Commands/Command.h"
 
 class OI
 {
@@ -11,6 +12,7 @@ class OI
     OI();
     Joystick* GetRightStick();
     Joystick* GetLeftStick();
+    Command *raise, *lower;
 };
 #endif
 // vim: ts=2:sw=2:et

Subsystems/Elevator.cpp

@@ -5,10 +5,16 @@ Elevator::Elevator(){
   elevatorEncoder=new Encoder(ELEVATOR_ENCODERA,ELEVATOR_ENCODERB,false);
   elevatorBottom=new DigitalInput(ELEVATOR_BOTTOM_DIO);
   elevatorTop=new DigitalInput(ELEVATOR_TOP_DIO);
+  // Checks if the elevator is drifting
+  useEncoder=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(){
@@ -23,4 +29,10 @@ bool Elevator::GetElevatorBottom(){
 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

@@ -9,6 +9,7 @@ class Elevator{
     Encoder *elevatorEncoder;
     static constexpr double kP_real=4, kI_real=.0f, kP_simulation=18, kI_simulation=.2;
     DigitalInput *elevatorBottom, *elevatorTop;
+    bool useEncoder;
   public:
     Elevator();
     void InitDefaultCommand();
@@ -17,6 +18,8 @@ class Elevator{
     double GetHeight();
     bool GetElevatorTop();
     bool GetElevatorBottom();
+    void SetUseEncoder(bool);
+    bool GetUseEncoder();
 };
 #endif
 // vim: ts=2:sw=2:et