From ba21daae67a426a89b45edd93305ef6677345c5f Mon Sep 17 00:00:00 2001
From: Austen Adler <stonewareslord@gmail.com>
Date: Sun, 11 Jan 2015 02:24:24 +0000
Subject: [PATCH] Use throttle for power of collector (untested)

---
 src/HHRobot.cpp | 38 ++++++++++++++++++++------------------
 src/HHRobot.h   |  2 +-
 src/hhlib       |  2 +-
 3 files changed, 22 insertions(+), 20 deletions(-)

diff --git a/src/HHRobot.cpp b/src/HHRobot.cpp
index d05f564..1020bc1 100644
--- a/src/HHRobot.cpp
+++ b/src/HHRobot.cpp
@@ -4,7 +4,7 @@ HHRobot::HHRobot():
   hhdrive(new RobotDrive(2,0,3,1)),
   gyro(new Gyro(1)),
   collector(new DentCollector(4, 5, 6, 7)),
-  joystick1(new Extreme3dPro(0)){
+  driveStick(new Extreme3dPro(0)){
     hhdrive->SetExpiration(0.1);
     hhdrive->SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
     hhdrive->SetInvertedMotor(RobotDrive::kRearLeftMotor,true);
@@ -13,32 +13,34 @@ void HHRobot::Init(){
   printf("Initing\n");
   printf("Code Version: %f\n",0000.1);
   gyro->Reset();
-  //Put table values initally to avoid annoying refreshing
 }
 //Main function used to handle periodic tasks on the robot
 void HHRobot::Handler(){
   const float Kp = 0.3;
-  if(joystick1->GetJoystickButton(1)==1){
-    hhdrive->MecanumDrive_Cartesian(joystick1->GetJoystickAxis("z"), 0, joystick1->GetJoystickAxis("x"));
-  }else if(joystick1->GetJoystickButton(2)==1){
-    hhdrive->MecanumDrive_Cartesian(joystick1->GetJoystickAxis("z"), joystick1->GetJoystickAxis("y"), 0);
-  }else if(joystick1->GetJoystickButton(3)==1){
-    hhdrive->Drive(joystick1->GetJoystickAxis("y"), joystick1->GetJoystickAxis("y")*Kp*-gyro->GetAngle());
+  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(joystick1->GetJoystickAxis("z"), joystick1->GetJoystickAxis("y"), joystick1->GetJoystickAxis("x"));
+    hhdrive->MecanumDrive_Cartesian(driveStick->GetJoystickAxis("z"), driveStick->GetJoystickAxis("y"), driveStick->GetJoystickAxis("x"));
   }
-  if(joystick1->GetJoystickButton(11)){
-    collector->Collect(-1.0f);
-  }else if(joystick1->GetJoystickButton(12)){
-    collector->Collect(1.0f);
-  }else if(joystick1->GetJoystickButton(9)){
-    collector->Raise(255);
-  }else if(joystick1->GetJoystickButton(10)){
-    collector->Raise(1);
+  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);
-  // hhdrive->MecanumDrive_Cartesian(joystick1->GetJoystickAxis("z"), joystick1->GetJoystickAxis("y"), joystick1->GetJoystickAxis("x"));
 }
 // vim: ts=2:sw=2:et
diff --git a/src/HHRobot.h b/src/HHRobot.h
index a930468..f6dc004 100644
--- a/src/HHRobot.h
+++ b/src/HHRobot.h
@@ -9,7 +9,7 @@ class HHRobot{
     RobotDrive *hhdrive;
     Gyro *gyro;
     DentCollector *collector;
-    Extreme3dPro *joystick1;
+    Extreme3dPro *driveStick;
   public:
     HHRobot();
     void Init();
diff --git a/src/hhlib b/src/hhlib
index 17606cc..38a965d 160000
--- a/src/hhlib
+++ b/src/hhlib
@@ -1 +1 @@
-Subproject commit 17606cc482c4cdde363c96fda0b6efd92ccdbad9
+Subproject commit 38a965dd1f7c6737f722f56c0f7940805ece9917