From c31dfcf26df14f8efdce8bff778d26dbc009062a Mon Sep 17 00:00:00 2001
From: Adam Long <macadam.long@gmail.com>
Date: Thu, 20 Nov 2014 23:55:29 +0000
Subject: [PATCH] Made the controller more versitile (and actually work)

---
 src/HHRobot.cpp               | 17 +++++++------
 src/Subsystems/Controller.cpp | 46 ++++++++++++++++++-----------------
 src/Subsystems/Controller.h   |  8 +++---
 3 files changed, 36 insertions(+), 35 deletions(-)

diff --git a/src/HHRobot.cpp b/src/HHRobot.cpp
index c0c8ace..779019f 100644
--- a/src/HHRobot.cpp
+++ b/src/HHRobot.cpp
@@ -15,8 +15,8 @@ HHRobot::HHRobot():
     left1 = new Talon(DRIVE_LEFT_MOTOR_ONE, DRIVE_LEFT_SIDECAR);
     left2 = new Talon(DRIVE_LEFT_MOTOR_TWO, DRIVE_LEFT_SIDECAR);
     left3 = new Talon(DRIVE_LEFT_MOTOR_THREE, DRIVE_LEFT_SIDECAR);
-    rightStick = new Joystick(JOYSTICK_RIGHT);
-    leftStick = new Joystick(JOYSTICK_LEFT);
+    rightStick = new Joystick(3);
+    leftStick = new Joystick(4);
   }
 
 void HHRobot::Init(){
@@ -40,9 +40,8 @@ void HHRobot::DriveRobot(float x,float y){
   }else if(y!=0.0f&&y<-1.0f){
     y=-1.0f;
   }
-  float leftPower=((y+x)/2+1)*127+1;
-  float rightPower=((y-x)/2+1)*127+1;
-  printf("Driving and stuff\n");
+  //float leftPower=((y+x)/2+1)*127+1;
+  //float rightPower=((y-x)/2+1)*127+1;
   //printf("Left: %f\n", leftPower);
   //printf("Right: %f\n", rightPower);
   //right1->SetRaw(int(rightPower));
@@ -51,6 +50,7 @@ void HHRobot::DriveRobot(float x,float y){
   //left1->SetRaw(int(leftPower));
   //left2->SetRaw(int(leftPower));
   //left3->SetRaw(int(leftPower));
+  printf("Driving and stuff\n");
   printf("Left: %f\n",y+x);
   printf("Right: %f\n",y-x);
   right1->Set(y+x);
@@ -102,16 +102,17 @@ void HHRobot::Handler(){
   //TODO Need to implement a timing system to not break the spike (this function doesn't run the compressor at the moment)
   compressorSystem->CompressorSystemPeriodic(allowCompressing);
   collector->UpdateCollector(shooter->isShooting,shooter->GetAngle());
-  DriveRobot(rightStick->GetRawAxis(1),rightStick->GetRawAxis(2));
+  //TODO Fix whatever the heck is wrong with this
+  //DriveRobot(rightStick->GetRawAxis(1),rightStick->GetRawAxis(2));
   UpdateDashboard();
   //Shooting button
   if(ControlSystem->leftJoystickValues[SHOOTER_FIRE]) {
     shooter->StartShootingSequence(ControlSystem->throttle);
   }
   //Collector button assignments
-  if(rightStick->GetRawButton(COLLECTOR_INTAKE)) {
+  if(ControlSystem->GetJoystickButton(2,COLLECTOR_INTAKE)) {
     collector->CollectBall();
-  }else if(rightStick->GetRawButton(COLLECTOR_OUTTAKE)) {
+  }else if(ControlSystem->GetJoystickButton(2,COLLECTOR_OUTTAKE)) {
     collector->ReleaseBall();
   }else{
     collector->CollectorStop();
diff --git a/src/Subsystems/Controller.cpp b/src/Subsystems/Controller.cpp
index 67b5f0a..5f12741 100644
--- a/src/Subsystems/Controller.cpp
+++ b/src/Subsystems/Controller.cpp
@@ -1,33 +1,35 @@
 #include "Controller.h"
 JoystickController::JoystickController(){
-  rightJoystick=new Joystick(3);
-  leftJoystick=new Joystick(4);
+  driveJoystick=new Joystick(JOYSTICK_RIGHT);
+  shootJoystick=new Joystick(JOYSTICK_LEFT);
 }
 void JoystickController::UpdateJoysticks(){
-  GetRightJoystick();
-  GetLeftJoystick();
-  GetRightJoystickAxis();
-  GetLeftJoystickAxis();
   throttle=(-leftJoystickAxisValues[4]+1)/2;
 }
-void JoystickController::GetRightJoystick(){
-  for(int i=1;i<13;i++){
-    rightJoystickValues[i]=rightJoystick->GetRawButton(i);
+int JoystickController::GetJoystickButton(int joystick, int button){
+  switch (joystick){
+    case 1:
+      return driveJoystick->GetRawButton(button);
+      break;
+    case 2:
+      return shootJoystick->GetRawButton(button);
+      break;
+    default:
+      return -1;
+      break;
   }
 }
-void JoystickController::GetLeftJoystick(){
-  for(int i=1;i<13;i++){
-    leftJoystickValues[i]=leftJoystick->GetRawButton(i);
-  }
-}
-void JoystickController::GetRightJoystickAxis(){
-  for(int i=1;i<7;i++){
-    rightJoystickAxisValues[i]=rightJoystick->GetRawAxis(i);
-  }
-}
-void JoystickController::GetLeftJoystickAxis(){
-  for(int i=1;i<7;i++){
-    leftJoystickAxisValues[i]=leftJoystick->GetRawAxis(i);
+float JoystickController::GetJoystickAxis(int joystick, int axis){
+  switch (joystick){
+    case 1:
+      return driveJoystick->GetRawAxis(axis);
+      break;
+    case 2:
+      return shootJoystick->GetRawAxis(axis);
+      break;
+    default:
+      return 999;
+      break;
   }
 }
 // vim: ts=2:sw=2:et
diff --git a/src/Subsystems/Controller.h b/src/Subsystems/Controller.h
index 4ed2298..d140d6a 100644
--- a/src/Subsystems/Controller.h
+++ b/src/Subsystems/Controller.h
@@ -3,7 +3,7 @@
 class JoystickController
 {
   private:
-    Joystick *rightJoystick, *leftJoystick;
+    Joystick *driveJoystick, *shootJoystick;
   public:
     int leftJoystickValues[];
     int rightJoystickValues[];
@@ -12,9 +12,7 @@ class JoystickController
     float throttle;
     JoystickController();
     void UpdateJoysticks();
-    void GetRightJoystick();
-    void GetLeftJoystick();
-    void GetLeftJoystickAxis();
-    void GetRightJoystickAxis();
+    int GetJoystickButton(int,int);
+    float GetJoystickAxis(int,int);
 };
 // vim: ts=2:sw=2:et