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

arm pid works

Browse Source 
added pid to arm subsystem
This commit is contained in:
lukesta182
2019-03-06 21:51:23 -07:00
parent 363df64422
commit 1b120ac85c
5 changed files with 69 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java
+1
View File
@@ -92,6 +92,7 @@ public class OI
CloseIntake.whenPressed(new IntakePosition(false));
*/
SmartDashboard.putData("switch to manuel", new SetManual());
SmartDashboard.putData("run arm test", new ArmTest());
JoystickButton safteySwitch = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.START_BUTTON);
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetManual.java
+36
View File
@@ -0,0 +1,36 @@
/*----------------------------------------------------------------------------*/
/* 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.robot.commands;
import org.usfirst.frc4388.robot.subsystems.Arm.ArmControlMode;
import edu.wpi.first.wpilibj.command.CommandGroup;
public class SetManual extends CommandGroup {
/**
* Add your docs here.
*/
public SetManual() {
addSequential(new ArmSetMode(ArmControlMode.JOYSTICK_MANUAL));
// e.g. addSequential(new Command1());
// addSequential(new Command2());
// these will run in order.
// To run multiple commands at the same time,
// use addParallel()
// e.g. addParallel(new Command1());
// addSequential(new Command2());
// Command1 and Command2 will run in parallel.
// A command group will require all of the subsystems that each member
// would require.
// e.g. if Command1 requires chassis, and Command2 requires arm,
// a CommandGroup containing them would require both the chassis and the
// arm.
}
}
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java
+30 -12
View File
@@ -84,8 +84,10 @@ public class Arm extends Subsystem implements ControlLoopable
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);
public static final double KF_UP = 0.005;
public static final double KF_DOWN = 0.0;
public static final double KF_UP = 0.06;
public static final double KF_DOWN = 0.01;
public static final double P_Value = 0.6;
public static final double I_Value = .0005;
public static final double PID_ERROR_INCHES = 1.0;
LimitSwitchSource limitSwitchSource;
// Pneumatics
@@ -98,6 +100,8 @@ public class Arm extends Subsystem implements ControlLoopable
private double targetPositionInchesPID = 0;
private boolean firstMpPoint;
private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO;
private double p = 0;
public Arm() {
try {
@@ -117,7 +121,7 @@ public class Arm extends Subsystem implements ControlLoopable
motor2.setNeutralMode(NeutralMode.Brake);
motorControllers.add(motor1);
SmartDashboard.putNumber("arm P value", 0);
}
catch (Exception e) {
@@ -152,6 +156,7 @@ public class Arm extends Subsystem implements ControlLoopable
}
public void setPositionPID(double targetPositionInches) {
mpController.setPIDSlot(PID_SLOT);
updatePositionPID(targetPositionInches);
setArmControlMode(ArmControlMode.JOYSTICK_PID);
@@ -161,8 +166,13 @@ public class Arm extends Subsystem implements ControlLoopable
public void updatePositionPID(double targetPositionInches) {
targetPositionInchesPID = limitPosition(targetPositionInches);
double startPositionInches = motor1.getPositionWorld();
mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN);
System.err.println("it should get here");
//mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN);
motor1.set(ControlMode.Position, targetPositionInches);
motor1.config_kP(0, P_Value, TalonSRXEncoder.TIMEOUT_MS);
motor1.config_kI(0, I_Value, TalonSRXEncoder.TIMEOUT_MS);
motor1.config_kF(0, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN, TalonSRXEncoder.TIMEOUT_MS);
System.err.println(motor1.getControlMode());
//System.err.print(motor1.getClosedLoopError());
}
public void setPositionMP(double targetPositionInches) {
@@ -254,14 +264,17 @@ public class Arm extends Subsystem implements ControlLoopable
// Do the update
if (armControlMode == ArmControlMode.JOYSTICK_MANUAL) {
controlManualWithJoystick();
}
else if (!isFinished) {
if (armControlMode == ArmControlMode.MOTION_PROFILE) {
isFinished = mpController.controlLoopUpdate(getPositionInches());
}
if (armControlMode == ArmControlMode.JOYSTICK_PID){
System.err.println(motor1.getControlMode());
controlPidWithJoystick();
System.err.println("test of pid");
}
@@ -286,6 +299,8 @@ public class Arm extends Subsystem implements ControlLoopable
double deltaPosition = joystickPosition * joystickInchesPerMs;
targetPositionInchesPID = targetPositionInchesPID + deltaPosition;
updatePositionPID(targetPositionInchesPID);
}
private void ControlWithJoystickhold(){
@@ -296,7 +311,7 @@ public class Arm extends Subsystem implements ControlLoopable
private void controlManualWithJoystick() {
double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis();
setSpeedJoystick((joyStickSpeed*.30)/*+.1*/);
setSpeedJoystick((joyStickSpeed*.30)+.1);
}
/*
public void setShiftState(ElevatorSpeedShiftState state) {
@@ -344,15 +359,18 @@ public class Arm extends Subsystem implements ControlLoopable
//System.err.println("the encoder is right after this");
try {
SmartDashboard.putNumber("Elevator Position Inches", motor1.getPositionWorld());
SmartDashboard.putNumber("Elevator Motor 1 Amps", motor1.getOutputCurrent());
SmartDashboard.putNumber("Elevator Motor 2 Amps", motor2.getOutputCurrent());
SmartDashboard.putNumber("Elevator Average Amps", getAverageMotorCurrent());
p = SmartDashboard.getNumber("arm P value", 0);
SmartDashboard.putNumber("Arm Position Ticks", motor1.getPositionWorld());
SmartDashboard.putNumber("Arm Motor 1 Amps", motor1.getOutputCurrent());
SmartDashboard.putNumber("Arm Motor 2 Amps", motor2.getOutputCurrent());
SmartDashboard.putNumber("Arm Average Amps", getAverageMotorCurrent());
SmartDashboard.putNumber("arm pid error", motor1.getClosedLoopError());
SmartDashboard.putNumber("arm motor output", motor1.getMotorOutputPercent());
// SmartDashboard.putNumber("Elevator Motor 1 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_1_CAN_ID));
// SmartDashboard.putNumber("Elevator Motor 2 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_2_CAN_ID));
// SmartDashboard.putNumber("Elevator Motor 3 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID));
SmartDashboard.putNumber("Elevator Target PID Position", targetPositionInchesPID);
SmartDashboard.putNumber("Arm Target PID Position", targetPositionInchesPID);
}
catch (Exception e) {
System.err.println("Drivetrain update status error" +e.getMessage());
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java
+1 -1
View File
@@ -102,7 +102,7 @@ public class Wrist extends Subsystem implements ControlLoopable
public Wrist() {
try {
wristmotor1 = TalonSRXFactory.createTalonEncoder(RobotMap.WRIST_MOTOR_ID, (ENCODER_TICKS_TO_INCHES), false, FeedbackDevice.QuadEncoder);
System.err.println("the tallon shold be made in wrist");
System.err.println("the talon should be made in wrist");
wristmotor1.setInverted(true);
2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java
+1 -2
View File
@@ -218,14 +218,13 @@ public class MPTalonPIDController
}
public void setTarget(double position, double Kf) {
System.err.println("set target" + position);
// Kf gets multipled by position in the Talon
double KfPerPosition = Math.abs(position) > 0.001 ? Kf / position : 0;
for (TalonSRXEncoder motorController : motorControllers) {
motorController.config_kF(0, KfPerPosition, TalonSRXEncoder.TIMEOUT_MS);
motorController.setWorld(ControlMode.Position, position);
//System.err.println(motorController.getControlMode());
}
}