From 09fe0ca9c657e145d7657dea342121adba18d954 Mon Sep 17 00:00:00 2001 From: Austen Adler Date: Thu, 12 Feb 2015 21:18:48 -0500 Subject: [PATCH] Removed HH* files, changed auto, implemented encoder (untested) --- Commands/Autonomous/Autonomous.cpp | 5 ++-- Commands/Elevator/Raise.cpp | 4 +++ DentRobot.cpp | 12 ++++++-- HHBase.cpp | 28 ------------------ HHBase.h | 26 ----------------- HHRobot.cpp | 46 ------------------------------ HHRobot.h | 19 ------------ OI.cpp | 4 +-- OI.h | 2 ++ Subsystems/Elevator.cpp | 12 ++++++++ Subsystems/Elevator.h | 3 ++ 11 files changed, 34 insertions(+), 127 deletions(-) delete mode 100644 HHBase.cpp delete mode 100644 HHBase.h delete mode 100644 HHRobot.cpp delete mode 100644 HHRobot.h diff --git a/Commands/Autonomous/Autonomous.cpp b/Commands/Autonomous/Autonomous.cpp index bc64654..0d0df58 100644 --- a/Commands/Autonomous/Autonomous.cpp +++ b/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()); diff --git a/Commands/Elevator/Raise.cpp b/Commands/Elevator/Raise.cpp index 6f900b4..cd8885f 100644 --- a/Commands/Elevator/Raise.cpp +++ b/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(){ diff --git a/DentRobot.cpp b/DentRobot.cpp index f5eafec..e801009 100644 --- a/DentRobot.cpp +++ b/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(){ } diff --git a/HHBase.cpp b/HHBase.cpp deleted file mode 100644 index 0023bc3..0000000 --- a/HHBase.cpp +++ /dev/null @@ -1,28 +0,0 @@ -#include "HHBase.h" -#include -#include -#include -#include -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 diff --git a/HHBase.h b/HHBase.h deleted file mode 100644 index c4e66e2..0000000 --- a/HHBase.h +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef __HH_BASE_H__ -#define __HH_BASE_H__ -#include -#include -#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 diff --git a/HHRobot.cpp b/HHRobot.cpp deleted file mode 100644 index 1020bc1..0000000 --- a/HHRobot.cpp +++ /dev/null @@ -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 diff --git a/HHRobot.h b/HHRobot.h deleted file mode 100644 index f6dc004..0000000 --- a/HHRobot.h +++ /dev/null @@ -1,19 +0,0 @@ -#ifndef __ROBOT_H__ -#define __ROBOT_H__ -#include -#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 diff --git a/OI.cpp b/OI.cpp index 54c4212..e01e119 100644 --- a/OI.cpp +++ b/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); diff --git a/OI.h b/OI.h index 7f0929f..357dbda 100644 --- a/OI.h +++ b/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 diff --git a/Subsystems/Elevator.cpp b/Subsystems/Elevator.cpp index 9264b4a..ffe24c3 100644 --- a/Subsystems/Elevator.cpp +++ b/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 diff --git a/Subsystems/Elevator.h b/Subsystems/Elevator.h index bdc0c8d..5454063 100644 --- a/Subsystems/Elevator.h +++ b/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