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;
}
/// <summary>
/// Instantiates the Motor COntrollers, Encoders, Gyroscope.
/// </summary>
public Drive()
{
#region Instantiating
L1 = new RampMotor<Talon>(Constants.Drive_L1Channel);
L2 = new RampMotor<Talon>(Constants.Drive_L2Channel);
L3 = new RampMotor<Talon>(Constants.Drive_L3Channel);
R1 = new RampMotor<Talon>(Constants.Drive_R1Channel);
R2 = new RampMotor<Talon>(Constants.Drive_R2Channel);
R3 = new RampMotor<Talon>(Constants.Drive_R3Channel);
LShift = new Solenoid(Constants.Drive_LShiftChannel);
RShift = new Solenoid(Constants.Drive_RShiftChannel);
LEncode = new Encoder(Constants.Drive_LEncoderAChannel, Constants.Drive_LEncoderBChannel);
REncode = new Encoder(Constants.Drive_REncoderAChannel, Constants.Drive_REncoderBChannel);
LSpeedFilter = new InputFilter(0);
RSpeedFilter = new InputFilter(0);
gyro = new Gyro(Constants.Drive_GyroChannel);
#endregion
//Invert the Right Side Motors and Encoder
R1.Inverted = true;
R2.Inverted = true;
R3.Inverted = true;
REncode.ReverseDirection = true;
L1.MaxChange = Constants.Drive_MaxPowerChange;
L2.MaxChange = Constants.Drive_MaxPowerChange;
L3.MaxChange = Constants.Drive_MaxPowerChange;
R1.MaxChange = Constants.Drive_MaxPowerChange;
R2.MaxChange = Constants.Drive_MaxPowerChange;
R3.MaxChange = Constants.Drive_MaxPowerChange;
gyro.Reset();
LShift.Set(false);
RShift.Set(false);
//Secondary Motors are set to the same max acceleration as the primary motors by default, until we get these numbers tuned.
shiftingMotorValue = Constants.Drive_MaxPowerChange;
}
private static void AddGyro(string key, Gyro gyro)
{
GyroKeys.Add(key);
Gyros.Add(key, gyro);
}
private static void InitializeDriveTrain()
{
FrontRight = new CANJaguar(22);
FrontLeft = new CANJaguar(21);
RearRight = new CANJaguar(23);
RearLeft = new CANJaguar(20);
DriveTrain = new List<CANJaguar>
{
FrontRight,
FrontLeft,
RearRight,
RearLeft
};
PivotGyro = new Gyro(0);
PitchGyro = new Gyro(1);
PivotGyro.Sensitivity = 0.007;
PitchGyro.Sensitivity = 0.007;
Gyros = new List<Gyro>
{
PivotGyro,
PitchGyro
};
}