From 5a89a6d7122424c5fe9f061c66064cec70288f23 Mon Sep 17 00:00:00 2001 From: Adam Long Date: Sun, 23 Nov 2014 14:45:15 +0000 Subject: [PATCH] Fixed driving mathmatically, some talons dont get power still --- src/Definitions.h | 4 +-- src/HHRobot.cpp | 49 +++++++++++++++++++---------------- src/HHRobot.h | 2 +- src/Subsystems/Controller.cpp | 20 ++++++++++++-- src/Subsystems/Controller.h | 3 ++- 5 files changed, 50 insertions(+), 28 deletions(-) diff --git a/src/Definitions.h b/src/Definitions.h index 4fff652..06bdc0c 100644 --- a/src/Definitions.h +++ b/src/Definitions.h @@ -43,7 +43,7 @@ #define SHOOTER_ANGLE_POT 6 //Ultrasonic (DIO, Analog, etc) - + #define SONAR_FRONT_RIGHT_DIO 4 #define SONAR_FRONT_LEFT_DIO 4 #define SONAR_BACK_LEFT_DIO 5 @@ -71,7 +71,7 @@ #define SHOOTER_ANGLE_TWO 4 #define SHOOTER_ANGLE_THREE 5 #define SHOOTER_ANGLE_FOUR 6 -//#define SHOOTER_POWER_ONE +//#define SHOOTER_POWER_ONE //#define SHOOTER_POWER_TWO //#define SHOOTER_POWER_THREE //#define SHOOTER_POWER_FOUR diff --git a/src/HHRobot.cpp b/src/HHRobot.cpp index 833c45f..6ce5c0e 100644 --- a/src/HHRobot.cpp +++ b/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((-.1PutBoolValue("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)){ diff --git a/src/HHRobot.h b/src/HHRobot.h index 86aa9d8..5b6eada 100644 --- a/src/HHRobot.h +++ b/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; diff --git a/src/Subsystems/Controller.cpp b/src/Subsystems/Controller.cpp index 2166dcb..9878232 100644 --- a/src/Subsystems/Controller.cpp +++ b/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 diff --git a/src/Subsystems/Controller.h b/src/Subsystems/Controller.h index 469ffb7..0434df7 100644 --- a/src/Subsystems/Controller.h +++ b/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