Team4388/2019-Hit-or-Miss
1
0
Fork 0
mirror of https://github.com/Team4388/2019-Hit-or-Miss.git synced 2026-06-09 00:28:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

Inches- Degrees, pid loop on arm

Browse Source 
arm to a pid loop, need to add commands to use it, change constans for for arm and wrist  from inshes to degree, need to add trigonomic functions for x,y cordinates
This commit is contained in:
lukesta182
2019-01-26 13:48:06 -07:00
parent 05ea5026e3
commit 567dfcf228
6 changed files with 176 additions and 381 deletions
Download Patch File Download Diff File Expand all files Collapse all files
2019robot/build.gradle
+1 -1
View File
@@ -1,6 +1,6 @@
plugins {
id "java"
id "edu.wpi.first.GradleRIO" version "2019.1.1"
id "edu.wpi.first.GradleRIO" version "2019.2.1"
}
def ROBOT_MAIN_CLASS = "org.usfirst.frc4388.robot.Main"
2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java
+9 -4
View File
@@ -30,12 +30,17 @@ public class Constants {
public static double kDriveEncoderTicksPerInch = (double)kDriveEncoderTicksPerRev / (kDriveWheelDiameterInches * Math.PI);
// Arm
public static int kArmEncoderTickesPerRev = 256;
public static double kArmInchesOfTravelPerRev = 3.75;
public static double kArmEncoderTicksPerInch = 126.36;
public static int kArmEncoderTickesPerRev = 4096;
public static double kArmDegreesOfTravelPerRev = 360;
public static double kArmEncoderTicksPerDegree = 11.38;
public static double kArmBasicPercentOutputUp = -0.85;
public static double kArmBasicPercentOutputDown =.7;
// Wrist
public static int kWristEncoderTicksPerRev = 4096;
public static double kWristDegreesOfTravelPerRev = 360;
public static double kWristEncoderTicksPerDegree = 11.38;
// CONTROL LOOP GAINS
// PID gains for drive velocity loop (LOW GEAR)
2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java
+6 -1
View File
@@ -65,7 +65,12 @@ public class OI
//Wrist
JoystickButton wristManualMode = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.A_BUTTON);
wristManualMode.whenPressed(new WristSetMode(WristControlMode.JOYSTICK_MANUAL));
// uncoment the line above
//Operator Xbox
/*
2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java
+1 -1
View File
@@ -65,7 +65,7 @@ public class Robot extends IterativeRobot
oi = OI.getInstance();
controlLoop.addLoopable(drive);
controlLoop.addLoopable(arm);
//controlLoop.addLoopable(arm);
operationModeChooser = new SendableChooser<OperationMode>();
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java
+158 -373
View File
@@ -1,11 +1,15 @@
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;
@@ -13,16 +17,6 @@ import org.usfirst.frc4388.utility.ControlLoopable;
import org.usfirst.frc4388.utility.PIDParams;
import org.usfirst.frc4388.utility.SoftwarePIDPositionController;
import com.ctre.phoenix.motorcontrol.ControlMode;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource;
@@ -31,394 +25,185 @@ import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.SensorCollection;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
public class Arm extends Subsystem implements ControlLoopable
import com.ctre.phoenix.motorcontrol.ControlMode;
/**
* Add your docs here.
*/
public class Arm extends Subsystem
{
//PID encoder and motor
private CANTalonEncoder armMotor;
//private WPI_TalonSRX ArmLeft;
//Control Mode Array
public static enum ArmControlMode {PID, JOYSTICK_MANUAL};
//PID controller Max Scale
private SoftwarePIDPositionController pidPositionControllerMaxScale;
//private PIDParams PositionPIDParamsMaxScale = new PIDParams(2.0, 0.0, 0.0);
private PIDParams PositionPMaxScale;
//PID controller Max Scale
private SoftwarePIDPositionController pidPositionControllerLowScale;
//private PIDParams PositionPIDParamsLowScale = new PIDParams(2.0, 0.0, 0.0);
private PIDParams PositionPLowScale;
//PID controller Max Scale
private SoftwarePIDPositionController pidPositionControllerSwitch;
//private PIDParams PositionPIDParamsSwitch = new PIDParams(2.0, 0.0, 0.0);
private PIDParams PositionPSwitch;
//PID controller Max Scale
private SoftwarePIDPositionController pidPositionControllerLowest;
//private PIDParams PositionPIDParamsLowest = new PIDParams(2.0, 0.0, 0.0);
private PIDParams PositionPLowest;
//Motor Controllers
private ArrayList<CANTalonEncoder> motorControllers = new ArrayList<CANTalonEncoder>();
//PID target
private double targetPPosition;
//Encoder ticks to inches for encoders
public static final double ENCODER_TICKS_TO_INCHES = Constants.kArmEncoderTicksPerInch;
//control mode for joystick control
private DriveControlMode controlMode = DriveControlMode.JOYSTICK;
private double periodMs;
//Non Linear Joystick
public static final double STICK_DEADBAND = 0.02;
public static final double MOVE_NON_LINEARITY = 1.0;
private int moveNonLinear = 0;
private double moveScale = 1.0;
private double moveTrim = 0.0;
private boolean isFinished;
private DifferentialDrive m_drive;
//private LimitSwitchSource limitSwitch = new DigitalInput(1);
LimitSwitchSource limitSwitchSource;
public boolean pressed;
SensorCollection isPressed;
public boolean armControlMode = false;
// Motor controllers
//private ArrayList<CANTalonEncoder> motorControllers = new ArrayList<CANTalonEncoder>();
public Arm()
{
try
{
//PID Arm encoder and talon
armMotor = new CANTalonEncoder(RobotMap.ARM_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder);
//ArmLeft = new WPI_TalonSRX(RobotMap.ARM_MOTOR2_ID);
//ArmMotor.setInverted(false);
private CANTalonEncoder arm1;
//Setting left Arm motor as follower
//ArmLeft.set(ControlMode.Follower, ArmMotor.getDeviceID());
//ArmLeft.setInverted(false);
//ArmLeft.setNeutralMode(NeutralMode.Brake);
armMotor.setNeutralMode(NeutralMode.Brake);
armMotor.setSensorPhase(true);
//Limit Switch Left
//ArmLeft.overrideLimitSwitchesEnable(true);
//ArmLeft.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
//ArmLeft.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
//Limit Switch Right
//ArmMotor.overrideLimitSwitchesEnable(true);
//ArmMotor.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
//ArmMotor.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
//Change This boi
// ArmMotor.configForwardSoftLimitThreshold(10000, 0); //right here
//ArmMotor.configReverseSoftLimitThreshold(5, 0);
//ArmMotor.configForwardSoftLimitEnable(true, 0);
//ArmMotor.configReverseSoftLimitEnable(true, 0);
//sos
//ArmMotor.enableLimitSwitch(true, true);
//Encoder ticks to inches for encoders
public static final double ENCODER_TICKS_TO_INCHES = Constants.kArmEncoderTicksPerDegree;
// PID controller and params
private MPTalonPIDController mpController;
}
catch(Exception e)
{
System.err.println("You thought the code would work, but it was me, error. An error occurred in the Arm Construtor");
}
public static int PID_SLOT = 0;
public static int MP_SLOT = 1;
private PIDParams mpPIDParams = new PIDParams(0.2, 0.0, 0.0, 0.0, 0.005, 0.0);
private PIDParams pidPIDParamsHiGear = new PIDParams(0.075, 0.0, 0.0, 0.0, 0.0, 0.0);
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;
// 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);
}
private double nonlinearStickCalcPositive(double move, double moveNonLinearity)
catch(Exception e)
{
return Math.sin(Math.PI / 2.0 * moveNonLinearity * move) / Math.sin(Math.PI / 2.0 * moveNonLinearity);
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);
}
private double nonlinearStickCalcNegative(double move, double moveNonLinearity)
{
return Math.asin(moveNonLinearity * move) / Math.asin(moveNonLinearity);
}
private boolean inDeadZone(double input)
{
boolean inDeadZone;
if (Math.abs(input) < STICK_DEADBAND)
{
inDeadZone = true;
}
else
{
inDeadZone = false;
//Setting the target position for the PID loop and set the control mode to PID
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) {
if (targetPosition < MIN_POSITION_INCHES) {
return MIN_POSITION_INCHES;
}
else if (targetPosition > MAX_POSITION_INCHES) {
return MAX_POSITION_INCHES;
}
return inDeadZone;
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);
}
//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 onLoop(double timestamp)
{
synchronized (Arm.this) {
switch(getArmControlMode() ) {
case PID:
controlPID();
break;
case JOYSTICK_MANUAL:
controlManualWithJoystick();
break;
default:
break;
}
}
}
private void controlPID()
{
double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis();
double deltaPosition = joystickPosition * joystickInchesPerMs;
targetPositionInchesPID = targetPositionInchesPID + deltaPosition;
updatePositionPID(targetPositionInchesPID);
}
//Method for controlling the motor with the joystick
private void controlManualWithJoystick()
{
double joystickSpeed;
private double limitValue(double value)
{
if (value > 1.0)
{
value = 1.0;
}
else if (value < -1.0)
{
value = -1.0;
}
return value;
}
public double adjustJoystickSensitivity(double scale, double trim, double move, int nonLinearFactor, double wheelNonLinearity)
{
if (inDeadZone(move))
{
return 0;
}
move += trim;
move *= scale;
move = limitValue(move);
int iterations = Math.abs(nonLinearFactor);
for (int i = 0; i < iterations; i++)
{
if (nonLinearFactor > 0)
{
move = nonlinearStickCalcPositive(move, wheelNonLinearity);
}
else
{
move = nonlinearStickCalcNegative(move, wheelNonLinearity);
}
}
return move;
}
public void setArmMode()
{
setControlMode(DriveControlMode.JOYSTICK);
}
public void resetArmEncoder()
{
armMotor.setSelectedSensorPosition(0, 0, 0);
}
public void moveArmXbox()
{
double moveArmInput;
double armSafeZone = 15;
double armPos = getEncoderArmPosition();
moveArmInput = Robot.oi.getOperatorController().getLeftYAxis();
//double moveArmSensitivity = adjustJoystickSensitivity(moveScale, moveTrim, moveArmInput, moveNonLinear, MOVE_NON_LINEARITY);
boolean holdButtonPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.A_BUTTON);
boolean armTuningPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.Y_BUTTON);
if(armTuningPressed == true)
{
armMotor.set(moveArmInput * 0.5);
}
else if(armTuningPressed == false)
{
armMotor.set(moveArmInput);
}
}
// System.out.println(ArmPos); //-6.9 to 1.9 total: 8.8 range
//PID encoder position
public double getEncoderArmPosition()
{
return armMotor.getPositionWorld();
}
public double getArmHeightInchesAboveFloor()
{
return armMotor.getPositionWorld();
joystickSpeed = -Robot.oi.getOperatorController().getLeftYAxis();
setSpeedJoystick(joystickSpeed);
}
public synchronized void setControlMode(DriveControlMode controlMode)
{
this.controlMode = controlMode;
isFinished = false;
}
/*
public void setArmPIDMaxScale(double ArmPosition, double maxError, double minError)
{
double ArmTargetPos = 0;
this.targetPPosition = ArmTargetPos;
pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); ///////TARGET POSITION WHERE??
Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_MAX_SCALE);
}
public void setArmPIDLowScale(double ArmPosition, double maxError, double minError)
{
double ArmTargetPos = 0;
this.targetPPosition = ArmTargetPos;
pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError);
Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_LOW_SCALE);
}
public void setArmPIDSwitch(double ArmPosition, double maxError, double minError)
{
double ArmTargetPos = 0;
this.targetPPosition = ArmTargetPos;
pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError);
Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_SWITCH);
}
public void setArmPIDLowest(double ArmPosition, double maxError, double minError)
{
double ArmTargetPos = 0;
this.targetPPosition = ArmTargetPos;
pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError);
Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_LOWEST);
}
*/
public void rawSetOutput(double output){
armMotor.set(/*ControlMode.PercentOutput,*/ output);
}
public void holdInPos()
{
armMotor.set(-0.43 * 0.2);
}
public void stopMotors()
{
armMotor.set(0);
}
public void isSwitchPressed()
{
pressed = false;
isPressed = armMotor.getSensorCollection();
if(isPressed.isFwdLimitSwitchClosed() == true)
{
if (controlMode == DriveControlMode.JOYSTICK) {
Robot.arm.setControlMode(DriveControlMode.STOP_MOTORS);
}
pressed = true;
}
else
{
if (controlMode == DriveControlMode.STOP_MOTORS){
{
Robot.arm.setControlMode(DriveControlMode.JOYSTICK);
}
pressed = false;
}
}
}
//pressed = (isPressed.isFwdLimitSwitchClosed() == true) ? true : false;
@Override
public void controlLoopUpdate()
{
if (controlMode == DriveControlMode.JOYSTICK || controlMode == DriveControlMode.CLIMB)
{
moveArmXbox();
}
else if (!isFinished)
{
//PID control mode
if(controlMode == DriveControlMode.MOVE_POSITION_MAX_SCALE)
{
isFinished = pidPositionControllerMaxScale.controlLoopUpdate(getEncoderArmPosition());
}
else if(controlMode == DriveControlMode.MOVE_POSITION_LOW_SCALE)
{
isFinished = pidPositionControllerLowScale.controlLoopUpdate(getEncoderArmPosition());
}
else if(controlMode == DriveControlMode.MOVE_POSITION_SWITCH)
{
isFinished = pidPositionControllerSwitch.controlLoopUpdate(getEncoderArmPosition());
}
else if(controlMode == DriveControlMode.MOVE_POSITION_LOWEST)
{
isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderArmPosition());
}
/*
else if(controlMode == DriveControlMode.RAW)
{
isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderArmPosition());
}
*/
}
}
@Override
public void initDefaultCommand()
{
}
@Override
public void periodic()
{
// isSwitchPressed();
}
@Override
public void setPeriodMs(long periodMs)
{
//PID controller
pidPositionControllerMaxScale = new SoftwarePIDPositionController(PositionPMaxScale, armMotor);
pidPositionControllerLowScale = new SoftwarePIDPositionController(PositionPLowScale, armMotor);
pidPositionControllerSwitch = new SoftwarePIDPositionController(PositionPSwitch, armMotor);
pidPositionControllerLowest = new SoftwarePIDPositionController(PositionPLowest, armMotor);
this.periodMs = periodMs;
}
public synchronized boolean isFinished()
{
public synchronized boolean isFinished()
{
return isFinished;
}
public double getPeriodMs()
{
return periodMs;
public synchronized void setFinished(boolean isFinished)
{
this.isFinished = isFinished;
}
public void updateStatus(Robot.OperationMode operationMode)
@Override
public void initDefaultCommand()
{
}
public void updateStatus(Robot.OperationMode operationMode)
{
if (operationMode == Robot.OperationMode.TEST)
{
try
{
SmartDashboard.putNumber("Arm Pos Ticks", armMotor.getSelectedSensorPosition(0));
SmartDashboard.putNumber("Arm Pos Inches", getArmHeightInchesAboveFloor());
//SmartDashboard.putData(pressed);
}
catch (Exception e)
catch (Exception e)
{
System.err.println("Drivetrain update status error");
System.err.println("Arm update status error");
}
}
}
}
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java
+1 -1
View File
@@ -40,7 +40,7 @@ public class Wrist extends Subsystem
private CANTalonEncoder wristRight;
//Encoder ticks to inches for encoders
public static final double ENCODER_TICKS_TO_INCHES = Constants.kArmEncoderTicksPerInch;
public static final double ENCODER_TICKS_TO_INCHES = Constants.kArmEncoderTicksPerDegree;
// PID controller and params
private MPTalonPIDController mpController;