Fixed driving mathmatically, some talons dont get power still

src/HHRobot.cpp

@@ -8,7 +8,7 @@ HHRobot::HHRobot():
   compressorSystem(new HHCompressor()),
   dashboard(new HHDashboard()){
   //sonar(new HHSonar()){
-    netTable=NetworkTable::GetTable("datatable");
+    driveTable=NetworkTable::GetTable("ZaphodDrive");
     right1 = new Talon(DRIVE_RIGHT_MOTOR_ONE, DRIVE_RIGHT_SIDECAR);
     right2 = new Talon(DRIVE_RIGHT_MOTOR_TWO, DRIVE_RIGHT_SIDECAR);
     right3 = new Talon(DRIVE_RIGHT_MOTOR_THREE, DRIVE_RIGHT_SIDECAR);
@@ -22,8 +22,8 @@ void HHRobot::Init(){
   collector->CollectBallAtSpeed(0);
 }
 bool HHRobot::CheckJoystickValues(){
-  float x=controlSystem->GetJoystickAxis(1,1);
-  float y=controlSystem->GetJoystickAxis(1,2);
+  float x=controlSystem->GetJoystickAxis(1,"x");
+  float y=controlSystem->GetJoystickAxis(1,"y");
   if((-.1<x && x<.1) && (-.1<y && y<.1)) {
     dashboard->PutBoolValue("Joysticks Valid",true);
     return true;
@@ -32,31 +32,38 @@ bool HHRobot::CheckJoystickValues(){
     return false;
   }
 }
-void HHRobot::DriveRobot(float x,float y){
+void HHRobot::DriveRobot(float y,float x){
   if(y>1.0f){
     y=1.0f;
   }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("Left: %f\n", leftPower);
-  //printf("Right: %f\n", rightPower);
-  //right1->SetRaw(int(rightPower));
+  float leftPower=((y+x)/2+1)*127+1;
+  float rightPower=((y-x)/2+1)*127+1;
+  driveTable->PutNumber("joystickRawX",x);
+  driveTable->PutNumber("joystickRawY",y);
+  driveTable->PutNumber("leftSidePower",leftPower);
+  driveTable->PutNumber("rightSidePower",rightPower);
+  driveTable->PutNumber("right1MotorPower",right1->GetRaw());
+  driveTable->PutNumber("right2MotorPower",right2->GetRaw());
+  driveTable->PutNumber("right3MotorPower",right3->GetRaw());
+  driveTable->PutNumber("left1MotorPower",left1->GetRaw());
+  driveTable->PutNumber("left2MotorPower",left2->GetRaw());
+  driveTable->PutNumber("left3MotorPower",left3->GetRaw());
+  right1->SetRaw(int(rightPower));
   //right2->SetRaw(int(rightPower));
   //right3->SetRaw(int(rightPower));
   //left1->SetRaw(int(leftPower));
-  //left2->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);
-  right2->Set(y+x);
-  right3->Set(y+x);
-  left1->Set(y-x);
-  left2->Set(y-x);
-  left3->Set(y-x);
+  //printf("Left: %f\n",y+x);
+  //printf("Right: %f\n",y-x);
+  //right1->Set(y+x);
+  //right2->Set(y+x);
+  //right3->Set(y+x);
+  //left1->Set(y-x);
+  //left2->Set(y-x);
+  //left3->Set(y-x);
 }
 void HHRobot::UpdateDashboard(){
   dashboard->PutFloatValue("Shooting Power",controlSystem->GetThrottle());
@@ -85,8 +92,6 @@ void HHRobot::RunAuto(){
     time=0;
     step=1;
   }
-  //Important periodic things
-  netTable->PutNumber("AutoStep",step); //Debugging purposes
   time++;
 }
 
@@ -100,7 +105,7 @@ void HHRobot::Handler(){
   compressorSystem->CompressorSystemPeriodic(allowCompressing);
   collector->UpdateCollector(shooter->isShooting,shooter->GetAngle());
   //TODO Fix whatever the heck is wrong with this
-  //DriveRobot(controlSystem->GetJoystickAxis(1,1),controlSystem->GetJoystickAxis(1,2));
+  DriveRobot(controlSystem->GetJoystickAxis(1,"x"),controlSystem->GetJoystickAxis(1,"y"));
   UpdateDashboard();
   //Shooting button
   if(controlSystem->GetJoystickButton(1,SHOOTER_FIRE)){

src/HHRobot.h

@@ -15,7 +15,7 @@ class HHRobot{
     Talon *right1, *right2, *right3, *left1, *left2, *left3;
     Joystick *rightStick, *leftStick;
     JoystickController *controlSystem;
-    NetworkTable *netTable;
+    NetworkTable *driveTable;
     HHShooter *shooter;
     HHCollector *collector;
     HHCompressor *compressorSystem;

src/Subsystems/Controller.cpp

@@ -4,7 +4,7 @@ JoystickController::JoystickController(){
   shootJoystick=new Joystick(JOYSTICK_LEFT);
 }
 float JoystickController::GetThrottle(){
-  return (-GetJoystickAxis(2,4)+1)/2;
+  return (-GetRawJoystickAxis(2,4)+1)/2;
 }
 int JoystickController::GetJoystickButton(int joystick, int button){
   switch (joystick){
@@ -20,7 +20,7 @@ int JoystickController::GetJoystickButton(int joystick, int button){
       break;
   }
 }
-float JoystickController::GetJoystickAxis(int joystick, int axis){
+float JoystickController::GetRawJoystickAxis(int joystick, int axis){
   switch (joystick){
     case 1:
       return driveJoystick->GetRawAxis(axis);
@@ -34,4 +34,20 @@ float JoystickController::GetJoystickAxis(int joystick, int axis){
       break;
   }
 }
+
+float JoystickController::GetJoystickAxis(int joystick, std::string axis){
+  if (joystick == 1) {
+    if (axis == "x"){
+      return driveJoystick->GetX();
+    }else if (axis == "y"){
+      return driveJoystick->GetY();
+    }else if (axis == "z"){
+      return driveJoystick->GetZ();
+    }else{
+      return 0;
+    }
+  }else{
+    return 0;
+  }
+}
 // vim: ts=2:sw=2:et

src/Subsystems/Controller.h

@@ -8,6 +8,7 @@ class JoystickController
     JoystickController();
     float GetThrottle();
     int GetJoystickButton(int,int);
-    float GetJoystickAxis(int,int);
+    float GetRawJoystickAxis(int,int);
+    float GetJoystickAxis(int, std::string axis);
 };
 // vim: ts=2:sw=2:et