mirror of
https://github.com/Team4388/2019-Hit-or-Miss.git
synced 2026-06-09 00:28:05 -06:00
OPERATIONAL FIXES WITH UNTESTED WRIST MM CODE
This commit is contained in:
lukesta182
@@ -7,7 +7,7 @@ import buttons.XBoxTriggerButton;
|
||||
import org.usfirst.frc4388.controller.IHandController;
|
||||
import org.usfirst.frc4388.controller.XboxController;
|
||||
import org.usfirst.frc4388.robot.commands.*;
|
||||
import org.usfirst.frc4388.robot.commands.presets.SetPositionArmWrist;
|
||||
import org.usfirst.frc4388.robot.commands.presets.StowArm;
|
||||
import org.usfirst.frc4388.robot.constants.LEDPatterns;
|
||||
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
@@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj.Joystick;
|
||||
import edu.wpi.first.wpilibj.buttons.*;
|
||||
import org.usfirst.frc4388.robot.subsystems.*;
|
||||
import org.usfirst.frc4388.robot.subsystems.Arm.ArmControlMode;
|
||||
import org.usfirst.frc4388.robot.subsystems.Arm.PlaceMode;
|
||||
import org.usfirst.frc4388.robot.subsystems.Wrist.WristControlMode;
|
||||
import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
|
||||
import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
|
||||
@@ -68,6 +69,10 @@ public class OI
|
||||
|
||||
JoystickButton liftBothIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON);
|
||||
liftBothIntake.whenPressed(new HatchAndBallUp());
|
||||
|
||||
JoystickButton cargoPlaceMode = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON);
|
||||
cargoPlaceMode.whenPressed(new SwitchArmPlaceMode(PlaceMode.CARGO));
|
||||
cargoPlaceMode.whenReleased(new SwitchArmPlaceMode(PlaceMode.HATCH));
|
||||
|
||||
JoystickButton liftHatchIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON);
|
||||
liftHatchIntake.whenPressed(new LiftHatchDropBall());
|
||||
@@ -87,9 +92,9 @@ public class OI
|
||||
JoystickButton lowHeight = new JoystickButton(m_operatorXbox.getJoyStick(),XboxController.LEFT_JOYSTICK_BUTTON);
|
||||
lowHeight.whenPressed(new GrabFromLoadingSatation());
|
||||
|
||||
JoystickButton stow = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON);
|
||||
stow.whenPressed(new StowArm());
|
||||
stow.whenPressed(new setLEDPattern(LEDPatterns.SOLID_GREEN));
|
||||
//JoystickButton stow = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON);
|
||||
//stow.whenPressed(new StowArm());
|
||||
//stow.whenPressed(new setLEDPattern(LEDPatterns.SOLID_GREEN));
|
||||
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
@@ -122,8 +127,10 @@ public class OI
|
||||
|
||||
|
||||
SmartDashboard.putData("switch to manuel", new SetManual());
|
||||
SmartDashboard.putData("grab from station", new GrabFromLoadingSatation());
|
||||
SmartDashboard.putData("wrist test", new wristTest());
|
||||
SmartDashboard.putData("WristTest", new wristTest());
|
||||
SmartDashboard.putData("arm test", new ArmTest());
|
||||
//SmartDashboard.putData("arm test2", new ArmTest2());
|
||||
|
||||
|
||||
|
||||
} catch (Exception e) {
|
||||
|
||||
@@ -49,8 +49,8 @@ public class Robot extends TimedRobot
|
||||
//Printing game data to riolog
|
||||
String gameData = DriverStation.getInstance().getGameSpecificMessage();
|
||||
System.out.println(gameData);
|
||||
CameraServer.getInstance().startAutomaticCapture();
|
||||
CameraServer.getInstance().putVideo("res", 300, 220);
|
||||
//CameraServer.getInstance().startAutomaticCapture();
|
||||
//CameraServer.getInstance().putVideo("res", 300, 220);
|
||||
|
||||
try {
|
||||
oi = OI.getInstance();
|
||||
@@ -89,38 +89,30 @@ public class Robot extends TimedRobot
|
||||
|
||||
public void autonomousInit() {
|
||||
updateChoosers();
|
||||
|
||||
controlLoop.start();
|
||||
drive.endGyroCalibration();
|
||||
drive.resetEncoders();
|
||||
drive.resetGyro();
|
||||
drive.setIsRed(getAlliance().equals(Alliance.Red));
|
||||
arm.resetEncoder();
|
||||
|
||||
drive.endGyroCalibration();
|
||||
arm.targetPositionInchesMM = arm.motor1.getPositionWorld();
|
||||
wrist.targetPositionInchesMM = wrist.wristMotor1.getPositionWorld();
|
||||
//drive.resetEncoders();
|
||||
//drive.resetGyro();
|
||||
//drive.setIsRed(getAlliance().equals(Alliance.Red));
|
||||
//arm.resetEncoder();
|
||||
updateStatus();
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
public void autonomousPeriodic() {
|
||||
Scheduler.getInstance().run();
|
||||
Scheduler.getInstance().run();
|
||||
updateStatus();
|
||||
}
|
||||
|
||||
public void teleopInit() {
|
||||
System.err.println("Beginning of teleopInit.");
|
||||
|
||||
drive.setToBrakeOnNeutral(false);
|
||||
// drive.setToBrakeOnNeutral(false);
|
||||
updateChoosers();
|
||||
System.err.println("TeleopInit after UpdateChoosers");
|
||||
controlLoop.start();
|
||||
System.err.println("TeleopInit after controlLoop");
|
||||
//drive.resetEncoders();
|
||||
//System.err.println("TeleopInit after resetEncoders");
|
||||
drive.endGyroCalibration();
|
||||
System.err.println("TeleopInit after endGyroCalibrations");
|
||||
arm.targetPositionInchesMM = arm.motor1.getPositionWorld();
|
||||
wrist.targetPositionInchesPID = wrist.wristMotor1.getPositionWorld();
|
||||
|
||||
updateStatus();
|
||||
}
|
||||
|
||||
|
||||
@@ -14,7 +14,7 @@ public class ArmTest extends CommandGroup {
|
||||
* Add your docs here.
|
||||
*/
|
||||
public ArmTest() {
|
||||
addSequential(new ArmSetPositionMM(1200));
|
||||
addSequential(new ArmSetPositionMM(3600));
|
||||
// Add Commands here:
|
||||
// e.g. addSequential(new Command1());
|
||||
// addSequential(new Command2());
|
||||
|
||||
@@ -80,9 +80,9 @@ public class DriveStraightBasic extends Command {
|
||||
}
|
||||
if (!finished) {
|
||||
m_speed = velocityToMoveSpeed(velocity, m_goingBackwards);
|
||||
SmartDashboard.putNumber("DSB Dist", position);
|
||||
//SmartDashboard.putNumber("DSB Dist", position);
|
||||
} else {
|
||||
SmartDashboard.putNumber("DSB finDist", position);
|
||||
//SmartDashboard.putNumber("DSB finDist", position);
|
||||
}
|
||||
return finished;
|
||||
}
|
||||
@@ -90,7 +90,7 @@ public class DriveStraightBasic extends Command {
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
double currentTimestamp = Timer.getFPGATimestamp();
|
||||
SmartDashboard.putNumber("DSB Runtime", currentTimestamp - m_commandInitTimestamp);
|
||||
//SmartDashboard.putNumber("DSB Runtime", currentTimestamp - m_commandInitTimestamp);
|
||||
Robot.drive.rawMoveSteer(0.0, 0.0);
|
||||
Robot.drive.setControlMode(DriveControlMode.JOYSTICK);
|
||||
}
|
||||
|
||||
+2
-2
@@ -92,9 +92,9 @@ public class DriveStraightBasicAbs extends Command {
|
||||
}
|
||||
if (!finished) {
|
||||
m_speed = velocityToMoveSpeed(velocity, m_goingBackwards);
|
||||
SmartDashboard.putNumber("DSB Dist", position);
|
||||
//SmartDashboard.putNumber("DSB Dist", position);
|
||||
} else {
|
||||
SmartDashboard.putNumber("DSB finDist", position);
|
||||
//SmartDashboard.putNumber("DSB finDist", position);
|
||||
}
|
||||
return finished;
|
||||
}
|
||||
|
||||
@@ -24,7 +24,11 @@ public class WristSetMode extends Command {
|
||||
}
|
||||
else if (controlMode == WristControlMode.JOYSTICK_MANUAL) {
|
||||
Robot.wrist.setSpeedJoystick(0);
|
||||
}
|
||||
}
|
||||
else if (controlMode == WristControlMode.MOTION_MAGIC){
|
||||
Robot.wrist.setPositionMM(Robot.wrist.getPositionInches());
|
||||
}
|
||||
|
||||
else {
|
||||
Robot.wrist.setSpeed(0.0);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,57 @@
|
||||
package org.usfirst.frc4388.robot.commands;
|
||||
|
||||
import org.usfirst.frc4388.robot.Robot;
|
||||
|
||||
import edu.wpi.first.wpilibj.command.Command;
|
||||
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class WristSetPositionMM extends Command {
|
||||
|
||||
private double targetPositionInches;
|
||||
private boolean isAtTarget;
|
||||
private static final double MIN_DELTA_TARGET = 20;
|
||||
|
||||
public WristSetPositionMM(double targetPositionInches) {
|
||||
this.targetPositionInches = targetPositionInches;
|
||||
requires(Robot.wrist);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize() {
|
||||
|
||||
if (Math.abs(targetPositionInches - Robot.wrist.getPositionInches()) < MIN_DELTA_TARGET) {
|
||||
isAtTarget = true;
|
||||
System.out.println("Wrist is at target");
|
||||
}
|
||||
else {
|
||||
isAtTarget = false;
|
||||
Robot.wrist.setPositionMM(targetPositionInches);
|
||||
}
|
||||
// System.out.println("Arm set MP initialized, target = " + targetPositionInches);
|
||||
}
|
||||
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
protected void execute() {
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
protected boolean isFinished() {
|
||||
return isAtTarget || Robot.wrist.isFinished();
|
||||
}
|
||||
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
Robot.wrist.setPositionMM(Robot.wrist.getPositionInches());
|
||||
// System.out.println("Arm set MP end");
|
||||
}
|
||||
|
||||
// Called when another command which requires one or more of the same
|
||||
// subsystems is scheduled to run
|
||||
protected void interrupted() {
|
||||
// System.out.println("ArmSetPositionMP interrupted");
|
||||
Robot.wrist.setFinished(true);
|
||||
Robot.wrist.setPositionMM(Robot.wrist.getPositionInches());
|
||||
}
|
||||
}
|
||||
@@ -8,6 +8,7 @@
|
||||
package org.usfirst.frc4388.robot.commands.presets;
|
||||
|
||||
import org.usfirst.frc4388.robot.commands.ArmSetPositionMM;
|
||||
import org.usfirst.frc4388.robot.commands.HatchFlip;
|
||||
import org.usfirst.frc4388.robot.commands.WristSetPositionPID;
|
||||
|
||||
import edu.wpi.first.wpilibj.command.CommandGroup;
|
||||
@@ -17,9 +18,10 @@ public class CargoHigh extends CommandGroup {
|
||||
* Add your docs here.
|
||||
*/
|
||||
public CargoHigh() {
|
||||
|
||||
addParallel(new WristSetPositionPID(3243));
|
||||
addSequential(new ArmSetPositionMM(4298));
|
||||
addSequential(new HatchFlip(false));
|
||||
addParallel(new setWrist(2781));
|
||||
addParallel(new DelayHatch());
|
||||
addSequential(new ArmSetPositionMM(3812));
|
||||
|
||||
// Add Commands here:
|
||||
// e.g. addSequential(new Command1());
|
||||
|
||||
@@ -18,8 +18,8 @@ public class CargoLow extends CommandGroup {
|
||||
*/
|
||||
public CargoLow() {
|
||||
|
||||
addParallel(new WristSetPositionPID(2500));
|
||||
addSequential(new ArmSetPositionMM(1388));
|
||||
addParallel(new setWrist(2300));
|
||||
addSequential(new ArmSetPositionMM(1300));
|
||||
|
||||
// Add Commands here:
|
||||
// e.g. addSequential(new Command1());
|
||||
|
||||
@@ -18,8 +18,8 @@ public class CargoMid extends CommandGroup {
|
||||
*/
|
||||
public CargoMid() {
|
||||
|
||||
addParallel(new WristSetPositionPID(2830));
|
||||
addSequential(new ArmSetPositionMM(2830));
|
||||
addParallel(new setWrist(2430));
|
||||
addSequential(new ArmSetPositionMM(2630));
|
||||
|
||||
// Add Commands here:
|
||||
// e.g. addSequential(new Command1());
|
||||
|
||||
@@ -0,0 +1,39 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.presets;
|
||||
|
||||
import org.usfirst.frc4388.robot.commands.HatchFlip;
|
||||
|
||||
import edu.wpi.first.wpilibj.command.CommandGroup;
|
||||
import edu.wpi.first.wpilibj.command.WaitCommand;
|
||||
|
||||
public class DelayHatch extends CommandGroup {
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public DelayHatch() {
|
||||
addSequential(new WaitCommand(1));
|
||||
addSequential(new HatchFlip (true));
|
||||
// Add Commands here:
|
||||
// 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.
|
||||
}
|
||||
}
|
||||
@@ -8,6 +8,7 @@
|
||||
package org.usfirst.frc4388.robot.commands.presets;
|
||||
|
||||
import org.usfirst.frc4388.robot.commands.ArmSetPositionMM;
|
||||
import org.usfirst.frc4388.robot.commands.HatchFlip;
|
||||
import org.usfirst.frc4388.robot.commands.WristSetPositionPID;
|
||||
|
||||
import edu.wpi.first.wpilibj.command.CommandGroup;
|
||||
@@ -17,8 +18,9 @@ public class HatchHigh extends CommandGroup {
|
||||
* Add your docs here.
|
||||
*/
|
||||
public HatchHigh() {
|
||||
|
||||
addParallel(new WristSetPositionPID(1144));
|
||||
addSequential(new HatchFlip(false));
|
||||
addParallel(new setWrist(1144));
|
||||
addParallel(new DelayHatch());
|
||||
addSequential(new ArmSetPositionMM(3605));
|
||||
|
||||
// Add Commands here:
|
||||
|
||||
@@ -18,8 +18,8 @@ public class HatchLow extends CommandGroup {
|
||||
*/
|
||||
public HatchLow() {
|
||||
|
||||
addParallel(new WristSetPositionPID(450));
|
||||
addSequential(new ArmSetPositionMM(590));
|
||||
addParallel(new setWrist(350));
|
||||
addSequential(new ArmSetPositionMM(550));
|
||||
|
||||
// Add Commands here:
|
||||
// e.g. addSequential(new Command1());
|
||||
|
||||
@@ -18,7 +18,7 @@ public class HatchMid extends CommandGroup {
|
||||
*/
|
||||
public HatchMid() {
|
||||
|
||||
addParallel(new WristSetPositionPID(750));
|
||||
addParallel(new setWrist(750));
|
||||
addSequential(new ArmSetPositionMM(2000));
|
||||
|
||||
// Add Commands here:
|
||||
|
||||
@@ -11,6 +11,8 @@ import org.usfirst.frc4388.robot.commands.ArmSetPositionMM;
|
||||
import org.usfirst.frc4388.robot.commands.HatchFlip;
|
||||
import org.usfirst.frc4388.robot.commands.WristPlacement;
|
||||
import org.usfirst.frc4388.robot.commands.WristSetPositionPID;
|
||||
import org.usfirst.frc4388.robot.commands.setLEDPattern;
|
||||
import org.usfirst.frc4388.robot.constants.LEDPatterns;
|
||||
|
||||
import edu.wpi.first.wpilibj.command.CommandGroup;
|
||||
|
||||
@@ -22,6 +24,7 @@ public class StowArm extends CommandGroup {
|
||||
public StowArm() {
|
||||
addSequential(new HatchFlip(false));
|
||||
addParallel(new WristPlacement(true));
|
||||
addParallel(new setLEDPattern(LEDPatterns.SOLID_GREEN));
|
||||
addParallel(new WristSetPositionPID(110), 2);
|
||||
addSequential(new ArmSetPositionMM(10));
|
||||
// Add Commands here:
|
||||
|
||||
@@ -0,0 +1,38 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.presets;
|
||||
|
||||
import org.usfirst.frc4388.robot.commands.WristSetPositionPID;
|
||||
|
||||
import edu.wpi.first.wpilibj.command.CommandGroup;
|
||||
import edu.wpi.first.wpilibj.command.WaitCommand;
|
||||
|
||||
public class setWrist extends CommandGroup {
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public setWrist(double wrist) {
|
||||
addSequential(new WaitCommand(1));
|
||||
addSequential(new WristSetPositionPID(wrist));
|
||||
// Add Commands here:
|
||||
// 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.
|
||||
}
|
||||
}
|
||||
@@ -14,7 +14,7 @@ public class wristTest extends CommandGroup {
|
||||
* Add your docs here.
|
||||
*/
|
||||
public wristTest() {
|
||||
addSequential(new WristSetPositionPID(500));
|
||||
addSequential(new WristSetPositionMM(1000));
|
||||
// Add Commands here:
|
||||
// e.g. addSequential(new Command1());
|
||||
// addSequential(new Command2());
|
||||
|
||||
@@ -64,18 +64,9 @@ public class Arm extends Subsystem implements ControlLoopable
|
||||
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 = -25;
|
||||
public static final double MAX_POSITION_INCHES = 4400;
|
||||
public static final double MAX_POSITION_INCHES = 4200;
|
||||
public static final double AFTER_INTAKE_POSITION_INCHES = 4.0;
|
||||
|
||||
public static final double SWITCH_POSITION_INCHES = 24.0;
|
||||
public static final double SWITCH_POSITION_HIGH_INCHES = 36.0; //Switch Position for First Cube APR
|
||||
public static final double SCALE_LOW_POSITION_INCHES = 56.0;
|
||||
public static final double SCALE_FIRST_CUBE_POSITION_INCHES = 78.0; //72.0
|
||||
public static final double SCALE_SECOND_CUBE_POSITION_INCHES = 77.0;
|
||||
public static final double SCALE_HIGH_POSITION_INCHES = MAX_POSITION_INCHES;
|
||||
public static final double CLIMB_BAR_POSITION_INCHES = 70.0;
|
||||
public static final double CLIMB_HIGH_POSITION_INCHES = 10.0;
|
||||
public static final double CLIMB_ASSIST_POSITION_INCHES = 50.0;
|
||||
|
||||
// Motion profile max velocities and accel times
|
||||
public static final double MP_MAX_VELOCITY_INCHES_PER_SEC = 60;
|
||||
@@ -99,20 +90,20 @@ public class Arm 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 = 1;//0.01;
|
||||
public static final double KF_DOWN = 0;// 0.0;
|
||||
public static final double P_Value = 0.5;// 2;
|
||||
public static final double I_Value = 0.0008;// 0.00030;
|
||||
public static final double D_Value = 100;// 200;
|
||||
public static final double P_Value = 4;// 2;
|
||||
public static final double I_Value = 0.0001;// 0.00030;
|
||||
public static final double D_Value = 200;// 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 maxGravityComp = 0.01;
|
||||
public static final double RampRate = 0;// 0.0;
|
||||
public static final int A_value = 450;
|
||||
public static final int CV_value = 740;
|
||||
public static final int A_value = 400;
|
||||
public static final int CV_value = 500;
|
||||
|
||||
|
||||
|
||||
|
||||
private PIDParams armPIDParams = new PIDParams(P_Value, I_Value, D_Value, KF_DOWN); // KF gets updated later
|
||||
public static final double PID_ERROR_INCHES = 5;
|
||||
public static final double PID_ERROR_INCHES = 50;
|
||||
LimitSwitchSource limitSwitchSource;
|
||||
|
||||
// Pneumatics
|
||||
@@ -149,7 +140,7 @@ public class Arm extends Subsystem implements ControlLoopable
|
||||
motor1.configPeakOutputReverse(-1, TalonSRXEncoder.TIMEOUT_MS);
|
||||
|
||||
motor1.selectProfileSlot(MM_SLOT, 0);
|
||||
motor1.config_kF(MM_SLOT, F_Value, TalonSRXEncoder.TIMEOUT_MS);
|
||||
//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);
|
||||
@@ -179,8 +170,8 @@ public class Arm extends Subsystem implements ControlLoopable
|
||||
|
||||
public void resetEncoder(){
|
||||
motor1.setPosition(0);
|
||||
targetPositionInchesMM = 0;
|
||||
targetPositionInchesPID = 0;
|
||||
//targetPositionInchesMM = 0;
|
||||
//targetPositionInchesPID = 0;
|
||||
}
|
||||
|
||||
private synchronized void setArmControlMode(ArmControlMode controlMode) {
|
||||
@@ -266,9 +257,9 @@ public class Arm extends Subsystem implements ControlLoopable
|
||||
}
|
||||
|
||||
private double limitPosition(double targetPosition) {
|
||||
/*if (targetPosition < MIN_POSITION_INCHES) {
|
||||
if (targetPosition < MIN_POSITION_INCHES) {
|
||||
return MIN_POSITION_INCHES;
|
||||
}*/
|
||||
}
|
||||
if (targetPosition > MAX_POSITION_INCHES) {
|
||||
return MAX_POSITION_INCHES;
|
||||
}
|
||||
@@ -334,7 +325,7 @@ public class Arm extends Subsystem implements ControlLoopable
|
||||
// Do the update
|
||||
if (armControlMode == ArmControlMode.JOYSTICK_MANUAL) {
|
||||
controlManualWithJoystick();
|
||||
System.err.println(motor1.getControlMode());
|
||||
//System.err.println(motor1.getControlMode());
|
||||
}
|
||||
else if (!isFinished) {
|
||||
if (armControlMode == ArmControlMode.MOTION_PROFILE) {
|
||||
@@ -343,11 +334,11 @@ public class Arm extends Subsystem implements ControlLoopable
|
||||
}
|
||||
if (armControlMode == ArmControlMode.JOYSTICK_PID){
|
||||
controlPidWithJoystick();
|
||||
System.err.println(motor1.getControlMode());
|
||||
//System.err.println(motor1.getControlMode());
|
||||
}
|
||||
if (armControlMode == ArmControlMode.MOTION_MAGIC){
|
||||
controlMMWithJoystick();
|
||||
System.err.println(motor1.getControlMode());
|
||||
//System.err.println(motor1.getControlMode());
|
||||
}
|
||||
|
||||
/*else if (armControlMode == ArmControlMode.MP_PATH_VELOCITY) {
|
||||
@@ -378,7 +369,7 @@ public class Arm extends Subsystem implements ControlLoopable
|
||||
targetPositionInchesMM = targetPositionInchesMM + deltaPosition;
|
||||
updatePositionMM(targetPositionInchesMM);
|
||||
//Robot.wrist.targetPositionInchesPID = targetPositionInchesPID - (deltaPosition/3);
|
||||
Robot.wrist.updatePositionPID(Robot.wrist.targetPositionInchesPID);
|
||||
//Robot.wrist.updatePositionPID(Robot.wrist.targetPositionInchesPID);
|
||||
|
||||
|
||||
}
|
||||
@@ -391,7 +382,7 @@ public class Arm extends Subsystem implements ControlLoopable
|
||||
|
||||
private void controlManualWithJoystick() {
|
||||
double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis();
|
||||
setSpeedJoystick((joyStickSpeed*.3)+.08);
|
||||
setSpeedJoystick(joyStickSpeed);
|
||||
}
|
||||
/*
|
||||
public void setShiftState(ElevatorSpeedShiftState state) {
|
||||
@@ -439,19 +430,19 @@ public class Arm extends Subsystem implements ControlLoopable
|
||||
//System.err.println("the encoder is right after this");
|
||||
try {
|
||||
|
||||
SmartDashboard.putNumber("Arm Position Ticks", motor1.getPositionWorld());
|
||||
SmartDashboard.putNumber("Arm 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("sensor vel", motor1.getSelectedSensorVelocity());
|
||||
//SmartDashboard.putNumber("Arm Average Amps", getAverageMotorCurrent());
|
||||
SmartDashboard.putNumber("Arm Error", motor1.getClosedLoopError());
|
||||
SmartDashboard.putNumber("Arm Amps", getAverageMotorCurrent());
|
||||
SmartDashboard.putNumber("Arm Target MM", 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));
|
||||
SmartDashboard.putNumber("Arm Target PID Position", targetPositionInchesPID);
|
||||
//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("Arm Target PID Position", targetPositionInchesPID);
|
||||
}
|
||||
catch (Exception e) {
|
||||
System.err.println("Arm update status error" +e.getMessage());
|
||||
|
||||
@@ -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;
|
||||
@@ -30,7 +31,7 @@ public class Wrist extends Subsystem implements ControlLoopable
|
||||
{
|
||||
private static Wrist instance;
|
||||
|
||||
public static enum WristControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL };
|
||||
public static enum WristControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL, MOTION_MAGIC };
|
||||
|
||||
// One revolution of the 30T Drive 1.880" PD pulley = Pi * PD inches = 36/24 revs due to pulleys * 34/24 revs due to gears * 36/12 revs due mag encoder gear on ball shifter * 4096 ticks
|
||||
public static final double ENCODER_TICKS_TO_INCHES = (1);
|
||||
@@ -79,16 +80,21 @@ public class Wrist 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.01;
|
||||
public static final double KF_DOWN = 0.0;
|
||||
public static final double P_Value = 1;
|
||||
public static final double I_Value = 0.0001;
|
||||
public static final double D_Value = 100;
|
||||
public static final double P_Value = 4;
|
||||
public static final double I_Value = 0.0000;
|
||||
public static final double D_Value = 000;
|
||||
public static final double F_Value = 1;
|
||||
public static final int CV_value = 100;
|
||||
public static final int A_value = 100;
|
||||
public static final double RampRate = 0.0;
|
||||
public static final double maxGravityComp = 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 = 150;
|
||||
LimitSwitchSource limitSwitchSource;
|
||||
@@ -98,8 +104,9 @@ public class Wrist extends Subsystem implements ControlLoopable
|
||||
// Misc
|
||||
public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0;
|
||||
private boolean isFinished;
|
||||
private WristControlMode wristControlMode = WristControlMode.JOYSTICK_PID;
|
||||
private WristControlMode wristControlMode = WristControlMode.MOTION_MAGIC;
|
||||
public double targetPositionInchesPID = 0;
|
||||
public double targetPositionInchesMM = 0;
|
||||
private boolean firstMpPoint;
|
||||
private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO;
|
||||
private double p = 0;
|
||||
@@ -120,9 +127,18 @@ public class Wrist extends Subsystem implements ControlLoopable
|
||||
wristMotor1.setNeutralMode(NeutralMode.Brake);
|
||||
wristMotor1.enableCurrentLimit(true);
|
||||
//wristMotor1.setSensorPhase(true);
|
||||
|
||||
wristMotor1.configNominalOutputForward(0, TalonSRXEncoder.TIMEOUT_MS);
|
||||
wristMotor1.configNominalOutputReverse(0, TalonSRXEncoder.TIMEOUT_MS);
|
||||
wristMotor1.configPeakOutputForward(1, TalonSRXEncoder.TIMEOUT_MS);
|
||||
wristMotor1.configPeakOutputReverse(-1, TalonSRXEncoder.TIMEOUT_MS);
|
||||
|
||||
wristMotor1.selectProfileSlot(MM_SLOT, 0);
|
||||
wristMotor1.config_kF(MM_SLOT, F_Value, TalonSRXEncoder.TIMEOUT_MS);
|
||||
wristMotor1.config_kP(MM_SLOT, P_Value, TalonSRXEncoder.TIMEOUT_MS);
|
||||
wristMotor1.config_kI(MM_SLOT, I_Value, TalonSRXEncoder.TIMEOUT_MS);
|
||||
wristMotor1.config_kD(MM_SLOT, D_Value, TalonSRXEncoder.TIMEOUT_MS);
|
||||
motorControllers.add(wristMotor1);
|
||||
|
||||
|
||||
}
|
||||
catch (Exception e) {
|
||||
System.err.println("An error occurred in the Wrist constructor");
|
||||
@@ -160,6 +176,37 @@ public class Wrist extends Subsystem implements ControlLoopable
|
||||
setWristControlMode(WristControlMode.JOYSTICK_MANUAL);
|
||||
}
|
||||
|
||||
public void setPositionMM(double targetPositionInches){
|
||||
wristMotor1.set(ControlMode.MotionMagic, targetPositionInches);
|
||||
//System.err.println(wristMotor1.getControlMode());
|
||||
wristMotor1.selectProfileSlot(MM_SLOT, 0);
|
||||
setWristControlMode(WristControlMode.MOTION_MAGIC);
|
||||
updatePositionMM(targetPositionInches);
|
||||
setFinished(false);
|
||||
}
|
||||
|
||||
public double calcGravityCompensationAtCurrentPosition() {
|
||||
int ticks = wristMotor1.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);
|
||||
System.err.println("we are here");
|
||||
//double startPositionInches = motor1.getPositionWorld();
|
||||
//System.err.println("compensation = " + compensation);
|
||||
wristMotor1.set(ControlMode.MotionMagic, targetPositionInches);
|
||||
//wristMotor1.set(ControlMode.MotionMagic, targetPositionInchesMM, DemandType.ArbitraryFeedForward, 0);
|
||||
//System.err.println(motor1.getControlMode());
|
||||
wristMotor1.configMotionCruiseVelocity(CV_value, TalonSRXEncoder.TIMEOUT_MS);
|
||||
wristMotor1.configMotionAcceleration(A_value, TalonSRXEncoder.TIMEOUT_MS);
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
public void setPositionPID(double targetPositionInches) {
|
||||
wristMotor1.set(ControlMode.Position, targetPositionInches);
|
||||
mpController.setPIDSlot(PID_SLOT); //TODO: verify that motor's selectProfileSlot() should be called AFTER its control mode is set
|
||||
@@ -279,6 +326,10 @@ public class Wrist extends Subsystem implements ControlLoopable
|
||||
isFinished = mpController.controlLoopUpdate(getPositionInches());
|
||||
|
||||
}
|
||||
if (wristControlMode == WristControlMode.MOTION_MAGIC){
|
||||
controlMMWithJoystick();
|
||||
//System.err.println(wristMotor1.getControlMode());
|
||||
}
|
||||
if (wristControlMode == WristControlMode.JOYSTICK_PID){
|
||||
//System.err.println(wristMotor1.getControlMode());
|
||||
controlPidWithJoystick();
|
||||
@@ -298,7 +349,14 @@ public class Wrist extends Subsystem implements ControlLoopable
|
||||
|
||||
|
||||
|
||||
|
||||
private void controlMMWithJoystick() {
|
||||
double joystickPosition = -Robot.oi.getOperatorController().getRightYAxis();
|
||||
double deltaPosition = joystickPosition * joystickInchesPerMs;
|
||||
targetPositionInchesMM = targetPositionInchesMM + deltaPosition;
|
||||
updatePositionMM(targetPositionInchesMM);
|
||||
//Robot.wrist.targetPositionInchesPID = targetPositionInchesPID - (deltaPosition/3);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -319,7 +377,7 @@ public class Wrist extends Subsystem implements ControlLoopable
|
||||
|
||||
private void controlManualWithJoystick() {
|
||||
double joyStickSpeed = -Robot.oi.getOperatorController().getRightYAxis();
|
||||
setSpeedJoystick(joyStickSpeed*.5);
|
||||
setSpeedJoystick(joyStickSpeed);
|
||||
}
|
||||
/*
|
||||
public void setShiftState(ElevatorSpeedShiftState state) {
|
||||
@@ -365,15 +423,16 @@ public class Wrist extends Subsystem implements ControlLoopable
|
||||
//System.err.println("the encoder is right after this");
|
||||
try {
|
||||
|
||||
SmartDashboard.putNumber("Wrist Position Ticks", wristMotor1.getPositionWorld());
|
||||
SmartDashboard.putNumber("Wrist Motor 1 Amps", wristMotor1.getOutputCurrent());
|
||||
SmartDashboard.putNumber("wrist pid error", wristMotor1.getClosedLoopError());
|
||||
SmartDashboard.putNumber("Wrist Ticks", wristMotor1.getPositionWorld());
|
||||
SmartDashboard.putNumber("Wrist Amps", wristMotor1.getOutputCurrent());
|
||||
SmartDashboard.putNumber("wrist error", wristMotor1.getClosedLoopError());
|
||||
SmartDashboard.putNumber("wrist motor output", wristMotor1.getMotorOutputPercent());
|
||||
|
||||
SmartDashboard.putNumber("wrist velocity", wristMotor1.getSelectedSensorVelocity());
|
||||
SmartDashboard.putNumber("Wrist Target MM", targetPositionInchesMM);
|
||||
// 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("Wrist Target PID Position", targetPositionInchesPID);
|
||||
SmartDashboard.putNumber("Wrist Target PID", targetPositionInchesPID);
|
||||
}
|
||||
catch (Exception e) {
|
||||
System.err.println("Drivetrain update status error" +e.getMessage());
|
||||
|
||||
@@ -124,10 +124,10 @@ public class RobotState {
|
||||
public void updateStatus(OperationMode operationMode) {
|
||||
if (operationMode == OperationMode.TEST) {
|
||||
RigidTransform2d odometry = getLatestFieldToVehicle().getValue();
|
||||
SmartDashboard.putNumber("robot_pose_x", odometry.getTranslation().x());
|
||||
SmartDashboard.putNumber("robot_pose_y", odometry.getTranslation().y());
|
||||
SmartDashboard.putNumber("robot_pose_theta", odometry.getRotation().getDegrees());
|
||||
SmartDashboard.putNumber("robot velocity", vehicle_velocity_measured_.dx);
|
||||
//SmartDashboard.putNumber("robot_pose_x", odometry.getTranslation().x());
|
||||
//SmartDashboard.putNumber("robot_pose_y", odometry.getTranslation().y());
|
||||
//SmartDashboard.putNumber("robot_pose_theta", odometry.getRotation().getDegrees());
|
||||
// SmartDashboard.putNumber("robot velocity", vehicle_velocity_measured_.dx);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user