public Drive(CANTalon _rightPrimary, CANTalon _leftPrimary, CANTalon _rightSecondary, CANTalon _leftSecondary)
{
RightPrimary = _rightPrimary;
LeftPrimary = _leftPrimary;
LeftSecondary = _leftSecondary;
RightSecondary = _rightSecondary;
}
public void TestTalonIdOverLimit()
{
Assert.Throws<ArgumentOutOfRangeException>(() =>
{
var s = new CANTalon(CANTalon.MaxTalonId);
});
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="channel">channel/address of the talon</param>
/// <param name="commonName">CommonName the component will have</param>
/// <param name="isReversed">if the controller output should be reversed</param>
public CanTalonItem(int channel, string commonName, bool isReversed = false)
{
talon = new CANTalon(channel);
Name = commonName;
IsReversed = isReversed;
talon.MotorControlMode = ControlMode.PercentVbus;
talon.ControlEnabled = true;
}
/// <summary>
/// Slave Constructor
/// </summary>
/// <param name="channel">channel/address of the talon</param>
/// <param name="commonName">CommonName the component will have</param>
/// <param name="master">master talon (this is the slave)</param>
/// <param name="isReversed">if the controller output should be reversed</param>
public CanTalonItem(int channel, string commonName, CanTalonItem master, bool isReversed = false)
{
talon = new CANTalon(channel);
Name = commonName;
IsReversed = isReversed;
talon.ReverseOutput(isReversed);
Slave = true;
talon.MotorControlMode = ControlMode.Follower;
master.AddSlave(this);
this.master = master.Name;
}
/// <summary>
/// PID Constructor
/// </summary>
/// <param name="channel">channel/address of the talon</param>
/// <param name="commonName">CommonName the component will have</param>
/// <param name="p">proportion</param>
/// <param name="i">integral</param>
/// <param name="d">derivative</param>
/// <param name="isReversed">if the controller output should be reversed</param>
public CanTalonItem(int channel, string commonName, double p, double i, double d, bool isReversed = false)
{
talon = new CANTalon(channel);
Name = commonName;
IsReversed = isReversed;
Master = true;
slaves = new List <CanTalonItem>();
talon.MotorControlMode = ControlMode.PercentVbus;
talon.FeedBackDevice = CANTalon.FeedbackDevice.QuadEncoder;
talon.SetPID(p, i, d);
talon.ControlEnabled = true;
}
private void Init()
{
LeftDrive = new CANTalon((int)TalonIDs.LeftDrive);
LeftSlave = new CANTalon((int)TalonIDs.LeftSlave)
{
MotorControlMode = WPILib.Interfaces.ControlMode.Follower
};
LeftSlave.Set(LeftDrive.DeviceId);
RightDrive = new CANTalon((int)TalonIDs.RightDrive);
RightSlave = new CANTalon((int)TalonIDs.RightSlave)
{
MotorControlMode = WPILib.Interfaces.ControlMode.Follower
};
RightSlave.Set(RightDrive.DeviceId);
}
/// <summary>
/// Configure the desired talon for closed-loop control.
/// Uses the default configuration parameters.
/// </summary>
/// <param name="talon">The talon to configure.</param>
/// <param name="type">Configure the talon for velocity or positional control.</param>
public static void ConfigureTalon(CANTalon talon, ConfigureType type)
{
switch (type)
{
case ConfigureType.Position:
EncoderParameters parametersPosition = new EncoderParameters
{
Device = FeedbackDevice.CtreMagEncoderRelative,
ReverseSensor = true,
PIDFValues = new PIDF
{
kP = 0.02,
kI = 0.00,
kD = 0.00,
kF = 0.00
},
AllowedError = 1,
NominalVoltage = 0f,
PeakVoltage = 12f
};
ConfigureTalon(talon, type, parametersPosition);
break;
case ConfigureType.Velocity:
EncoderParameters parametersVelocity = new EncoderParameters
{
Device = FeedbackDevice.CtreMagEncoderRelative,
ReverseSensor = true,
PIDFValues = new Functions.PIDF
{
kP = 0.02,
kI = 0.00,
kD = 0.00,
kF = 0.00
},
AllowedError = 1,
NominalVoltage = 0f,
PeakVoltage = 12f
};
ConfigureTalon(talon, type, parametersVelocity);
break;
}
}
/// <summary>
/// Configure the desired talon for closed-loop control.
/// </summary>
/// <param name="talon">The talon to configure.</param>
/// <param name="type">Configure the talon for velocity or positional control.</param>
/// <param name="parameters">Set the settings for the talon. Some</param>
public static void ConfigureTalon(CANTalon talon, ConfigureType type, EncoderParameters parameters)
{
// Grab the 360 degree of the magnetic encoder's absolute position.
int absolutePosition = talon.GetPulseWidthPosition() & 0xFFF; // Throw out the bits dealing with wrap-arounds.
// Set the encoder's signal.
talon.SetEncoderPostition(absolutePosition);
// Set the sensor and direction.
talon.FeedBackDevice = parameters.Device;
talon.ReverseSensor(parameters.ReverseSensor);
// Set the peak and nominal outputs. +12V is full throttle forward, -12V is full throttle backwards.
talon.ConfigNominalOutputVoltage(+parameters.NominalVoltage, -parameters.NominalVoltage);
talon.ConfigPeakOutputVoltage(+parameters.PeakVoltage, -parameters.PeakVoltage);
// Set the allowed closed-loop error.
talon.SetAllowableClosedLoopErr(parameters.AllowedError);
// Set the PIDF gains.
talon.Profile = parameters.PIDProfile; // Sets the current profile for saving the variables.
talon.P = parameters.PIDFValues.kP; // Sets the proportional gain.
talon.I = parameters.PIDFValues.kI; // Sets the integral gain.
talon.D = parameters.PIDFValues.kD; // Sets the derivative gain.
talon.F = parameters.PIDFValues.kF; // Sets the filter gain.
// Switches how to configure the talon based on the 'type' parameter.
switch (type)
{
// Run the positional config.
case ConfigureType.Position:
// Set the talon to be in positional control mode.
talon.MotorControlMode = WPILib.Interfaces.ControlMode.Position;
break;
// Run the velocity config.
case ConfigureType.Velocity:
// Set the talon to be in speed control mode.
talon.MotorControlMode = WPILib.Interfaces.ControlMode.Speed;
break;
}
}
public Drive()
{
LeftM = new CANTalon(Config.DriveLeftM);
Left1 = new CANTalon(Config.DriveLeft1);
Left1.MotorControlMode = WPILib.Interfaces.ControlMode.Follower;
Left1.Set(Config.DriveLeft1);
Left2 = new CANTalon(Config.DriveLeft2);
Left2.MotorControlMode = WPILib.Interfaces.ControlMode.Follower;
Left2.Set(Config.DriveLeft2);
LeftFollowers = new SpeedControllerGroup(Left1, Left2);
RightM = new CANTalon(Config.DriveRightM);
Right1 = new CANTalon(Config.DriveRight1);
Right1.MotorControlMode = WPILib.Interfaces.ControlMode.Follower;
Right1.Set(Config.DriveRight1);
Right2 = new CANTalon(Config.DriveRight2);
Right2.MotorControlMode = WPILib.Interfaces.ControlMode.Follower;
Right2.Set(Config.DriveRight2);
RightFollowers = new SpeedControllerGroup(Right1, Right2);
}//end Drive
void initParts()
{
scalingB = new CANTalon(0);
rightMotorA = new CANTalon(1)
{
NeutralMode = NeutralMode.Brake, Inverted = true
};
rightMotorB = new CANTalon(2)
{
NeutralMode = NeutralMode.Brake, Inverted = true
};
rightMotorC = new CANTalon(3)
{
NeutralMode = NeutralMode.Brake, Inverted = true
};
scalingArm = new CANTalon(4)
{
NeutralMode = NeutralMode.Brake
};
indexer = new CANTalon(5)
{
NeutralMode = NeutralMode.Brake
};
collector = new CANTalon(6);
shooterA = new CANTalon(9);
shooterB = new CANTalon(10);
shooterAngle = new CANTalon(11)
{
NeutralMode = NeutralMode.Brake
};
leftMotorA = new CANTalon(12)
{
NeutralMode = NeutralMode.Brake
};
leftMotorB = new CANTalon(13)
{
NeutralMode = NeutralMode.Brake
};
leftMotorC = new CANTalon(14)
{
NeutralMode = NeutralMode.Brake
};
scalingA = new CANTalon(15);
leftDriveEncoder = new Encoder(0, 1, false, EncodingType.K1X);
rightDriveEncoder = new Encoder(2, 3, true, EncodingType.K1X);
shooterSpeedB = new Counter(4)
{
SamplesToAverage = 5
};
shooterSpeedA = new Counter(5)
{
SamplesToAverage = 5
};
armLowerLimit = new DigitalInput(6);
ballSensor = new DigitalInput(7);
tapeSensor = new DigitalInput(8);
armUpperLimit = new DigitalInput(9);
flag = new Servo(9);
armPot = new AnalogInput(0);
shooterPot = new AnalogInput(1);
sonar = new AnalogInput(2);
navx = new AHRS(SPI.Port.MXP, (byte)50);
autoTimer = new Timer();
leftStick = new Joystick(0);
rightStick = new Joystick(1);
angleControl = new PIDController(.03, .0005, .5, navx, null)
{
Continuous = true
};
angleControl.SetInputRange(-180, 180);
angleControl.SetOutputRange(-.6, .6);
angleControl.SetAbsoluteTolerance(1.0);
driveControl = new PIDController(.04, .00005, .03, navx, null)
{
Continuous = true
};
driveControl.SetInputRange(-180, 180);
driveControl.SetOutputRange(-1, 1);
}
/// <summary>
/// Initialize the controllers.
/// </summary>
public static void Init()
{
// Instantiates all the hardware devices with the respective ports.
#region InstantiateDevices
can_01 = new CANTalon(1);
can_02 = new CANTalon(2);
//can_02.MotorControlMode = ControlMode.Follower;
//can_02.Set(can_01.DeviceId);
//can_02.ReverseOutput(true);
can_03 = new CANTalon(3);
can_04 = new CANTalon(4);
//can_04.MotorControlMode = ControlMode.Follower;
//can_04.Set(can_03.DeviceId);
can_05 = new CANTalon(5);
can_06 = new CANTalon(6);
can_07 = new CANTalon(7);
can_08 = new CANTalon(8);
can_09 = new CANTalon(9);
can_10 = new CANTalon(10);
can_11 = new Compressor(11);
can_12 = new PowerDistributionPanel(12);
pcm_11_0 = new Solenoid(11, 0);
pcm_11_1 = new Solenoid(11, 1);
pcm_11_2 = new Solenoid(11, 2);
usb_0 = new Joystick(0);
usb_1 = new Joystick(1);
usb_2 = new Joystick(2);
sw_0 = new DriveTrainObject(Left1, Left2, Right1, Right2);
#endregion
// Clears the sticky faults on all CAN devices.
#region ClearCANStickyFaults
can_01.ClearStickyFaults();
can_02.ClearStickyFaults();
can_03.ClearStickyFaults();
can_04.ClearStickyFaults();
can_05.ClearStickyFaults();
can_06.ClearStickyFaults();
can_07.ClearStickyFaults();
can_08.ClearStickyFaults();
can_09.ClearStickyFaults();
can_10.ClearStickyFaults();
can_11.ClearAllPCMStickyFaults();
can_12.ClearStickyFaults();
#endregion
// Disable motor safety so that it doesn't stop motors from being output to.
#region DisableSafety
can_01.SafetyEnabled = false;
can_02.SafetyEnabled = false;
can_03.SafetyEnabled = false;
can_04.SafetyEnabled = false;
can_05.SafetyEnabled = false;
can_06.SafetyEnabled = false;
can_07.SafetyEnabled = false;
can_08.SafetyEnabled = false;
can_09.SafetyEnabled = false;
can_10.SafetyEnabled = false;
sw_0.SafetyEnabled = false;
#endregion
Intake2.Inverted = true;
Shooter_Pivot.Inverted = true;
Intake1.Inverted = true;
Agitator.Inverted = true;
// Initializes the camera server with the default options.
CameraServer.Instance.StartAutomaticCapture();
//UsbCamera cam = new UsbCamera("cam0", 0);
//cam.SetResolution(320, 240);
//CameraServer.Instance.StartAutomaticCapture(cam);
// Instantiates the NavX.
NavX = new AHRS(SPI.Port.MXP);
// Creates a new copy of the turntable.
TurntableEncoder = new Encoder(8, 9);
#region PID Controllers
// Handles moving forwards and backwards for the shooter using the Pixy as the sensor.
CamForward = new CamForwardPID(3.00, 0.00, 0.00, 0.00);
// Handles the setpoint needed for the shooter using Pixy data.
ShooterPos = new ShooterPosPID(3.00, 0.00, 0.00, 0.00);
// Creates a new instance of the turn controller.
TurnController = new TurningPID(new PIDF
{
kP = 0.0465, // 0.054
kI = 0.00,
kD = 0.00,
kF = 0.00
});
// Sets the tolerance of the PID controller to 0.02.
// Cancels the turning if the error is within 0.02.
TurnController.Controller.SetAbsoluteTolerance(0.2);
// Adds the PID controller to the Live Window for easier testing.
LiveWindow.AddActuator("PID Controllers", "Turn Control", TurnController.Controller);
#endregion
}
/// <summary>
/// Sets the talon to run at the given rpm.
/// </summary>
/// <param name="rpm">Amount of RPMs to run the motor at.</param>
/// <param name="talon">Which talon to run.</param>
public static void SetVelocity(double rpm, CANTalon talon)
{
talon.Set(rpm);
}
public CANTalon.FeedbackDeviceStatus TestIsSensorPresent(CANTalon.FeedbackDevice device)
{
using (CANTalon t = NewTalon())
{
return t.IsSensorPresent(device);
}
}
