/FRC-Team_207

Permalink
Find file
FRC-Team_207/2016/Rotate to Angle/src/Robot.cpp
Fetching contributors…
Cannot retrieve contributors at this time
Raw Blame History
409 lines (338 sloc) 11.9 KB
#include "WPILib.h"
#include "AHRS.h"
/*------------------------Change Log (For Champs)----------------------------------
* Implenting the Navx in Autonomous in order to align with the
* the goal.
*
* 4/18/16 Successfully made a loop where the Navx was being read by
* the specific function that i wanted to do with the
* Double yaw = ahrs.GetYaw(); line. Now working on aligning in auto.
*/
static const double kOffBalanceThresholdDegrees = 32.2;
static const double kOnBalanceThresholdDegrees = 20.0;
static const double kToleranceDegrees = 2.0f;
static const double kP = 0.07f;
static const double kI = 0.00f;
static const double kD = 0.00f;
class Robot: public PIDOutput, public SampleRobot
{
//----------- Drive Train -------------
RobotDrive myRobot;
//----------- Joysticks ---------------
Joystick driver; // Driver
Joystick codriver; // CoDriver
//----------- Dashboard ---------------
SendableChooser *chooser; // Options for autonomous
//----------- Pneumatics --------------
//----------- Switches ---------------
DigitalInput BallUp; // Ball is at top of feeder
DigitalInput BallDown; // Ball is at bottom of feeder
DigitalInput WinchSafety; // Safety for the winch
// ---------- Flag variables ---------
bool loaded = FALSE;
bool collected = FALSE;
bool fired = FALSE;
bool autoBalanceXMode = FALSE;
bool autoBalanceYMode = FALSE;
bool rotateToAngle = true;
// ---------- Numeric variables -------------
float reduction = 0;
float startpos = 0;
//-------- Motor Controllers ----------
VictorSP Shooter; // Victor for Shooter
VictorSP Collector; // Victor for Collector
VictorSP Feeder; // Victor for Feeder
CANTalon CollectorLift; // Victor for Collector Lift
VictorSP Winch; // Victor for Winch
Servo WinchRelease;
Relay Light;
AHRS *ahrs;
PIDController *turnController;
//------------ Variables -------------
const std::string autoNameDefault = "Default Auto";
const std::string autoNameCustom1 = "Forwards";
const std::string autoNameCustom2 = "Attack";
const std::string autoNameCustom3 = "Third Auto";
const std::string autoNameCustom4 = "Over Static and Shooting Low *not tested*";
double kUpdatePeriod = 2.0;
double rotateToAngleRate;
double currentRotationRate;
double header = 0;
public:
Robot() :
//----------- Drive Train -------------
myRobot(0, 1, 7, 8),
//----------- Joysticks ---------------
driver(0),
codriver(1),
//----------- Dashboard ---------------
chooser(),
//----------- Pneumatics --------------
//-------- Switches -------------
BallUp(1),
BallDown(0),
WinchSafety(2),
//-------- Motor Controllers ----------
Shooter(2),
Collector(5),
Feeder(3), // 3
CollectorLift(1), // id 1 Make sure the device id is changed in the web interface
Winch(4), //
WinchRelease(6),
//--------- Relays---------------------
Light(0),
ahrs(),
turnController()
{
myRobot.SetExpiration(0.1);
ahrs = new AHRS(SPI::Port::kMXP);
turnController = new PIDController(kP, kI, kD, ahrs, this);
turnController->SetInputRange(-180.0f, 180.0f);
turnController->SetOutputRange(-1.0, 1.0);
turnController->SetAbsoluteTolerance(kToleranceDegrees);
turnController->SetContinuous(true);
rotateToAngleRate = 0.0f;
currentRotationRate = rotateToAngleRate;
LiveWindow::GetInstance()->AddActuator("DriveSystem", "RotateController", turnController);
if ( ahrs )
{
LiveWindow::GetInstance()->AddSensor("IMU", "Gyro", ahrs);
}
}
void RobotInit()
{
chooser = new SendableChooser();
chooser->AddDefault(autoNameDefault,(void*)&autoNameDefault);
chooser->AddObject(autoNameCustom1, (void*)&autoNameCustom1);
chooser->AddObject(autoNameCustom2, (void*)&autoNameCustom2);
chooser->AddObject(autoNameCustom3, (void*)&autoNameCustom3);
chooser->AddObject(autoNameCustom4, (void*)&autoNameCustom4);
SmartDashboard::PutData("Auto Modes", chooser);
SmartDashboard::GetData("Accelerometer");
SmartDashboard::PutNumber( "IMU_Yaw", ahrs->GetYaw());
turnController->Enable();
ahrs->Reset();
}
void Autonomous()
{
std::string autoSelected = *((std::string*)chooser->GetSelected());
std::cout << "Auto selected: " << autoSelected << std::endl;
SmartDashboard::PutNumber( "IMU_Yaw", ahrs->GetYaw());
if(autoSelected == autoNameCustom1)
{
myRobot.SetSafetyEnabled(false);
ahrs->ZeroYaw();
myRobot.Drive(-0.5, 0.0); //positive values backwards; negative values forward
Wait(2.0);
myRobot.Drive(0.0, 0.0);
}
else if (autoSelected == autoNameCustom2)
{
myRobot.SetSafetyEnabled(false);
ahrs->ZeroYaw();
std::cout << "Running custom Autonomous" << std::endl;
myRobot.Drive(-0.95, 0.0); //negative values forwards
Wait(3.0);
myRobot.Drive(0.0, 0.0);
}
else if (autoSelected == autoNameCustom3)
{
myRobot.SetSafetyEnabled(false);
header = ahrs->GetYaw();
myRobot.Drive(-.5,0.0);
Wait(1.5);
while(header < 33)
{
myRobot.Drive(-0.4, -1.0);
header = ahrs->GetYaw();
}
while(header > 30)
{
myRobot.Drive(-.3, 1.0);
header = ahrs->GetYaw();
}
myRobot.Drive(0.0, 0.0);
Wait(0.5);
myRobot.Drive(-.5, 0.0);
Wait(.5);
myRobot.Drive(0.0, 0.0);
}
else if (autoSelected == autoNameCustom4)
{
myRobot.SetSafetyEnabled(false);
ahrs->ZeroYaw();
myRobot.Drive(-1.0, 0.0);
Wait(5.0);
myRobot.Drive(0.0, 0.0);
Wait(2.0);
Collector.Set(-1.0);
Feeder.Set(-1.0);
Shooter.Set(1.0);
Wait(2.0);
Collector.Set(0.0);
Feeder.Set(0.0);
Shooter.Set(0.0);
}
}
void OperatorControl()
{
myRobot.SetSafetyEnabled(false);
while (IsOperatorControl() && IsEnabled())
{
double pitchAngleDegrees = ahrs->GetPitch();
bool rotateToAngle = false;
double currentRotationRate;
myRobot.ArcadeDrive(driver); // drive with arcade style (use right driver)
Wait(0.005);
//---------- Collector Control (CODRIVER)-----------
if (codriver.GetY() < 0.4 && codriver.GetY() > -0.4)
{
CollectorLift.Set(0.0);
}
// joystick forward collector down
else if (codriver.GetY() > 0.4 && codriver.GetY() < 0.9) //joystick pulled back (+ values)
{
CollectorLift.Set(0.4);
}
else if (codriver.GetY() > 0.9)
{
CollectorLift.Set(0.7);
}
else if (codriver.GetY() < -0.6)
{
CollectorLift.Set(-0.15); // -0.05; -0.1
}
else
{
}
//---------- Winch Motor Control (DRIVER)--------------
if (driver.GetRawButton(6) == 1 && driver.GetRawButton(7) && WinchSafety.Get() == 1)
{
Winch.Set(1.0);
}
else
{
Winch.Set(0.0);
}
//----------- Hook Deployment Control (DRIVER) -----------------
// buttons 8 & 9 - for safety pressed at same time
if (driver.GetRawButton(8) == 1 && driver.GetRawButton(9) == 1)
{
WinchRelease.Set(0); // open position
}
else
{
WinchRelease.Set(60); // locked position
}
//---------- END of Hook Deployment Control ------------------------------------------------
// ---------- Shoots The Ball (DRIVER) -----------
// button 1 - holding allows wheel to spool up
if (driver.GetRawButton(1) == 1 && driver.GetRawButton(2) == 0)
{
Shooter.Set(1.0);
}
// buttons 1 & 2 - pressed at same time fires ball
else if (driver.GetRawButton(1) == 1 && driver.GetRawButton(2) == 1)
{
Shooter.Set(1.0);
Feeder.Set(-1.0);
collected = FALSE;
fired = TRUE;
}
else
{
Shooter.Set(0.0);
Feeder.Set(0.0);
fired = FALSE;
loaded = FALSE;
}
// ---------- End of Shooting The Ball -------------------------------------------------------------
// ---------- Picking Ball Up (CODRIVER)--------------
// button 2 - collector/feeder STOPS when collected into robot
if(codriver.GetRawButton(2) == 1 && codriver.GetRawButton(3) == 0 && BallUp.Get() == 0 &&
collected == FALSE && fired == FALSE)
{
Feeder.Set(0.0);
Collector.Set(0.0);
collected = TRUE;
}
// button 2 - collect ball from field
else if (codriver.GetRawButton(2) == 1 && codriver.GetRawButton(3) == 0 && collected == FALSE)
{
Collector.Set(-1.0);
Feeder.Set(-0.5);
}
// button 3 - SLOW reverse feed the ball
else if (codriver.GetRawButton(2) == 0 && codriver.GetRawButton(3) == 1 &&
codriver.GetRawButton(7) == 0)
{
Collector.Set(0.4);
Feeder.Set(0.4);
}
// button 3 & 7 - FAST reverse feed the ball
else if (codriver.GetRawButton(2) == 0 && codriver.GetRawButton(3) == 1 &&
codriver.GetRawButton(7) == 1)
{
Collector.Set(1.0);
Feeder.Set(1.0);
}
else
{
Collector.Set(0.0);
}
// ---------- END of Picking up Ball ---------------------------------------------------
//--------------------- NavX Functions -------------------------------------------------
SmartDashboard::PutNumber( "IMU_Yaw", ahrs->GetYaw());
SmartDashboard::PutNumber( "IMU_Pitch", ahrs->GetPitch());
SmartDashboard::PutNumber( "IMU_Roll", ahrs->GetRoll());
if ( !autoBalanceXMode && (fabs(pitchAngleDegrees) >= fabs(kOffBalanceThresholdDegrees)))
{
autoBalanceXMode = true;
}
else if ( autoBalanceXMode && (fabs(pitchAngleDegrees) <= fabs(kOnBalanceThresholdDegrees)))
{
autoBalanceXMode = false;
}
if ( !autoBalanceYMode && (fabs(pitchAngleDegrees) >= fabs(kOffBalanceThresholdDegrees)))
{
autoBalanceYMode = true;
}
else if ( autoBalanceYMode && (fabs(pitchAngleDegrees) <= fabs(kOnBalanceThresholdDegrees)))
{
autoBalanceYMode = false;
}
if ( codriver.GetRawButton(2))
{
turnController->SetSetpoint(0.0f);
rotateToAngle = true;
}
else if ( codriver.GetRawButton(3))
{
turnController->SetSetpoint(90.0f);
rotateToAngle = true;
}
else if ( codriver.GetRawButton(4))
{
turnController->SetSetpoint(179.9f);
rotateToAngle = true;
}
else if ( codriver.GetRawButton(5))
{
turnController->SetSetpoint(-90.0f);
rotateToAngle = true;
}
if ( rotateToAngle ) {
turnController->Enable();
currentRotationRate = rotateToAngleRate;
} else {
turnController->Disable();
currentRotationRate = codriver.GetTwist();
}
myRobot.ArcadeDrive(driver.GetX(), currentRotationRate, ahrs->GetAngle());
}
}
void PIDWrite(float output) {
rotateToAngleRate = output;
}
};
START_ROBOT_CLASS(Robot)