From b9b25144b65ad8490b32c87d43ab2192affa16c8 Mon Sep 17 00:00:00 2001 From: Adam Long Date: Tue, 7 Oct 2014 06:33:29 -0400 Subject: [PATCH] Added shooter angle limits, sonar auto driving, and a base for autonomous --- src/Auto/AutoMain.cpp | 5 +++++ src/Auto/AutoMain.h | 6 ++++++ src/Definitions.h | 5 +++++ src/HHBase.cpp | 4 +++- src/HHRobot.cpp | 38 ++++++++++++++++++++++++++++++++++-- src/HHRobot.h | 5 +++++ src/Subsystems/Collector.cpp | 3 +++ src/Subsystems/Collector.h | 1 + src/Subsystems/Shooter.cpp | 4 ++-- src/Subsystems/Shooter.h | 2 +- 10 files changed, 67 insertions(+), 6 deletions(-) create mode 100644 src/Auto/AutoMain.cpp create mode 100644 src/Auto/AutoMain.h diff --git a/src/Auto/AutoMain.cpp b/src/Auto/AutoMain.cpp new file mode 100644 index 0000000..974c242 --- /dev/null +++ b/src/Auto/AutoMain.cpp @@ -0,0 +1,5 @@ +#include "AutoMain.h" +HHAuto::HHAuto(){ + +} + diff --git a/src/Auto/AutoMain.h b/src/Auto/AutoMain.h new file mode 100644 index 0000000..375f74c --- /dev/null +++ b/src/Auto/AutoMain.h @@ -0,0 +1,6 @@ +#include +#include "../Definitions.h" +class HHAuto{ + public: + HHAuto(); +}; diff --git a/src/Definitions.h b/src/Definitions.h index 178e06d..32c3ece 100644 --- a/src/Definitions.h +++ b/src/Definitions.h @@ -65,6 +65,11 @@ #define COLLECTOR_OUTTAKE 2 #define COLLECTOR_EXTEND 9 #define COLLECTOR_RETRACT 10 +#define SHOOTER_ANGLE_ONE 3 +#define SHOOTER_ANGLE_TWO 4 +#define SHOOTER_ANGLE_THREE 5 +#define SHOOTER_ANGLE_FOUR 6 +#define DRIVE_FOR_DISTANCE 11 //Drive threshold definitions diff --git a/src/HHBase.cpp b/src/HHBase.cpp index 3bd972e..85d21fb 100644 --- a/src/HHBase.cpp +++ b/src/HHBase.cpp @@ -31,7 +31,9 @@ void HHBase::DisabledContinuous(){} void HHBase::AutonomousContinuous(){} void HHBase::TeleopContinuous(){} void HHBase::DisabledPeriodic(){} -void HHBase::AutonomousPeriodic(){} +void HHBase::AutonomousPeriodic(){ + hHBot->RunAuto(); +} void HHBase::TeleopPeriodic(){ hHBot->Handler(); } diff --git a/src/HHRobot.cpp b/src/HHRobot.cpp index b8373ff..e4cc433 100644 --- a/src/HHRobot.cpp +++ b/src/HHRobot.cpp @@ -5,7 +5,9 @@ HHRobot::HHRobot(): shooter(new HHShooter()), collector(new HHCollector()), compressorSystem(new HHCompressor()), - dashboard(new HHDashboard()){ + dashboard(new HHDashboard()), + autoseq(new HHAuto()), + sonar(new HHSonar()){ } bool HHRobot::CheckJoystickValues(){ float x=ControlSystem->rightJoystickAxisValues[1]; @@ -36,12 +38,24 @@ void HHRobot::DriveRobot(float x, float y){ void HHRobot::UpdateDashboard(){ dashboard->PutFloatValue("Shooting Power", ControlSystem->throttle); } +void HHRobot::RunAuto(){ + int step, time; + if(step == 0 && time < 200){ + compressorSystem->ExtendCollector(); + collector->CollectBallAtSpeed(50); + } + else{ + step = 1; + } + time++; +} //Main function used to handle periodic tasks on the robot void HHRobot::Handler(){ + int targetAngle; //Periodic tasks that should be run by every loop ControlSystem->UpdateJoysticks(); - shooter->UpdateShooterPosition(); + shooter->UpdateShooterPosition(targetAngle); //TODO Need to implement a timing system to not break the spike (this function doesn't run the compressor at the moment) compressorSystem->CompressorSystemPeriodic(); collector->UpdateCollector(shooter->isShooting, shooter->GetAngle()); @@ -65,5 +79,25 @@ void HHRobot::Handler(){ if(ControlSystem->rightJoystickValues[COLLECTOR_RETRACT]) { compressorSystem->RetractCollector(); } + if(ControlSystem->leftJoystickValues[SHOOTER_ANGLE_ONE]){ + targetAngle = 100; + } + if(ControlSystem->leftJoystickValues[SHOOTER_ANGLE_TWO]){ + targetAngle = 120; + } + if(ControlSystem->leftJoystickValues[SHOOTER_ANGLE_THREE]){ + targetAngle = 90; + } + if(ControlSystem->leftJoystickValues[SHOOTER_ANGLE_FOUR]){ + targetAngle = 130; + } + if(ControlSystem->rightJoystickValues[DRIVE_FOR_DISTANCE]){ + if(sonar->GetInches("FRONTLEFT") >= 40){ + DriveRobot(0,-.5); + } + else{ + DriveRobot(0,0); + } + } } // vim: ts=2:sw=2:et diff --git a/src/HHRobot.h b/src/HHRobot.h index d0943e5..ce5e971 100644 --- a/src/HHRobot.h +++ b/src/HHRobot.h @@ -2,6 +2,7 @@ #define __ZAPHOD_ROBOT_H__ #include #include "HHBase.h" +#include "Auto/AutoMain.h" #include "Definitions.h" class JoystickController; class HHShooter; @@ -9,6 +10,7 @@ class HHCollector; class HHCompressor; class HHDashboard; class HHRobot; +class HHSonar; class HHRobot{ private: Jaguar *right1, *right2, *right3, *left1, *left2, *left3; @@ -17,12 +19,15 @@ class HHRobot{ HHCollector *collector; HHCompressor *compressorSystem; HHDashboard *dashboard; + HHAuto *autoseq; + HHSonar *sonar; public: HHRobot(); bool CheckJoystickValues(); void DriveRobot(float,float); void UpdateDashboard(); void Handler(); + void RunAuto(); }; #endif // vim: ts=2:sw=2:et diff --git a/src/Subsystems/Collector.cpp b/src/Subsystems/Collector.cpp index f2b343d..5358b71 100644 --- a/src/Subsystems/Collector.cpp +++ b/src/Subsystems/Collector.cpp @@ -19,6 +19,9 @@ void HHCollector::UpdateCollector(bool shooting, float angle){ collectorMotor->Set(0); } } +void HHCollector::CollectBallAtSpeed(float speed){ + collectorMotor->Set(speed); +} void HHCollector::CollectBall(){ collectorMotor->Set(1); } diff --git a/src/Subsystems/Collector.h b/src/Subsystems/Collector.h index d12116a..b9e7826 100644 --- a/src/Subsystems/Collector.h +++ b/src/Subsystems/Collector.h @@ -11,6 +11,7 @@ class HHCollector{ STOP }e_CollectorState; void UpdateCollector(bool, float); + void CollectBallAtSpeed(float speed); void CollectBall(); void ReleaseBall(); void SpinWithShot(float); diff --git a/src/Subsystems/Shooter.cpp b/src/Subsystems/Shooter.cpp index 1d674d0..aeac0c0 100644 --- a/src/Subsystems/Shooter.cpp +++ b/src/Subsystems/Shooter.cpp @@ -47,14 +47,14 @@ void HHShooter::ShootRaw(float power){ shooterRight2->SetRaw(int(FloatToPWM(-power))); } //Should be run in a loop -void HHShooter::UpdateShooterPosition(){ +void HHShooter::UpdateShooterPosition(int angle){ if(e_ShooterState == IDLE_PRESHOT){ isShooting=false; StopShooter(); } if(e_ShooterState == FIRING){ isShooting=true; - ShootForAngle(shootingPower,110); + ShootForAngle(shootingPower,angle); } if(e_ShooterState == IDLE_POSTSHOT){ isShooting=false; diff --git a/src/Subsystems/Shooter.h b/src/Subsystems/Shooter.h index 4e59c2e..1f95550 100644 --- a/src/Subsystems/Shooter.h +++ b/src/Subsystems/Shooter.h @@ -19,7 +19,7 @@ class HHShooter{ void ShootRaw(float); void Lower(float); void StopShooter(); - void UpdateShooterPosition(); + void UpdateShooterPosition(int); float FloatToPWM(float input); float GetAngle(); };