Added shooter angle limits, sonar auto driving, and a base for autonomous

src/Auto/AutoMain.cpp

@@ -0,0 +1,5 @@
+#include "AutoMain.h"
+HHAuto::HHAuto(){
+	
+}
+

src/Auto/AutoMain.h

@@ -0,0 +1,6 @@
+#include <WPILib.h>
+#include "../Definitions.h"
+class HHAuto{	
+	public:
+		HHAuto();
+};

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
 

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();
 }

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

src/HHRobot.h

@@ -2,6 +2,7 @@
 #define __ZAPHOD_ROBOT_H__
 #include <WPILib.h>
 #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

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);
 }

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);

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;

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();
 };