mirror of
https://github.com/Team4388/2019-Hit-or-Miss.git
synced 2026-06-09 00:28:05 -06:00
Arm added, All IDS updated
This commit is contained in:
lukesta182
@@ -9,6 +9,7 @@ package org.usfirst.frc4388.robot;
|
||||
|
||||
public class Constants {
|
||||
|
||||
public static double kLooperDt = 0.01;
|
||||
public static double kDriveWheelDiameterInches = 6.04;
|
||||
public static double kTrackLengthInches = 10;
|
||||
public static double kTrackWidthInches = 26.5;
|
||||
|
||||
@@ -18,19 +18,29 @@ public class RobotMap {
|
||||
public static final int DRIVETRAIN_RIGHT_MOTOR2_CAN_ID = 5;
|
||||
|
||||
//carriage motors
|
||||
public static final int WRIST_LEFT_MOTOR_CAN_ID = 8;
|
||||
public static final int WRITST_RIGHT_MOTOR_CAN_ID = 9;
|
||||
public static final int WRIST_LEFT_MOTOR_CAN_ID = 6;
|
||||
public static final int INTAKE_MOTOR = 7;
|
||||
|
||||
//Arm Motors
|
||||
public static final int ARM_MOTOR1_ID = 6;
|
||||
public static final int ARM_MOTOR2_ID = 7;
|
||||
public static final int ARM_MOTOR1_ID = 8;
|
||||
public static final int ARM_MOTOR2_ID = 9;
|
||||
public static final int CLIMBER_CAN_ID = 10;
|
||||
public static final int FLIP_OUT_CLIMBER = 11;
|
||||
public static final int CLIMBER_LEFT = 12;
|
||||
public static final int CLIMBER_RIGHT = 13;
|
||||
|
||||
|
||||
|
||||
// Pneumatics
|
||||
public static final int DRIVETRAIN_SPEEDSHIFT_PCM_ID = 0;
|
||||
public static final int DRIVETRAIN_SPEEDSHIFT2_PCM_ID = 1;
|
||||
public static final int OPEN_INTAKE_PCM_ID = 4;
|
||||
public static final int CLOSE_INTAKE_PCM_ID = 5;
|
||||
public static final int OPEN_BALL_INTAKE_PCM_ID = 2;
|
||||
public static final int CLOSE_BALL_INTAKE_PCM_ID = 3;
|
||||
public static final int LIFT_HATCH_PCM_ID = 4;
|
||||
public static final int LOWER_HATCH_INTAKE_PCM_ID = 5;
|
||||
public static final int END_EFECTOR_EXPAND_PCM_ID = 6;
|
||||
public static final int END_EFECTOR_CONTRACT_PCM_ID = 7;
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
@@ -1,99 +0,0 @@
|
||||
package org.usfirst.frc4388.robot.commands;
|
||||
|
||||
import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
|
||||
import org.usfirst.frc4388.robot.Constants;
|
||||
import org.usfirst.frc4388.robot.Robot;
|
||||
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj.command.Command;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
|
||||
|
||||
public class ArmBasic extends Command {
|
||||
private double m_targetHeightInchesAboveFloor;
|
||||
private double m_speed;
|
||||
private boolean m_goingUp;
|
||||
private double m_commandInitTimestamp;
|
||||
private double m_lastCommandExecuteTimestamp = 0.0;
|
||||
private double m_lastCommandExecutePeriod = 0.0;
|
||||
public static boolean isfinishedElevatorBasic;
|
||||
|
||||
public ArmBasic(double targetHeightInchesAboveFloor) {
|
||||
requires(Robot.arm);
|
||||
m_targetHeightInchesAboveFloor = targetHeightInchesAboveFloor;
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize()
|
||||
{
|
||||
Robot.arm.setControlMode(DriveControlMode.RAW);
|
||||
|
||||
double currentHeight = Robot.arm.getArmHeightInchesAboveFloor();
|
||||
// start out at half speed, as crude way to reduce slippage
|
||||
m_goingUp = (m_targetHeightInchesAboveFloor > currentHeight);
|
||||
System.out.println("initialize(): cur=" + currentHeight + " , target=" + m_targetHeightInchesAboveFloor + " , going " + (m_goingUp ? "UP" : "DOWN"));
|
||||
if (m_goingUp) {
|
||||
m_speed = Constants.kArmBasicPercentOutputUp;
|
||||
}
|
||||
else {
|
||||
m_speed = Constants.kArmBasicPercentOutputDown;
|
||||
}
|
||||
m_commandInitTimestamp = Timer.getFPGATimestamp();
|
||||
|
||||
}
|
||||
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
protected void execute() {
|
||||
// Measure *actual* update period
|
||||
double currentTimestamp = Timer.getFPGATimestamp();
|
||||
if (m_lastCommandExecuteTimestamp > 0.001) { // ie, if this is NOT the first time
|
||||
m_lastCommandExecutePeriod = currentTimestamp - m_lastCommandExecuteTimestamp;
|
||||
}
|
||||
m_lastCommandExecuteTimestamp = currentTimestamp;
|
||||
Robot.arm.rawSetOutput(m_speed);
|
||||
//SmartDashboard.putNumber("DSB Period", m_lastCommandExecutePeriod);
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
protected boolean isFinished() {
|
||||
boolean finished=false;
|
||||
double currentHeight = Robot.arm.getArmHeightInchesAboveFloor();
|
||||
double remaining = (m_targetHeightInchesAboveFloor - currentHeight) * (m_goingUp ? 1.0 : -1.0);
|
||||
System.out.println("cur=" + currentHeight + " , remaining=" + remaining + " , target=" + m_targetHeightInchesAboveFloor);
|
||||
if (remaining < 0.0) {
|
||||
finished = true;
|
||||
|
||||
}
|
||||
/*} else if (remaining < 0.1 * m_maxVelocityInchesPerSec / 2.0) { // last 100 ms
|
||||
velocity = m_maxVelocityInchesPerSec / 4.0; // quarter speed
|
||||
} else if (remaining < 0.3 * m_maxVelocityInchesPerSec) { // last 300 ms
|
||||
velocity = m_maxVelocityInchesPerSec / 2.0; // half speed
|
||||
}*/
|
||||
|
||||
|
||||
if (!finished) {
|
||||
SmartDashboard.putNumber("EB Dist", currentHeight);
|
||||
} else {
|
||||
SmartDashboard.putNumber("EB finDist", currentHeight);
|
||||
}
|
||||
return finished;
|
||||
}
|
||||
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
double currentTimestamp = Timer.getFPGATimestamp();
|
||||
SmartDashboard.putNumber("EB Runtime", currentTimestamp - m_commandInitTimestamp);
|
||||
|
||||
isfinishedElevatorBasic = isFinished();
|
||||
|
||||
Robot.arm.rawSetOutput(0.0);
|
||||
|
||||
Robot.arm.setControlMode(DriveControlMode.JOYSTICK);
|
||||
}
|
||||
|
||||
// Called when another command which requires one or more of the same
|
||||
// subsystems is scheduled to run
|
||||
protected void interrupted() {
|
||||
end();
|
||||
}
|
||||
}
|
||||
@@ -1,49 +1,62 @@
|
||||
|
||||
package org.usfirst.frc4388.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.wpilibj.command.Subsystem;
|
||||
|
||||
import java.util.ArrayList;
|
||||
|
||||
import org.usfirst.frc4388.controller.XboxController;
|
||||
import org.usfirst.frc4388.robot.Constants;
|
||||
import org.usfirst.frc4388.robot.Robot;
|
||||
import org.usfirst.frc4388.robot.RobotMap;
|
||||
import org.usfirst.frc4388.utility.MPTalonPIDController;
|
||||
import org.usfirst.frc4388.robot.commands.*;
|
||||
import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
|
||||
import org.usfirst.frc4388.utility.CANTalonEncoder;
|
||||
import org.usfirst.frc4388.utility.ControlLoopable;
|
||||
import org.usfirst.frc4388.utility.Loop;
|
||||
import org.usfirst.frc4388.utility.MPTalonPIDController;
|
||||
import org.usfirst.frc4388.utility.PIDParams;
|
||||
import org.usfirst.frc4388.utility.SoftwarePIDPositionController;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
|
||||
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
|
||||
import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource;
|
||||
import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
|
||||
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
import com.ctre.phoenix.motorcontrol.SensorCollection;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
|
||||
import org.usfirst.frc4388.utility.TalonSRXEncoder;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.ControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
|
||||
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public class Arm extends Subsystem
|
||||
import edu.wpi.first.wpilibj.Solenoid;
|
||||
import edu.wpi.first.wpilibj.command.Subsystem;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
|
||||
public class Arm extends Subsystem implements Loop
|
||||
{
|
||||
//Control Mode Array
|
||||
public static enum ArmControlMode {PID, JOYSTICK_MANUAL};
|
||||
private static Arm instance;
|
||||
|
||||
//Motor Controllers
|
||||
private ArrayList<CANTalonEncoder> motorControllers = new ArrayList<CANTalonEncoder>();
|
||||
public static enum ArmControlMode {PID, JOYSTICK_MANUAL };
|
||||
|
||||
|
||||
private CANTalonEncoder arm1;
|
||||
// One revolution of the 1-3 GEAR RATION ON THE ARM * 4096 ticks
|
||||
public static final double ENCODER_TICKS_TO_DEGREES = (36.0 / 12.0) * (36.0 / 24.0) * (34.0 / 24.0) * 4096.0 / (1.88 * Math.PI);
|
||||
|
||||
// Defined speeds
|
||||
public static final double CLIMB_SPEED = -1.0;
|
||||
public static final double TEST_SPEED_UP = 0.3;
|
||||
public static final double TEST_SPEED_DOWN = -0.3;
|
||||
public static final double AUTO_ZERO_SPEED = -0.3;
|
||||
public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75;
|
||||
|
||||
// Defined positions
|
||||
public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0;
|
||||
public static final double ZERO_POSITION_INCHES = -0.25;
|
||||
public static final double NEAR_ZERO_POSITION_INCHES = 0.0;
|
||||
public static final double MIN_POSITION_INCHES = 0.0;
|
||||
public static final double MAX_POSITION_INCHES = 83.4;
|
||||
public static final double AFTER_INTAKE_POSITION_INCHES = 4.0;
|
||||
|
||||
//Encoder ticks to inches for encoders
|
||||
public static final double ENCODER_TICKS_TO_INCHES = Constants.kArmEncoderTicksPerDegree;
|
||||
|
||||
// PID controller and params
|
||||
|
||||
// Motion profile max velocities and accel times
|
||||
public static final double MP_MAX_VELOCITY_INCHES_PER_SEC = 60;
|
||||
public static final double MP_T1 = 400; // Fast = 300
|
||||
public static final double MP_T2 = 150; // Fast = 150
|
||||
|
||||
// Motor controllers
|
||||
private ArrayList<TalonSRXEncoder> motorControllers = new ArrayList<TalonSRXEncoder>();
|
||||
|
||||
private CANTalonEncoder motor1;
|
||||
private TalonSRX motor2;
|
||||
|
||||
// PID controller and params
|
||||
private MPTalonPIDController mpController;
|
||||
|
||||
public static int PID_SLOT = 0;
|
||||
@@ -54,64 +67,68 @@ public class Arm extends Subsystem
|
||||
private PIDParams pidPIDParamsLoGear = new PIDParams(0.45, 0.0, 0.0, 0.0, 0.0, 0.0);
|
||||
public static final double KF_UP = 0.005;
|
||||
public static final double KF_DOWN = 0.0;
|
||||
public static final double PID_ERROR_INCHES = 1.0;
|
||||
public static final double PID_ERROR_INCHES = 1.0;
|
||||
private long periodMs = (long)(Constants.kLooperDt * 1000.0);
|
||||
|
||||
// Defined positions
|
||||
public static final double MIN_POSITION_INCHES = 0.0;
|
||||
public static final double MAX_POSITION_INCHES = 83.4;
|
||||
public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75;
|
||||
public static final double JOYSTICK_INCHES_PER_MS_LO = JOYSTICK_INCHES_PER_MS_HI/3.68 * 0.8;
|
||||
|
||||
//Misc
|
||||
private ArmControlMode armControlMode = ArmControlMode.JOYSTICK_MANUAL;
|
||||
private boolean isFinished;
|
||||
private double targetPositionInchesPID = 0;
|
||||
|
||||
private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO;
|
||||
|
||||
public Arm()
|
||||
{
|
||||
try
|
||||
{
|
||||
//PID arm encoder and talon
|
||||
arm1 = new CANTalonEncoder(RobotMap.ARM_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder);
|
||||
}
|
||||
catch(Exception e)
|
||||
{
|
||||
System.err.println("You thought the code would work, but it was me, error. An error occurred in the Arm Construtor");
|
||||
}
|
||||
}
|
||||
|
||||
//Method for setting the control mode for the arm
|
||||
private synchronized void setArmControlMode(ArmControlMode controlMode)
|
||||
{
|
||||
this.armControlMode = controlMode;
|
||||
}
|
||||
|
||||
//Getting the control mode for the arm
|
||||
private synchronized ArmControlMode getArmControlMode()
|
||||
{
|
||||
return this.armControlMode;
|
||||
}
|
||||
|
||||
//Setting the speed for the motor on the arm along with setting the control mode to manual
|
||||
public void setSpeed(double speed)
|
||||
{
|
||||
arm1.set(ControlMode.PercentOutput, speed);
|
||||
setArmControlMode(ArmControlMode.JOYSTICK_MANUAL);
|
||||
// Misc
|
||||
public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0;
|
||||
private boolean isFinished;
|
||||
private ArmControlMode armControlMode = ArmControlMode.JOYSTICK_MANUAL;
|
||||
private double targetPositionInchesPID = 0;
|
||||
private boolean firstMpPoint;
|
||||
|
||||
|
||||
private Arm() {
|
||||
try {
|
||||
motor1 = new CANTalonEncoder(RobotMap.ARM_MOTOR1_ID, ENCODER_TICKS_TO_DEGREES, FeedbackDevice.QuadEncoder);
|
||||
//motor2 = CANTallon.createPermanentSlaveTalon(RobotMap.ARM_MOTOR_2_CAN_ID, RobotMap.ELEVATOR_MOTOR_1_CAN_ID);
|
||||
|
||||
|
||||
|
||||
}
|
||||
catch (Exception e) {
|
||||
System.err.println("An error occurred in the DriveTrain constructor");
|
||||
}
|
||||
}
|
||||
|
||||
//Setting the target position for the PID loop and set the control mode to PID
|
||||
public void setPositionPID(double targetPositionInches)
|
||||
{
|
||||
mpController.setPIDSlot(PID_SLOT);
|
||||
@Override
|
||||
public void initDefaultCommand() {
|
||||
}
|
||||
|
||||
public void resetZeroPosition(double position) {
|
||||
mpController.resetZeroPosition(position);
|
||||
}
|
||||
|
||||
private synchronized void setArmControlMode(ArmControlMode controlMode) {
|
||||
this.armControlMode = controlMode;
|
||||
}
|
||||
|
||||
private synchronized ArmControlMode getArmControlMode() {
|
||||
return this.armControlMode;
|
||||
}
|
||||
|
||||
|
||||
public void setSpeedJoystick(double speed) {
|
||||
motor1.set(ControlMode.PercentOutput, speed);
|
||||
setArmControlMode(ArmControlMode.JOYSTICK_MANUAL);
|
||||
}
|
||||
|
||||
public void setPositionPID(double targetPositionInches) {
|
||||
mpController.setPIDSlot(PID_SLOT);
|
||||
updatePositionPID(targetPositionInches);
|
||||
setArmControlMode(ArmControlMode.PID);
|
||||
setFinished(false);
|
||||
}
|
||||
|
||||
//Setting range for target position
|
||||
private double limitPosition(double targetPosition) {
|
||||
}
|
||||
|
||||
public void updatePositionPID(double targetPositionInches) {
|
||||
targetPositionInchesPID = limitPosition(targetPositionInches);
|
||||
double startPositionInches = motor1.getPositionWorld();
|
||||
mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN);
|
||||
}
|
||||
|
||||
|
||||
private double limitPosition(double targetPosition) {
|
||||
if (targetPosition < MIN_POSITION_INCHES) {
|
||||
return MIN_POSITION_INCHES;
|
||||
}
|
||||
@@ -121,35 +138,25 @@ public class Arm extends Subsystem
|
||||
|
||||
return targetPosition;
|
||||
}
|
||||
|
||||
//Method for updating the PID target position
|
||||
public void updatePositionPID(double targetPositionInches)
|
||||
{
|
||||
targetPositionInchesPID = limitPosition(targetPositionInches);
|
||||
double startPositionInches = arm1.getPositionWorld();
|
||||
mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onStart(double timestamp) {
|
||||
mpController.setPIDSlot(PID_SLOT);
|
||||
mpController.setPIDSlot(PID_SLOT);
|
||||
}
|
||||
|
||||
//Getting the current encoder position
|
||||
public double getPositionInches()
|
||||
{
|
||||
return arm1.getPositionWorld();
|
||||
}
|
||||
|
||||
//Setting the speed for the motors in manual mode
|
||||
public void setSpeedJoystick(double speed)
|
||||
{
|
||||
arm1.set(ControlMode.PercentOutput, speed);
|
||||
setArmControlMode(armControlMode.JOYSTICK_MANUAL);
|
||||
}
|
||||
@Override
|
||||
public void onStop(double timestamp) {
|
||||
// TODO Auto-generated method stub
|
||||
|
||||
}
|
||||
|
||||
//@Override
|
||||
public void onLoop(double timestamp)
|
||||
{
|
||||
@Override
|
||||
public void onLoop(double timestamp) {
|
||||
synchronized (Arm.this) {
|
||||
switch(getArmControlMode() ) {
|
||||
switch( getArmControlMode() ) {
|
||||
case PID:
|
||||
controlPID();
|
||||
controlPidWithJoystick();
|
||||
break;
|
||||
case JOYSTICK_MANUAL:
|
||||
controlManualWithJoystick();
|
||||
@@ -158,52 +165,61 @@ public class Arm extends Subsystem
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private void controlPID()
|
||||
{
|
||||
}
|
||||
|
||||
private void controlPidWithJoystick() {
|
||||
double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis();
|
||||
double deltaPosition = joystickPosition * joystickInchesPerMs;
|
||||
|
||||
double deltaPosition = joystickPosition *.5;
|
||||
targetPositionInchesPID = targetPositionInchesPID + deltaPosition;
|
||||
updatePositionPID(targetPositionInchesPID);
|
||||
updatePositionPID(targetPositionInchesPID);
|
||||
}
|
||||
|
||||
//Method for controlling the motor with the joystick
|
||||
private void controlManualWithJoystick()
|
||||
{
|
||||
double joystickSpeed;
|
||||
|
||||
joystickSpeed = -Robot.oi.getOperatorController().getLeftYAxis();
|
||||
setSpeedJoystick(joystickSpeed);
|
||||
|
||||
private void controlManualWithJoystick() {
|
||||
double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis();
|
||||
setSpeedJoystick(joyStickSpeed);
|
||||
}
|
||||
|
||||
|
||||
public synchronized boolean isFinished()
|
||||
{
|
||||
public double getPositionInches() {
|
||||
return motor1.getPositionWorld();
|
||||
}
|
||||
|
||||
// public double getAverageMotorCurrent() {
|
||||
// return (Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_1_CAN_ID) + Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_2_CAN_ID) + Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID)) / 3;
|
||||
// }
|
||||
|
||||
public double getAverageMotorCurrent() {
|
||||
return (motor1.getOutputCurrent() + motor2.getOutputCurrent() / 2);
|
||||
}
|
||||
|
||||
public synchronized boolean isFinished() {
|
||||
return isFinished;
|
||||
}
|
||||
|
||||
public synchronized void setFinished(boolean isFinished)
|
||||
{
|
||||
|
||||
public synchronized void setFinished(boolean isFinished) {
|
||||
this.isFinished = isFinished;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initDefaultCommand()
|
||||
{
|
||||
}
|
||||
|
||||
public void updateStatus(Robot.OperationMode operationMode)
|
||||
{
|
||||
if (operationMode == Robot.OperationMode.TEST)
|
||||
{
|
||||
try
|
||||
{
|
||||
|
||||
public double getPeriodMs() {
|
||||
return periodMs;
|
||||
}
|
||||
|
||||
public void updateStatus(Robot.OperationMode operationMode) {
|
||||
if (operationMode == Robot.OperationMode.TEST) {
|
||||
try {
|
||||
SmartDashboard.putNumber("Elevator Position Inches", motor1.getPositionWorld());
|
||||
SmartDashboard.putNumber("Elevator Average Amps", getAverageMotorCurrent());
|
||||
SmartDashboard.putNumber("Elevator Target PID Position", targetPositionInchesPID);
|
||||
}
|
||||
catch (Exception e)
|
||||
{
|
||||
System.err.println("Arm update status error");
|
||||
catch (Exception e) {
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public static Arm getInstance() {
|
||||
if(instance == null) {
|
||||
instance = new Arm();
|
||||
}
|
||||
return instance;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,44 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package org.usfirst.frc4388.utility;
|
||||
|
||||
import edu.wpi.first.wpilibj.command.Command;
|
||||
|
||||
public class Loop extends Command {
|
||||
public Loop() {
|
||||
// Use requires() here to declare subsystem dependencies
|
||||
// eg. requires(chassis);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
@Override
|
||||
protected void initialize() {
|
||||
}
|
||||
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
@Override
|
||||
protected void execute() {
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
@Override
|
||||
protected boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Called once after isFinished returns true
|
||||
@Override
|
||||
protected void end() {
|
||||
}
|
||||
|
||||
// Called when another command which requires one or more of the same
|
||||
// subsystems is scheduled to run
|
||||
@Override
|
||||
protected void interrupted() {
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,84 @@
|
||||
package org.usfirst.frc4388.utility;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.ControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
|
||||
|
||||
public class TalonSRXEncoder extends WPI_TalonSRX
|
||||
{
|
||||
public static int TIMEOUT_MS = 0;
|
||||
public static int PID_IDX = 0;
|
||||
|
||||
private double encoderTicksToWorld;
|
||||
private boolean isRight = true;
|
||||
|
||||
public TalonSRXEncoder(int deviceId, double encoderTicksToWorld, FeedbackDevice feedbackDevice) {
|
||||
this(deviceId, encoderTicksToWorld, false, feedbackDevice);
|
||||
}
|
||||
|
||||
public TalonSRXEncoder(int deviceNumber, double encoderTicksToWorld, boolean isRight, FeedbackDevice feedbackDevice) {
|
||||
super(deviceNumber);
|
||||
this.configSelectedFeedbackSensor(feedbackDevice, PID_IDX, TIMEOUT_MS);
|
||||
this.encoderTicksToWorld = encoderTicksToWorld;
|
||||
this.isRight = isRight;
|
||||
}
|
||||
|
||||
public boolean isRight() {
|
||||
return isRight;
|
||||
}
|
||||
|
||||
public void setRight(boolean isRight) {
|
||||
this.isRight = isRight;
|
||||
}
|
||||
|
||||
public void setPID(int slotId, double kP, double kI, double kD) {
|
||||
this.setPIDF(slotId, kP, kI, kD, 0);
|
||||
}
|
||||
|
||||
public void setPIDF(int slotId, double kP, double kI, double kD, double kF) {
|
||||
this.config_kP(slotId, kP, TIMEOUT_MS);
|
||||
this.config_kI(slotId, kI, TIMEOUT_MS);
|
||||
this.config_kD(slotId, kD, TIMEOUT_MS);
|
||||
this.config_kF(slotId, kF, TIMEOUT_MS);
|
||||
}
|
||||
|
||||
public void setPIDFIZone(int slotId, double kP, double kI, double kD, double kF, int iZone) {
|
||||
this.config_kP(slotId, kP, TIMEOUT_MS);
|
||||
this.config_kI(slotId, kI, TIMEOUT_MS);
|
||||
this.config_kD(slotId, kD, TIMEOUT_MS);
|
||||
this.config_kF(slotId, kF, TIMEOUT_MS);
|
||||
this.config_IntegralZone(slotId, iZone, TIMEOUT_MS);
|
||||
}
|
||||
|
||||
public double convertEncoderTicksToWorld(double encoderTicks) {
|
||||
return encoderTicks / encoderTicksToWorld;
|
||||
}
|
||||
|
||||
public int convertEncoderWorldToTicks(double worldValue) {
|
||||
return (int)(worldValue * encoderTicksToWorld);
|
||||
}
|
||||
|
||||
public void setWorld(ControlMode mode, double worldValue) {
|
||||
this.set(mode, convertEncoderWorldToTicks(worldValue));
|
||||
}
|
||||
|
||||
public void setPosition(int value) {
|
||||
this.setSelectedSensorPosition(value, PID_IDX, TIMEOUT_MS);
|
||||
}
|
||||
|
||||
public void setPositionWorld(double worldValue) {
|
||||
this.setSelectedSensorPosition(convertEncoderWorldToTicks(worldValue), PID_IDX, TIMEOUT_MS);
|
||||
}
|
||||
|
||||
public double getPositionWorld() {
|
||||
return convertEncoderTicksToWorld(this.getSelectedSensorPosition(PID_IDX));
|
||||
}
|
||||
|
||||
public void setVelocityWorld(double worldValue) {
|
||||
this.set(ControlMode.Velocity, convertEncoderWorldToTicks(worldValue) * 0.1);
|
||||
}
|
||||
|
||||
public double getVelocityWorld() {
|
||||
return convertEncoderTicksToWorld(this.getSelectedSensorVelocity(PID_IDX) / 0.1);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user