From 22a9cfa9b2cbc107c16f44141a83d48ebc9b0d3c Mon Sep 17 00:00:00 2001 From: lukesta182 <33330639+lukesta182@users.noreply.github.com> Date: Tue, 12 Mar 2019 22:25:21 -0600 Subject: [PATCH] motion maggic works --- .../java/org/usfirst/frc4388/robot/OI.java | 1 + .../frc4388/robot/commands/ArmTest.java | 2 +- .../frc4388/robot/commands/ArmTest2.java | 34 ++++++++ .../usfirst/frc4388/robot/subsystems/Arm.java | 84 +++++++++---------- .../frc4388/robot/subsystems/Drive.java | 10 +-- .../frc4388/robot/subsystems/Wrist.java | 11 +-- 6 files changed, 89 insertions(+), 53 deletions(-) create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest2.java diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java index be7a95d..cec011f 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java @@ -97,6 +97,7 @@ public class OI SmartDashboard.putData("switch to manuel", new SetManual()); SmartDashboard.putData("run arm test", new ArmTest()); + SmartDashboard.putData("run arm2", new ArmTest2()); SmartDashboard.putData("wrist test", new wristTest()); //SmartDashboard.putData("PRE GAME!!!!", new PreGame()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest.java index 0fa8b18..3cb7813 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest.java @@ -14,7 +14,7 @@ public class ArmTest extends CommandGroup { * Add your docs here. */ public ArmTest() { - addSequential(new ArmSetPositionMM(600)); + addSequential(new ArmSetPositionMM(1200)); // Add Commands here: // e.g. addSequential(new Command1()); // addSequential(new Command2()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest2.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest2.java new file mode 100644 index 0000000..5e7c0c9 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest2.java @@ -0,0 +1,34 @@ +/*----------------------------------------------------------------------------*/ +/* 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 edu.wpi.first.wpilibj.command.CommandGroup; + +public class ArmTest2 extends CommandGroup { + /** + * Add your docs here. + */ + public ArmTest2() { + addSequential(new ArmSetPositionMM(600)); + // 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. + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java index 9fa6496..81884da 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java @@ -12,6 +12,7 @@ import org.usfirst.frc4388.utility.TalonSRXEncoder; import org.usfirst.frc4388.utility.TalonSRXFactory; import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.DemandType; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.LimitSwitchSource; @@ -81,18 +82,21 @@ public class Arm extends Subsystem implements ControlLoopable private MPTalonPIDController mpController; public static int PID_SLOT = 0; - public static int MP_SLOT = 1; + public static int MM_SLOT = 1; + public static int MP_SLOT = 2; 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;//0.01; + public static final double KF_UP = 1;//0.01; public static final double KF_DOWN = 0;// 0.0; - public static final double P_Value = 0;// 2; - public static final double I_Value = 0;// 0.00030; + public static final double P_Value = 0.5;// 2; + public static final double I_Value = 0.0001;// 0.00030; public static final double D_Value = 0;// 200; + public static final double F_Value = 0.75; // 1023 / 1360 max speed (ticks/100ms) + public static final double maxGravityComp = 0.08; public static final double RampRate = 0;// 0.0; - public static final int A_value = 25; - public static final int CV_value = 25; + public static final int A_value = 100; + public static final int CV_value = 340; @@ -128,6 +132,17 @@ public class Arm extends Subsystem implements ControlLoopable // Driver.reportError("Could not detect elevator motor 1 encoder encoder!", false); // } + motor1.configNominalOutputForward(0, TalonSRXEncoder.TIMEOUT_MS); + motor1.configNominalOutputReverse(0, TalonSRXEncoder.TIMEOUT_MS); + motor1.configPeakOutputForward(1, TalonSRXEncoder.TIMEOUT_MS); + motor1.configPeakOutputReverse(-1, TalonSRXEncoder.TIMEOUT_MS); + + motor1.selectProfileSlot(MM_SLOT, 0); + motor1.config_kF(MM_SLOT, F_Value, TalonSRXEncoder.TIMEOUT_MS); + motor1.config_kP(MM_SLOT, P_Value, TalonSRXEncoder.TIMEOUT_MS); + motor1.config_kI(MM_SLOT, I_Value, TalonSRXEncoder.TIMEOUT_MS); + motor1.config_kD(MM_SLOT, D_Value, TalonSRXEncoder.TIMEOUT_MS); + motor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); motor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); motor1.setNeutralMode(NeutralMode.Brake); @@ -174,50 +189,33 @@ public class Arm extends Subsystem implements ControlLoopable public void setPositionMM(double targetPositionInches){ motor1.set(ControlMode.MotionMagic, targetPositionInches); System.err.println(motor1.getControlMode()); + motor1.selectProfileSlot(MM_SLOT, 0); setArmControlMode(ArmControlMode.MOTION_MAGIC); updatePositionMM(targetPositionInches); setFinished(false); - System.err.println("should get here"); - System.err.println("should get here"); - System.err.println("should get here"); - System.err.println("should get here"); - System.err.println("should get here"); - System.err.println("should get here"); - System.err.println("should get here"); - System.err.println("should get here"); - System.err.println("should get here"); - System.err.println("should get here"); - System.err.println("should get here"); - System.err.println("should get here"); - System.err.println("should get here"); - System.err.println("should get here"); - System.err.println("should get here"); - System.err.println("should get here"); - System.err.println("should get here"); - System.err.println("should get here"); + + } + + public double calcGravityCompensationAtCurrentPosition() { + int ticks = motor1.getSelectedSensorPosition(); + double degreesFromDown = (ticks+920)*(360.0/(4096*3)); + double compensation = maxGravityComp * Math.sin(degreesFromDown*Math.PI/180); + System.err.println("comp(" + degreesFromDown + "^) = " + compensation); + return compensation; } public void updatePositionMM(double targetPositionInches){ targetPositionInchesMM = limitPosition(targetPositionInches); - double startPositionInches = motor1.getPositionWorld(); - motor1.set(ControlMode.MotionMagic, targetPositionInches); - motor1.configNominalOutputForward(0, TalonSRXEncoder.TIMEOUT_MS); - motor1.configNominalOutputReverse(0, TalonSRXEncoder.TIMEOUT_MS); - motor1.configPeakOutputForward(1, TalonSRXEncoder.TIMEOUT_MS); - motor1.configPeakOutputReverse(-1, TalonSRXEncoder.TIMEOUT_MS); - - - motor1.selectProfileSlot(1, 0); - motor1.config_kF(1, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN, TalonSRXEncoder.TIMEOUT_MS); - motor1.config_kP(1, P_Value, TalonSRXEncoder.TIMEOUT_MS); - motor1.config_kI(1, I_Value, TalonSRXEncoder.TIMEOUT_MS); - motor1.config_kD(1, D_Value, TalonSRXEncoder.TIMEOUT_MS); - - + //double startPositionInches = motor1.getPositionWorld(); + double compensation = calcGravityCompensationAtCurrentPosition(); + //System.err.println("compensation = " + compensation); + // motor1.set(ControlMode.MotionMagic, targetPositionInches); + motor1.set(ControlMode.MotionMagic, targetPositionInches, DemandType.ArbitraryFeedForward, compensation); + //System.err.println(motor1.getControlMode()); motor1.configMotionCruiseVelocity(CV_value, TalonSRXEncoder.TIMEOUT_MS); motor1.configMotionAcceleration(A_value, TalonSRXEncoder.TIMEOUT_MS); - + } @@ -306,7 +304,7 @@ public class Arm extends Subsystem implements ControlLoopable } if (armControlMode == ArmControlMode.MOTION_MAGIC){ controlMMWithJoystick(); - System.err.println(motor1.getControlMode()); + //System.err.println(motor1.getControlMode()); } /*else if (armControlMode == ArmControlMode.MP_PATH_VELOCITY) { @@ -348,7 +346,7 @@ public class Arm extends Subsystem implements ControlLoopable private void controlManualWithJoystick() { double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis(); - setSpeedJoystick((joyStickSpeed*.30)+.1); + setSpeedJoystick((joyStickSpeed*.3)+.08); } /* public void setShiftState(ElevatorSpeedShiftState state) { @@ -399,10 +397,12 @@ public class Arm extends Subsystem implements ControlLoopable 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("sensor vel", motor1.getSelectedSensorVelocity()); SmartDashboard.putNumber("Arm Average Amps", getAverageMotorCurrent()); SmartDashboard.putNumber("arm pid error", motor1.getClosedLoopError()); SmartDashboard.putNumber("arm motor output", motor1.getMotorOutputPercent()); SmartDashboard.putNumber("Arm Target MM Position", targetPositionInchesMM); + //SmartDashboard.putNumber("arm 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)); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java index 5fdc9b4..2e07a53 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java @@ -879,7 +879,7 @@ public class Drive extends Subsystem implements ControlLoopable //SmartDashboard.putNumber("Left Pos Inches", encoderLeft.getPosition());//leftDrive1.getPositionWorld()); //SmartDashboard.putNumber("Right Vel Ft-Sec", 0);//rightDrive1.getVelocityWorld() / 12); //SmartDashboard.putNumber("Left Vel Ft-Sec", 0);//leftDrive1.getVelocityWorld() / 12); - SmartDashboard.putBoolean("Drive Hold", controlMode == DriveControlMode.HOLD); + //SmartDashboard.putBoolean("Drive Hold", controlMode == DriveControlMode.HOLD); //SmartDashboard.putNumber("Yaw Angle Pigeon Deg", getGyroPigeonAngleDeg()); SmartDashboard.putNumber("Yaw Angle Deg", Math.abs(getGyroAngleDeg() % 360)); //MotionProfilePoint mpPoint = mpTurnController.getCurrentPoint(); @@ -887,12 +887,12 @@ public class Drive extends Subsystem implements ControlLoopable //SmartDashboard.putNumber("Gyro Delta", delta); //SmartDashboard.putBoolean("Gyro Calibrating", isCalibrating); SmartDashboard.putBoolean("Gyro Calibrating", gyroNavX.isCalibrating()); - SmartDashboard.putNumber("Gyro Offset", gyroOffsetDeg); - SmartDashboard.putNumber("Delta PID Angle", targetPIDAngle - getGyroAngleDeg()); + // SmartDashboard.putNumber("Gyro Offset", gyroOffsetDeg); + //SmartDashboard.putNumber("Delta PID Angle", targetPIDAngle - getGyroAngleDeg()); SmartDashboard.putNumber("Steer Output", m_steerOutput); SmartDashboard.putNumber("Move Output", m_moveOutput); - SmartDashboard.putNumber("Steer Input", m_steerInput); - SmartDashboard.putNumber("Move Input", m_moveInput); + //SmartDashboard.putNumber("Steer Input", m_steerInput); + //SmartDashboard.putNumber("Move Input", m_moveInput); SmartDashboard.putString("MODE", "TEST"); //if (left_distance.getPosition() != 0 && right_distance.getPosition() != 0){ // SmartDashboard.putNumber("Distance Inches", (left_distance.getPosition()-right_distance.getPosition())/2);//rightDrive1.getPositionWorld()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java index e405ce0..4a9c63a 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java @@ -85,12 +85,12 @@ public class Wrist extends Subsystem implements ControlLoopable private PIDParams pidPIDParamsHiGear = new PIDParams(0.075, 0.0, 0.0, 0.0, 0.0, 0.0); public static final double KF_UP = 0.01; public static final double KF_DOWN = 0.0; - public static final double P_Value = .5; - public static final double I_Value = 0.00300; - public static final double D_Value = 200; + public static final double P_Value = 2.3; + public static final double I_Value = 0.00004; + public static final double D_Value = 96; public static final double RampRate = 0.0; private PIDParams wristPIDParams = new PIDParams(P_Value, I_Value, D_Value, KF_DOWN); // KF gets updated later - public static final double PID_ERROR_INCHES = 5.0; + public static final double PID_ERROR_INCHES = 150; LimitSwitchSource limitSwitchSource; // Pneumatics private Solenoid speedShift; @@ -140,6 +140,7 @@ public class Wrist extends Subsystem implements ControlLoopable wristMotor1.setPosition(0); } + private synchronized void setWristControlMode(WristControlMode controlMode) { this.wristControlMode = controlMode; } @@ -316,7 +317,7 @@ public class Wrist extends Subsystem implements ControlLoopable private void controlManualWithJoystick() { double joyStickSpeed = -Robot.oi.getOperatorController().getRightYAxis(); - setSpeedJoystick(joyStickSpeed); + setSpeedJoystick(joyStickSpeed*.5); } /* public void setShiftState(ElevatorSpeedShiftState state) {