public SmartDrive()
{
turnPID = new SWave_PID(Constants.Drive_TurnP, 0, Constants.Drive_TurnD, -0.5, 0.5);
frontBackPID = new SWave_PID(Constants.Drive_AlignBackP, 0, Constants.Drive_AlignBackD, -0.25, 0.25);
sidePID = new SWave_PID(Constants.Drive_AlignSideP, 0, Constants.Drive_AlignSideD, -0.5, 0.5);
drive = new Drive();
frontBack = new SWave_AnalogueUltrasonic(Constants.ChannelAnalogue_BackUltrasonic, Constants.UltraScaling);
side = new SWave_AnalogueUltrasonic(Constants.ChannelAnalogue_SideUltrasonic, Constants.UltraScaling);
gyro = new Gyro(Constants.ChannelAnalogue_Gyro);
rotateTrigger = new SWave_EdgeTrigger(true, true);
fieldCentricToggle = new SWave_Toggle();
DriveSpeeds = new point(0, 0);
TurnSetpoint = 0; Rotation = 0;
FieldCentric = true; StrafeBackButton = false; StrafeLeftButton = false; StrafeForwardButton = false; StrafeRightButton = false;
AlignNoodle = false; AlignLoad = false; AlignOutput = false;
}
public void update()
{
if (rotateTrigger.Get(Rotation == 0))
{
TurnSetpoint = gyro.GetAngle();
WPILib.SmartDashboards.SmartDashboard.PutNumber("Heading set at", gyro.GetAngle());
WPILib.SmartDashboards.SmartDashboard.PutString("ResetRotate", "Reset");
}
rotateTrigger.Update(Rotation == 0);
if (AlignNoodle)
{
TurnSetpoint = Constants.Drive_AlignLoadAngle;
frontBackPID.setpoint = Constants.Drive_AlignBackSetNoodle;
DriveSpeeds = new point(0, frontBackPID.get(frontBack.get()));
}
else if (AlignOutput)
{
TurnSetpoint = 0;
frontBackPID.setpoint = Constants.Drive_AlignBackSetOutput;
DriveSpeeds = new point(0, frontBackPID.get(frontBack.get()));
}
else if (AlignLoad)
{
TurnSetpoint = Constants.Drive_AlignLoadAngle;
frontBackPID.setpoint = Constants.Drive_AlignBackSetLoad;
sidePID.setpoint = Constants.Drive_AlignSideSetpoint;
DriveSpeeds = new point(sidePID.get(side.get()), frontBackPID.get(frontBack.get()));
}
if (Rotation == 0 || AlignOutput || AlignNoodle || AlignLoad)
Rotation = turnPID.get(gyro.GetAngle());
if (StrafeRightButton)
DriveSpeeds = new point(Constants.Drive_StrafeButtonSpeed, 0);
else if (StrafeLeftButton)
DriveSpeeds = new point(-Constants.Drive_StrafeButtonSpeed, 0);
else if (StrafeForwardButton)
DriveSpeeds = new point(0, Constants.Drive_ForwardButtonSpeed);
else if (StrafeBackButton)
DriveSpeeds = new point(0, -Constants.Drive_ForwardButtonSpeed);
drive.Rotation = Rotation;
drive.X = FieldCentric ? fieldCentricAdj().x : DriveSpeeds.x;
drive.Y = FieldCentric ? fieldCentricAdj().y : DriveSpeeds.y;
drive.Update(null);
turnPID.Update(gyro.GetAngle());
sidePID.Update(side.get());
frontBackPID.Update(frontBack.get());
}
