From 5917ccb154b2e3a5e99d0aeacdf5c18b3807fb2a Mon Sep 17 00:00:00 2001 From: jac0rr Date: Mon, 20 Mar 2023 17:49:45 -0700 Subject: [PATCH 01/63] kinda working new joystick --- src/main/java/org/usfirst/frc4904/robot/Robot.java | 1 + src/main/java/org/usfirst/frc4904/robot/RobotMap.java | 3 +++ .../frc4904/robot/humaninterface/drivers/NathanGain.java | 1 + src/main/java/org/usfirst/frc4904/standard | 2 +- 4 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 2310536..4daee77 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -99,6 +99,7 @@ public void teleopInitialize() { public void teleopExecute() { SmartDashboard.putBoolean("isFlipped - IMPORTANT", NathanGain.isFlippy); SmartDashboard.putNumber("gyroooo", RobotMap.Component.navx.getAngle()); + SmartDashboard.putNumber("driverjoystick x", RobotMap.HumanInput.Driver.drivingJoystick.getX()); } @Override diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index a9d4f11..a3b2a6a 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -41,6 +41,7 @@ public static class Port { public static class HumanInput { public static final int joystick = 0; public static final int xboxController = 1; + public static final int drivingJoystick = 2; } // // blinky constants @@ -150,6 +151,7 @@ public static class Input { public static class HumanInput { public static class Driver { public static CustomCommandXbox xbox; + public static CustomCommandJoystick drivingJoystick; } public static class Operator { @@ -162,6 +164,7 @@ public RobotMap() { HumanInput.Driver.xbox = new CustomCommandXbox(Port.HumanInput.xboxController, 0.1); HumanInput.Operator.joystick = new CustomCommandJoystick(Port.HumanInput.joystick, 0.1); + HumanInput.Driver.drivingJoystick = new CustomCommandJoystick(Port.HumanInput.drivingJoystick, 0.1); // // UDP things // try { // Component.robotUDP = new RobotUDP(Port.Network.LOCAL_SOCKET_ADDRESS, Port.Network.LOCALIZATION_ADDRESS); diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java index 360c645..7a6ab8a 100644 --- a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java @@ -51,6 +51,7 @@ public void bindCommands() { RobotMap.HumanInput.Driver.xbox.y().onTrue(new InstantCommand(() -> NathanGain.precisionScaleY = 1)); RobotMap.HumanInput.Driver.xbox.y().onFalse(new InstantCommand(() -> NathanGain.precisionScaleY = NORMAL_SPEED_GAIN)); + RobotMap.HumanInput.Driver.drivingJoystick.button1.onTrue(new InstantCommand(() -> System.out.println("\n\n\n\n\n\nButton 1 pressed\n\n\n\n\n\n\n"))); } @Override diff --git a/src/main/java/org/usfirst/frc4904/standard b/src/main/java/org/usfirst/frc4904/standard index d921939..6984d40 160000 --- a/src/main/java/org/usfirst/frc4904/standard +++ b/src/main/java/org/usfirst/frc4904/standard @@ -1 +1 @@ -Subproject commit d921939710bf9bacde5e87d32f4f62756fcc1a2a +Subproject commit 6984d402dc16e4d48c9e783d946d4a77e4576c37 From ae5779ae1df6dac4c317a3e6a4745b537b0f318b Mon Sep 17 00:00:00 2001 From: jac0rr Date: Thu, 23 Mar 2023 21:09:26 -0700 Subject: [PATCH 02/63] instantiate nathangain to bind joystick commands --- src/main/java/org/usfirst/frc4904/robot/Robot.java | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 4daee77..55a9d6e 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -6,13 +6,11 @@ /*----------------------------------------------------------------------------*/ package org.usfirst.frc4904.robot; -import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import org.usfirst.frc4904.robot.humaninterface.drivers.NathanGain; import org.usfirst.frc4904.robot.humaninterface.operators.DefaultOperator; import org.usfirst.frc4904.robot.seenoevil.RobotContainer2; -import org.usfirst.frc4904.robot.subsystems.arm.ArmPivotSubsystem; import org.usfirst.frc4904.standard.CommandRobotBase; import org.usfirst.frc4904.standard.humaninput.Driver; @@ -20,10 +18,7 @@ import edu.wpi.first.math.controller.DifferentialDriveWheelVoltages; -import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; import static org.usfirst.frc4904.robot.Utils.nameCommand; @@ -32,7 +27,7 @@ public class Robot extends CommandRobotBase { private final RobotMap map = new RobotMap(); private final RobotContainer2 donttouchme = new RobotContainer2(RobotMap.Component.frontLeftWheelTalon, RobotMap.Component.backLeftWheelTalon, RobotMap.Component.frontRightWheelTalon, RobotMap.Component.backRightWheelTalon, RobotMap.Component.navx); - + private final NathanGain nathangain = new NathanGain(); private final Driver driver = new NathanGain(); private final org.usfirst.frc4904.standard.humaninput.Operator operator = new DefaultOperator(); @@ -93,6 +88,7 @@ public void teleopInitialize() { cmd1.setName("Intake - manual outtake activation"); RobotMap.HumanInput.Operator.joystick.button1.onTrue(cmd1); RobotMap.HumanInput.Operator.joystick.button1.onFalse(cmdnull); + nathangain.bindCommands(); } @Override @@ -100,6 +96,7 @@ public void teleopExecute() { SmartDashboard.putBoolean("isFlipped - IMPORTANT", NathanGain.isFlippy); SmartDashboard.putNumber("gyroooo", RobotMap.Component.navx.getAngle()); SmartDashboard.putNumber("driverjoystick x", RobotMap.HumanInput.Driver.drivingJoystick.getX()); + SmartDashboard.putNumber("driverjoystick y", RobotMap.HumanInput.Driver.drivingJoystick.getY()); } @Override From c49dcb95fe0a1307653a0d2937855c9bc73f628d Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Fri, 31 Mar 2023 14:52:51 -0700 Subject: [PATCH 03/63] odometry update is just trolling --- .../java/org/usfirst/frc4904/robot/Robot.java | 67 +-- .../robot/seenoevil/DriveSubsystem.java | 444 +++++++++--------- .../robot/seenoevil/RobotContainer2.java | 44 +- src/main/java/org/usfirst/frc4904/standard | 2 +- 4 files changed, 283 insertions(+), 274 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 39ccf97..002fc4f 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -11,7 +11,7 @@ import org.usfirst.frc4904.robot.humaninterface.drivers.NathanGain; import org.usfirst.frc4904.robot.humaninterface.operators.DefaultOperator; -import org.usfirst.frc4904.robot.seenoevil.DriveSubsystem; +// import org.usfirst.frc4904.robot.seenoevil.DriveSubsystem; import org.usfirst.frc4904.robot.seenoevil.RobotContainer2; import org.usfirst.frc4904.robot.subsystems.arm.ArmPivotSubsystem; import org.usfirst.frc4904.standard.CommandRobotBase; @@ -32,7 +32,7 @@ public class Robot extends CommandRobotBase { private final RobotMap map = new RobotMap(); - private final RobotContainer2 donttouchme = new RobotContainer2(RobotMap.Component.frontLeftWheelTalon, RobotMap.Component.backLeftWheelTalon, RobotMap.Component.frontRightWheelTalon, RobotMap.Component.backRightWheelTalon, RobotMap.Component.navx); + private final RobotContainer2 donttouchme = new RobotContainer2(RobotMap.Component.chassis, RobotMap.Component.navx); private final Driver driver = new NathanGain(); private final org.usfirst.frc4904.standard.humaninput.Operator operator = new DefaultOperator(); @@ -43,10 +43,10 @@ public void initialize() { @Override public void teleopInitialize() { - if (RobotContainer2.Component.leftATalonFX != null) RobotContainer2.Component.leftATalonFX.setNeutralMode(NeutralMode.Brake); - if (RobotContainer2.Component.leftBTalonFX != null) RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Brake); - if (RobotContainer2.Component.rightATalonFX != null) RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Brake); - if (RobotContainer2.Component.rightBTalonFX != null) RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Brake); + // if (RobotContainer2.Component.leftATalonFX != null) RobotContainer2.Component.leftATalonFX.setNeutralMode(NeutralMode.Brake); + // if (RobotContainer2.Component.leftBTalonFX != null) RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Brake); + // if (RobotContainer2.Component.rightATalonFX != null) RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Brake); + // if (RobotContainer2.Component.rightBTalonFX != null) RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Brake); RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.neutralOutput(); RobotMap.Component.arm.armExtensionSubsystem.motor.setBrakeOnNeutral(); @@ -58,10 +58,10 @@ public void teleopInitialize() { *************************/ // SATURDAY MORNING TEST - can you run drive train in queueline - donttouchme.m_robotDrive.m_leftMotors = null; - donttouchme.m_robotDrive.m_rightMotors = null; - donttouchme.m_robotDrive.m_drive = null; - DriveSubsystem.skuffedaf_teleop_initialized = true; + // donttouchme.m_robotDrive.m_leftMotors = null; + // donttouchme.m_robotDrive.m_rightMotors = null; + // donttouchme.m_robotDrive.m_drive = null; + // DriveSubsystem.skuffedaf_teleop_initialized = true; // donttouchme.m_robotDrive.m_leftEncoder = null; @@ -141,17 +141,20 @@ public void teleopExecute() { SmartDashboard.putNumber("arm extension length", RobotMap.Component.arm.armExtensionSubsystem.getCurrentExtensionLength()); SmartDashboard.putNumber("arm pivot angle", RobotMap.Component.arm.armPivotSubsystem.getCurrentAngleDegrees()); - SmartDashboard.putNumber("Falcon temp", RobotContainer2.Component.leftATalonFX.getTemperature()); + // SmartDashboard.putNumber("Falcon temp", RobotContainer2.Component.leftATalonFX.getTemperature()); } @Override public void autonomousInitialize() { - if (RobotContainer2.Component.leftATalonFX != null) RobotContainer2.Component.leftATalonFX.setNeutralMode(NeutralMode.Brake); - if (RobotContainer2.Component.leftBTalonFX != null) RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Brake); - if (RobotContainer2.Component.rightATalonFX != null) RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Brake); - if (RobotContainer2.Component.rightBTalonFX != null) RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Brake); + RobotMap.Component.chassis.leftMotors.coast(); + RobotMap.Component.chassis.rightMotors.coast(); + + // if (RobotContainer2.Component.leftATalonFX != null) RobotContainer2.Component.leftATalonFX.setNeutralMode(NeutralMode.Brake); + // if (RobotContainer2.Component.leftBTalonFX != null) RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Brake); + // if (RobotContainer2.Component.rightATalonFX != null) RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Brake); + // if (RobotContainer2.Component.rightBTalonFX != null) RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Brake); // if (RobotContainer2.Component.leftATalonFX != null) { RobotContainer2.Component.leftATalonFX.setNeutralMode(NeutralMode.Coast); RobotContainer2.Component.leftATalonFX.neutralOutput(); } @@ -172,8 +175,8 @@ public void autonomousInitialize() { // SATURDAY MORNING TEST: is the cube shooter auton gonna work // var commnand = donttouchme.balanceAutonAndShootCube(donttouchme.m_robotDrive::getWheelSpeeds, donttouchme.m_robotDrive::tankDriveVolts); - var commnand = donttouchme.getAutonomousCommand(donttouchme.getTrajectory("straight_forward")); - commnand.schedule(); + // var commnand = donttouchme.getAutonomousCommand(donttouchme.getTrajectory("straight_forward")); + // commnand.schedule(); } @Override @@ -182,25 +185,29 @@ public void autonomousExecute() { SmartDashboard.putBoolean("isFlipped - IMPORTANT", NathanGain.isFlippy); - SmartDashboard.putString("pose string", donttouchme.m_robotDrive.getPose().toString()); - SmartDashboard.putNumber("pose x", donttouchme.m_robotDrive.getPose().getX()); - SmartDashboard.putNumber("pose y", donttouchme.m_robotDrive.getPose().getY()); - SmartDashboard.putNumber("pose heading", donttouchme.m_robotDrive.getPose().getRotation().getDegrees()); + // SmartDashboard.putString("pose string", donttouchme.m_robotDrive.getPose().toString()); + // SmartDashboard.putNumber("pose x", donttouchme.m_robotDrive.getPose().getX()); + // SmartDashboard.putNumber("pose y", donttouchme.m_robotDrive.getPose().getY()); + // SmartDashboard.putNumber("pose heading", donttouchme.m_robotDrive.getPose().getRotation().getDegrees()); + + SmartDashboard.putString("pose string", RobotMap.Component.chassis.getPoseMeters().toString()); + SmartDashboard.putNumber("pose x", RobotMap.Component.chassis.getPoseMeters().getX()); + SmartDashboard.putNumber("pose y", RobotMap.Component.chassis.getPoseMeters().getY()); + SmartDashboard.putNumber("pose heading", RobotMap.Component.chassis.getPoseMeters().getRotation().getDegrees()); SmartDashboard.putNumber("gyroooo", RobotMap.Component.navx.getAngle()); SmartDashboard.putNumber("armV extension length", RobotMap.Component.arm.armExtensionSubsystem.getCurrentExtensionLength()); SmartDashboard.putNumber("arm pivot angle", RobotMap.Component.arm.armPivotSubsystem.getCurrentAngleDegrees()); - SmartDashboard.putNumber("Falcon temp", RobotContainer2.Component.leftATalonFX.getTemperature()); } @Override public void disabledInitialize() { - if (RobotContainer2.Component.leftATalonFX != null) RobotContainer2.Component.leftATalonFX.setNeutralMode(NeutralMode.Brake); - if (RobotContainer2.Component.leftBTalonFX != null) RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Brake); - if (RobotContainer2.Component.rightATalonFX != null) RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Brake); - if (RobotContainer2.Component.rightBTalonFX != null) RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Brake); + // if (RobotContainer2.Component.leftATalonFX != null) RobotContainer2.Component.leftATalonFX.setNeutralMode(NeutralMode.Brake); + // if (RobotContainer2.Component.leftBTalonFX != null) RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Brake); + // if (RobotContainer2.Component.rightATalonFX != null) RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Brake); + // if (RobotContainer2.Component.rightBTalonFX != null) RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Brake); RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.neutralOutput(); RobotMap.Component.arm.armExtensionSubsystem.motor.setBrakeOnNeutral(); @@ -217,10 +224,10 @@ public void disabledExecute() { @Override public void testInitialize() { - if (RobotContainer2.Component.leftATalonFX != null) { RobotContainer2.Component.leftATalonFX.setNeutralMode(NeutralMode.Coast); RobotContainer2.Component.leftATalonFX.neutralOutput(); } - if (RobotContainer2.Component.leftBTalonFX != null) { RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Coast); RobotContainer2.Component.leftBTalonFX.neutralOutput(); } - if (RobotContainer2.Component.rightATalonFX != null) { RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Coast); RobotContainer2.Component.rightATalonFX.neutralOutput(); } - if (RobotContainer2.Component.rightBTalonFX != null) { RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Coast); RobotContainer2.Component.rightBTalonFX.neutralOutput(); } + // if (RobotContainer2.Component.leftATalonFX != null) { RobotContainer2.Component.leftATalonFX.setNeutralMode(NeutralMode.Coast); RobotContainer2.Component.leftATalonFX.neutralOutput(); } + // if (RobotContainer2.Component.leftBTalonFX != null) { RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Coast); RobotContainer2.Component.leftBTalonFX.neutralOutput(); } + // if (RobotContainer2.Component.rightATalonFX != null) { RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Coast); RobotContainer2.Component.rightATalonFX.neutralOutput(); } + // if (RobotContainer2.Component.rightBTalonFX != null) { RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Coast); RobotContainer2.Component.rightBTalonFX.neutralOutput(); } RobotMap.Component.arm.armPivotSubsystem.initializeEncoderPositions(); RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.setCoastOnNeutral(); RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.neutralOutput(); diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/DriveSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/DriveSubsystem.java index cfc5143..272d4f9 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/DriveSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/DriveSubsystem.java @@ -1,225 +1,225 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package org.usfirst.frc4904.robot.seenoevil; - - -import com.kauailabs.navx.frc.AHRS; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; -import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; -import edu.wpi.first.wpilibj.CAN; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.I2C; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.interfaces.Gyro; -import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup; -import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; -import edu.wpi.first.wpilibj.motorcontrol.Talon; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj.SerialPort; - -import org.usfirst.frc4904.robot.seenoevil.Constants.DriveConstants; - -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.can.TalonFX; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import edu.wpi.first.wpilibj.motorcontrol.MotorController; - - -public class DriveSubsystem extends SubsystemBase { - public static boolean skuffedaf_teleop_initialized = false; - // The motors on the left side of the drive. - public MotorControllerGroup m_leftMotors = //motors need to be talons - new MotorControllerGroup( - RobotContainer2.Component.leftATalonFX, - RobotContainer2.Component.leftBTalonFX); - - // The motors on the right side of the drive. - public MotorControllerGroup m_rightMotors = - new MotorControllerGroup( - RobotContainer2.Component.rightATalonFX, - RobotContainer2.Component.rightBTalonFX); - - // The robot's drive - public DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors); - - // The left-side drive encoder - // private final ManualTalonEncoderLmao m_leftEncoder = - // new ManualTalonEncoderLmao( - // RobotContainer.Component.leftATalonFX, 1); - - // // The right-side drive encoder - // private final ManualTalonEncoderLmao m_rightEncoder = - // new ManualTalonEncoderLmao( - // RobotContainer.Component.rightATalonFX, 2); +// // Copyright (c) FIRST and other WPILib contributors. +// // Open Source Software; you can modify and/or share it under the terms of +// // the WPILib BSD license file in the root directory of this project. + +// package org.usfirst.frc4904.robot.seenoevil; + + +// import com.kauailabs.navx.frc.AHRS; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; +// import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; +// import edu.wpi.first.wpilibj.CAN; +// import edu.wpi.first.wpilibj.Encoder; +// import edu.wpi.first.wpilibj.I2C; +// import edu.wpi.first.wpilibj.drive.DifferentialDrive; +// import edu.wpi.first.wpilibj.interfaces.Gyro; +// import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup; +// import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +// import edu.wpi.first.wpilibj.motorcontrol.Talon; +// import edu.wpi.first.wpilibj2.command.SubsystemBase; +// import edu.wpi.first.wpilibj.SerialPort; + +// import org.usfirst.frc4904.robot.seenoevil.Constants.DriveConstants; + +// import com.ctre.phoenix.motorcontrol.FeedbackDevice; +// import com.ctre.phoenix.motorcontrol.NeutralMode; +// import com.ctre.phoenix.motorcontrol.can.TalonFX; +// import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +// import edu.wpi.first.wpilibj.motorcontrol.MotorController; + + +// public class DriveSubsystem extends SubsystemBase { +// public static boolean skuffedaf_teleop_initialized = false; +// // The motors on the left side of the drive. +// public MotorControllerGroup m_leftMotors = //motors need to be talons +// new MotorControllerGroup( +// RobotContainer2.Component.leftATalonFX, +// RobotContainer2.Component.leftBTalonFX); + +// // The motors on the right side of the drive. +// public MotorControllerGroup m_rightMotors = +// new MotorControllerGroup( +// RobotContainer2.Component.rightATalonFX, +// RobotContainer2.Component.rightBTalonFX); + +// // The robot's drive +// public DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors); + +// // The left-side drive encoder +// // private final ManualTalonEncoderLmao m_leftEncoder = +// // new ManualTalonEncoderLmao( +// // RobotContainer.Component.leftATalonFX, 1); + +// // // The right-side drive encoder +// // private final ManualTalonEncoderLmao m_rightEncoder = +// // new ManualTalonEncoderLmao( +// // RobotContainer.Component.rightATalonFX, 2); -public Imblueeeeeee m_leftEncoder = - new Imblueeeeeee( - "leftWheel", RobotContainer2.Component.leftATalonFX, true, DriveConstants.kEncoderDistancePerPulse, FeedbackDevice.IntegratedSensor); - -// The right-side drive encoder -public Imblueeeeeee m_rightEncoder = - new Imblueeeeeee( - "leftWheel", RobotContainer2.Component.rightATalonFX, false, DriveConstants.kEncoderDistancePerPulse, FeedbackDevice.IntegratedSensor); - - // The gyro sensor - public final AHRS m_gyro; - - - // Odometry class for tracking robot pose - private final DifferentialDriveOdometry m_odometry; - - /** Creates a new DriveSubsystem. */ - public DriveSubsystem(AHRS navx) { - m_gyro = navx; - // We need to invert one side of the drivetrain so that positive voltages - // result in both sides moving forward. Depending on how your robot's - // gearbox is constructed, you might have to invert the left side instead. - m_leftMotors.setInverted(true); - // Sets the distance per pulse for the encoders - m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); - m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); - - resetEncoders(); - m_odometry = - new DifferentialDriveOdometry( - m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance()); - } - - @Override - public void periodic() { - // Update the odometry in the periodic block - if (!skuffedaf_teleop_initialized) { - m_odometry.update( - m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance()); - } - // RobotContainer2.Component.leftATalonFX.feed(); - // RobotContainer2.Component.leftBTalonFX.feed(); - - // RobotContainer2.Component.rightATalonFX.feed(); - // RobotContainer2.Component.rightBTalonFX.feed(); +// public Imblueeeeeee m_leftEncoder = +// new Imblueeeeeee( +// "leftWheel", RobotContainer2.Component.leftATalonFX, true, DriveConstants.kEncoderDistancePerPulse, FeedbackDevice.IntegratedSensor); + +// // The right-side drive encoder +// public Imblueeeeeee m_rightEncoder = +// new Imblueeeeeee( +// "leftWheel", RobotContainer2.Component.rightATalonFX, false, DriveConstants.kEncoderDistancePerPulse, FeedbackDevice.IntegratedSensor); + +// // The gyro sensor +// public final AHRS m_gyro; + + +// // Odometry class for tracking robot pose +// private final DifferentialDriveOdometry m_odometry; + +// /** Creates a new DriveSubsystem. */ +// public DriveSubsystem(AHRS navx) { +// m_gyro = navx; +// // We need to invert one side of the drivetrain so that positive voltages +// // result in both sides moving forward. Depending on how your robot's +// // gearbox is constructed, you might have to invert the left side instead. +// m_leftMotors.setInverted(true); +// // Sets the distance per pulse for the encoders +// m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); +// m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); + +// resetEncoders(); +// m_odometry = +// new DifferentialDriveOdometry( +// m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance()); +// } + +// @Override +// public void periodic() { +// // Update the odometry in the periodic block +// if (!skuffedaf_teleop_initialized) { +// m_odometry.update( +// m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance()); +// } +// // RobotContainer2.Component.leftATalonFX.feed(); +// // RobotContainer2.Component.leftBTalonFX.feed(); + +// // RobotContainer2.Component.rightATalonFX.feed(); +// // RobotContainer2.Component.rightBTalonFX.feed(); - // m_leftEncoder.debug(); - // m_rightEncoder.debug(); - } - - /** - * Returns the currently-estimated pose of the robot. - * - * @return The pose. - */ - public Pose2d getPose() { - return m_odometry.getPoseMeters(); - } - - /** - * Returns the current wheel speeds of the robot. - * - * @return The current wheel speeds. - */ - public DifferentialDriveWheelSpeeds getWheelSpeeds() { - return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate()); - } - - /** - * Resets the odometry to the specified pose. - * - * @param pose The pose to which to set the odometry. - */ - public void resetOdometry(Pose2d pose) { - resetEncoders(); - m_odometry.resetPosition( - m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance(), pose); - } - - /** - * Drives the robot using arcade controls. - * - * @param fwd the commanded forward movement - * @param rot the commanded rotation - */ - public void arcadeDrive(double fwd, double rot) { - m_drive.arcadeDrive(fwd, rot); - } - - /** - * Controls the left and right sides of the drive directly with voltages. - * - * @param leftVolts the commanded left output - * @param rightVolts the commanded right output - */ - public void tankDriveVolts(double leftVolts, double rightVolts) { - m_leftMotors.setVoltage(leftVolts); - m_rightMotors.setVoltage(rightVolts); - m_drive.feed(); - } - - /** Resets the drive encoders to currently read a position of 0. */ - public void resetEncoders() { - m_leftEncoder.reset(); - m_rightEncoder.reset(); - } - - /** - * Gets the average distance of the two encoders. - * - * @return the average of the two encoder readings - */ - public double getAverageEncoderDistance() { - return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0; - } - - /** - * Gets the left drive encoder. - * - * @return the left drive encoder - */ - public Imblueeeeeee getLeftEncoder() { - return m_leftEncoder; - } - - /** - * Gets the right drive encoder. - *c - * @return the right drive encoder - */ - public Imblueeeeeee getRightEncoder() { - return m_rightEncoder; - } - - /** - * Sets the max output of the drive. Useful for scaling the drive to drive more slowly. - * - * @param maxOutput the maximum output to which the drive will be constrained - */ - public void setMaxOutput(double maxOutput) { - m_drive.setMaxOutput(maxOutput); - } - - /** Zeroes the heading of the robot. */ - public void zeroHeading() { - m_gyro.reset(); - } - - /** - * Returns the heading of the robot. - * - * @return the robot's heading in degrees, from -180 to 180 - */ - public double getHeading() { - return m_gyro.getRotation2d().getDegrees(); - } - - /** - * Returns the turn rate of the robot. - * - * @return The turn rate of the robot, in degrees per second - */ - public double getTurnRate() { - return -m_gyro.getRate(); - } -} +// // m_leftEncoder.debug(); +// // m_rightEncoder.debug(); +// } + +// /** +// * Returns the currently-estimated pose of the robot. +// * +// * @return The pose. +// */ +// public Pose2d getPose() { +// return m_odometry.getPoseMeters(); +// } + +// /** +// * Returns the current wheel speeds of the robot. +// * +// * @return The current wheel speeds. +// */ +// public DifferentialDriveWheelSpeeds getWheelSpeeds() { +// return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate()); +// } + +// /** +// * Resets the odometry to the specified pose. +// * +// * @param pose The pose to which to set the odometry. +// */ +// public void resetOdometry(Pose2d pose) { +// resetEncoders(); +// m_odometry.resetPosition( +// m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance(), pose); +// } + +// /** +// * Drives the robot using arcade controls. +// * +// * @param fwd the commanded forward movement +// * @param rot the commanded rotation +// */ +// public void arcadeDrive(double fwd, double rot) { +// m_drive.arcadeDrive(fwd, rot); +// } + +// /** +// * Controls the left and right sides of the drive directly with voltages. +// * +// * @param leftVolts the commanded left output +// * @param rightVolts the commanded right output +// */ +// public void tankDriveVolts(double leftVolts, double rightVolts) { +// m_leftMotors.setVoltage(leftVolts); +// m_rightMotors.setVoltage(rightVolts); +// m_drive.feed(); +// } + +// /** Resets the drive encoders to currently read a position of 0. */ +// public void resetEncoders() { +// m_leftEncoder.reset(); +// m_rightEncoder.reset(); +// } + +// /** +// * Gets the average distance of the two encoders. +// * +// * @return the average of the two encoder readings +// */ +// public double getAverageEncoderDistance() { +// return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0; +// } + +// /** +// * Gets the left drive encoder. +// * +// * @return the left drive encoder +// */ +// public Imblueeeeeee getLeftEncoder() { +// return m_leftEncoder; +// } + +// /** +// * Gets the right drive encoder. +// *c +// * @return the right drive encoder +// */ +// public Imblueeeeeee getRightEncoder() { +// return m_rightEncoder; +// } + +// /** +// * Sets the max output of the drive. Useful for scaling the drive to drive more slowly. +// * +// * @param maxOutput the maximum output to which the drive will be constrained +// */ +// public void setMaxOutput(double maxOutput) { +// m_drive.setMaxOutput(maxOutput); +// } + +// /** Zeroes the heading of the robot. */ +// public void zeroHeading() { +// m_gyro.reset(); +// } + +// /** +// * Returns the heading of the robot. +// * +// * @return the robot's heading in degrees, from -180 to 180 +// */ +// public double getHeading() { +// return m_gyro.getRotation2d().getDegrees(); +// } + +// /** +// * Returns the turn rate of the robot. +// * +// * @return The turn rate of the robot, in degrees per second +// */ +// public double getTurnRate() { +// return -m_gyro.getRate(); +// } +// } diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java index de566fe..d0bf1cd 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java @@ -8,6 +8,7 @@ import static edu.wpi.first.wpilibj.XboxController.Button; +import edu.wpi.first.math.controller.DifferentialDriveWheelVoltages; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.RamseteController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -42,6 +43,7 @@ import org.usfirst.frc4904.robot.seenoevil.Constants.AutoConstants; import org.usfirst.frc4904.robot.seenoevil.Constants.DriveConstants; import org.usfirst.frc4904.standard.custom.sensors.NavX; +import org.usfirst.frc4904.standard.subsystems.chassis.WestCoastDrive; import org.usfirst.frc4904.robot.Robot; import org.usfirst.frc4904.robot.RobotMap; import org.usfirst.frc4904.robot.seenoevil.Balance; @@ -191,44 +193,44 @@ public class RobotContainer2 { ); public static class Component { - public static WPI_TalonFX leftATalonFX; - public static WPI_TalonFX leftBTalonFX; - public static WPI_TalonFX rightATalonFX; - public static WPI_TalonFX rightBTalonFX; - public static WPI_TalonFX testTalon; + // public static WPI_TalonFX leftATalonFX; + // public static WPI_TalonFX leftBTalonFX; + // public static WPI_TalonFX rightATalonFX; + // public static WPI_TalonFX rightBTalonFX; + // public static WPI_TalonFX testTalon; } // The robot's subsystems // motors - public final DriveSubsystem m_robotDrive; + public final WestCoastDrive m_robotDrive; // public final XboxController m_driverController; /** * The container for the robot. Contains subsystems, OI devices, and commands. */ - public RobotContainer2(WPI_TalonFX leftATalonFX, WPI_TalonFX leftBTalonFX, WPI_TalonFX rightATalonFX, WPI_TalonFX rightBTalonFX, AHRS navx) { + public RobotContainer2(WestCoastDrive drive, AHRS navx) { // The driver's controller // m_driverController = new XboxController(OIConstants.kDriverControllerPort); // Configure the button bindings - Component.leftATalonFX = leftATalonFX; - Component.leftBTalonFX = leftBTalonFX; - Component.rightATalonFX = rightATalonFX; - Component.rightBTalonFX = rightBTalonFX; + // Component.leftATalonFX = leftATalonFX; + // Component.leftBTalonFX = leftBTalonFX; + // Component.rightATalonFX = rightATalonFX; + // Component.rightBTalonFX = rightBTalonFX; - RobotContainer2.Component.leftATalonFX.setNeutralMode(NeutralMode.Coast); - RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Coast); - RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Coast); - RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Coast); + // RobotContainer2.Component.leftATalonFX.setNeutralMode(NeutralMode.Coast); + // RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Coast); + // RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Coast); + // RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Coast); - Component.testTalon = new WPI_TalonFX(1); + // Component.testTalon = new WPI_TalonFX(1); // Component.leftATalonFX.setInverted(true); // Component.leftBTalonFX.setInverted(true); - this.m_robotDrive = new DriveSubsystem(navx); + this.m_robotDrive = drive; // Configure default commands // Set the default drive command to split-stick arcade drive @@ -246,7 +248,7 @@ public Command getAutonomousCommand(Trajectory trajectory) { EEEE.setEnabled(false); RamseteCommand ramseteCommand = new RamseteCommand( trajectory, - m_robotDrive::getPose, + m_robotDrive::getPoseMeters, EEEE, new SimpleMotorFeedforward( DriveConstants.ksVolts, @@ -257,7 +259,7 @@ public Command getAutonomousCommand(Trajectory trajectory) { // new PIDController(DriveConstants.kPDriveVel, 0, 0), new PIDController(DriveConstants.kPDriveVel, 0, 0), new PIDController(0, 0, 0), new PIDController(0, 0, 0), // RamseteCommand passes volts to the callback - m_robotDrive::tankDriveVolts, + m_robotDrive::setWheelVoltages, m_robotDrive); // Reset odometry to the starting pose of the trajectory. @@ -268,9 +270,9 @@ public Command getAutonomousCommand(Trajectory trajectory) { // return Commands.run(() -> m_robotDrive.tankDriveVolts(1, 1), m_robotDrive); //return Commands.runOnce(() -> m_robotDrive.arcadeDrive(0.5, 0), m_robotDrive); //return Commands.runOnce(() -> Component.testTalon.setVoltage(6)); - return Commands.runOnce(() -> m_robotDrive.resetOdometry(trajectory.getInitialPose()) + return Commands.runOnce(() -> m_robotDrive.resetPoseMeters(trajectory.getInitialPose()) ) .andThen( ramseteCommand) - .andThen(() -> m_robotDrive.tankDriveVolts(0, 0)); + .andThen(() -> m_robotDrive.setWheelVoltages(0, 0)); } /** diff --git a/src/main/java/org/usfirst/frc4904/standard b/src/main/java/org/usfirst/frc4904/standard index 5173578..08160ac 160000 --- a/src/main/java/org/usfirst/frc4904/standard +++ b/src/main/java/org/usfirst/frc4904/standard @@ -1 +1 @@ -Subproject commit 51735784ae2cc3ee6f420282d516eea222492983 +Subproject commit 08160ac4c6f47234f258862ce2256be06beae3c4 From 2ccb1277611940382a27365a2be94be0f529c8f4 Mon Sep 17 00:00:00 2001 From: aze12345 <88347612+aze12345@users.noreply.github.com> Date: Fri, 31 Mar 2023 14:57:12 -0700 Subject: [PATCH 04/63] added buttons to this branch --- src/main/java/org/usfirst/frc4904/standard | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/usfirst/frc4904/standard b/src/main/java/org/usfirst/frc4904/standard index 08160ac..5173578 160000 --- a/src/main/java/org/usfirst/frc4904/standard +++ b/src/main/java/org/usfirst/frc4904/standard @@ -1 +1 @@ -Subproject commit 08160ac4c6f47234f258862ce2256be06beae3c4 +Subproject commit 51735784ae2cc3ee6f420282d516eea222492983 From 7f2c56f9c520eabda7f481d924714b7685ee51d2 Mon Sep 17 00:00:00 2001 From: aze12345 <88347612+aze12345@users.noreply.github.com> Date: Fri, 31 Mar 2023 15:00:35 -0700 Subject: [PATCH 05/63] oops --- .../java/org/usfirst/frc4904/robot/Robot.java | 16 ++++++++++++++++ .../robot/subsystems/arm/ArmSubsystem.java | 2 +- 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 002fc4f..6159e25 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -122,6 +122,22 @@ public void teleopInitialize() { cmd1.setName("Intake - manual outtake activation"); RobotMap.HumanInput.Operator.joystick.button1.onTrue(cmd1); RobotMap.HumanInput.Operator.joystick.button1.onFalse(cmdnull); + + // position + place cube + RobotMap.HumanInput.Operator.joystick.button7.onTrue(RobotMap.Component.arm.c_angleCubes(3)); + RobotMap.HumanInput.Operator.joystick.button9.onTrue(RobotMap.Component.arm.c_angleCubes(2)); + RobotMap.HumanInput.Operator.joystick.button11.onTrue(RobotMap.Component.arm.c_angleCubes(1)); + + //position cone + RobotMap.HumanInput.Operator.joystick.button8.onTrue(RobotMap.Component.arm.placeCones(3)); + RobotMap.HumanInput.Operator.joystick.button10.onTrue(RobotMap.Component.arm.placeCones(2)); + RobotMap.HumanInput.Operator.joystick.button12.onTrue(RobotMap.Component.arm.placeCones(1)); + + //shelf intake + RobotMap.HumanInput.Operator.joystick.button6.onTrue(RobotMap.Component.arm.c_posIntakeShelf()); + + //ground intake + RobotMap.HumanInput.Operator.joystick.button4.onTrue(RobotMap.Component.arm.c_posIntakeGround()); } @Override diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index ecd9a98..d26d9cb 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -113,7 +113,7 @@ public Command placeCube(int shelf) { .alongWith(new WaitCommand(1).andThen(RobotMap.Component.intake.c_holdVoltage(Intake.DEFAULT_INTAKE_VOLTS)) )).withTimeout(5); //TODO: change timeout } - public Command c_angleCones(int shelf) { + public Command placeCones(int shelf) { var degreesFromHorizontal = cones.get(shelf).getFirst(); var extensionLengthMeters = cones.get(shelf).getSecond(); From 75ecd243915c0ed8e81991b2691e31703144d911 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Fri, 31 Mar 2023 15:08:23 -0700 Subject: [PATCH 06/63] Basic driver changes --- src/main/java/org/usfirst/frc4904/robot/Robot.java | 2 ++ .../frc4904/robot/humaninterface/drivers/NathanGain.java | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 002fc4f..4283d13 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -52,6 +52,8 @@ public void teleopInitialize() { RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.setBrakeOnNeutral(); RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.neutralOutput(); + + driver.bindCommands(); /*********************** * HAZMAT BLOCK START diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java index 4fb7e75..9eca294 100644 --- a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java @@ -11,11 +11,11 @@ public class NathanGain extends Driver { public static final double SPEED_EXP = 2; public static final double Y_SPEED_SCALE = 1; - public static final double TURN_EXP = 3; + public static final double TURN_EXP = 3.3; public static final double TURN_SPEED_SCALE = 1; - public static final double NORMAL_SPEED_GAIN = 0.7; // TODO TUNE + public static final double NORMAL_SPEED_GAIN = 0.5; // TODO TUNE public static final double NORMAL_TURN_GAIN = 0.3; public static final double PRECISE_SPEED_SCALE = 0.4; From e2b880e86548bdcfb2f6e23b63da540b471b1008 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Fri, 31 Mar 2023 15:09:24 -0700 Subject: [PATCH 07/63] turn correction --- .../frc4904/robot/humaninterface/drivers/NathanGain.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java index 4fb7e75..1a9b35a 100644 --- a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java @@ -21,6 +21,8 @@ public class NathanGain extends Driver { public static final double PRECISE_SPEED_SCALE = 0.4; public static final double PRECISE_TURN_SCALE = 0.3; + public static final double TURN_CORRECTION = 0.05; + // public static final double SPEEDY_TURN_GAIN = 0.7; public static double precisionScaleY = NORMAL_SPEED_GAIN; @@ -81,6 +83,6 @@ public double getTurnSpeed() { // double operatorControlTurnSpeed = scaleGain(RobotMap.HumanInput.Operator.joystick.getAxis(0), 0.2, 1.5); if (NathanGain.isFlippy) return (turnSpeed) * -1; - return turnSpeed;// + precisionTurnSpeed; + return turnSpeed + TURN_CORRECTION;// + precisionTurnSpeed; } } From 4b3687a26628e5c85849059d8bc3ed96ff15d70a Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Fri, 31 Mar 2023 15:11:30 -0700 Subject: [PATCH 08/63] Revert "oops" This reverts commit 7f2c56f9c520eabda7f481d924714b7685ee51d2. --- .../java/org/usfirst/frc4904/robot/Robot.java | 16 ---------------- .../robot/subsystems/arm/ArmSubsystem.java | 2 +- 2 files changed, 1 insertion(+), 17 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 5abef9c..4283d13 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -124,22 +124,6 @@ public void teleopInitialize() { cmd1.setName("Intake - manual outtake activation"); RobotMap.HumanInput.Operator.joystick.button1.onTrue(cmd1); RobotMap.HumanInput.Operator.joystick.button1.onFalse(cmdnull); - - // position + place cube - RobotMap.HumanInput.Operator.joystick.button7.onTrue(RobotMap.Component.arm.c_angleCubes(3)); - RobotMap.HumanInput.Operator.joystick.button9.onTrue(RobotMap.Component.arm.c_angleCubes(2)); - RobotMap.HumanInput.Operator.joystick.button11.onTrue(RobotMap.Component.arm.c_angleCubes(1)); - - //position cone - RobotMap.HumanInput.Operator.joystick.button8.onTrue(RobotMap.Component.arm.placeCones(3)); - RobotMap.HumanInput.Operator.joystick.button10.onTrue(RobotMap.Component.arm.placeCones(2)); - RobotMap.HumanInput.Operator.joystick.button12.onTrue(RobotMap.Component.arm.placeCones(1)); - - //shelf intake - RobotMap.HumanInput.Operator.joystick.button6.onTrue(RobotMap.Component.arm.c_posIntakeShelf()); - - //ground intake - RobotMap.HumanInput.Operator.joystick.button4.onTrue(RobotMap.Component.arm.c_posIntakeGround()); } @Override diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index d26d9cb..ecd9a98 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -113,7 +113,7 @@ public Command placeCube(int shelf) { .alongWith(new WaitCommand(1).andThen(RobotMap.Component.intake.c_holdVoltage(Intake.DEFAULT_INTAKE_VOLTS)) )).withTimeout(5); //TODO: change timeout } - public Command placeCones(int shelf) { + public Command c_angleCones(int shelf) { var degreesFromHorizontal = cones.get(shelf).getFirst(); var extensionLengthMeters = cones.get(shelf).getSecond(); From 3fb4c2f566a45000e087c540d185e50e56a77e49 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Fri, 31 Mar 2023 16:29:33 -0700 Subject: [PATCH 09/63] Tuned driver gain --- src/main/java/org/usfirst/frc4904/robot/Robot.java | 4 +++- src/main/java/org/usfirst/frc4904/robot/RobotMap.java | 4 ++-- .../frc4904/robot/humaninterface/drivers/NathanGain.java | 4 ++-- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 4283d13..06482b6 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -120,7 +120,7 @@ public void teleopInitialize() { RobotMap.HumanInput.Operator.joystick.button2.onFalse(cmdhold); // Outtake - var cmd1 = RobotMap.Component.intake.c_holdVoltage(3); + var cmd1 = RobotMap.Component.intake.c_holdVoltage(5); cmd1.setName("Intake - manual outtake activation"); RobotMap.HumanInput.Operator.joystick.button1.onTrue(cmd1); RobotMap.HumanInput.Operator.joystick.button1.onFalse(cmdnull); @@ -250,6 +250,8 @@ public void testExecute() { public void alwaysExecute() { // SmartDashboard.putNumber("Arm angle", RobotMap.Component.arm.armPivotSubsystem.getCurrentAngleDegrees()); // SmartDashboard.putNumber("gyroooo", RobotMap.Component.navx.getAngle()); + SmartDashboard.putNumber("joystick left T", RobotMap.HumanInput.Driver.xbox.getLeftTriggerAxis()); + SmartDashboard.putNumber("joystick right T", RobotMap.HumanInput.Driver.xbox.getRightTriggerAxis()); diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index cce0fa6..948fd8b 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -165,8 +165,8 @@ public static class Operator { public RobotMap() { Component.navx = new AHRS(SerialPort.Port.kMXP); - HumanInput.Driver.xbox = new CustomCommandXbox(Port.HumanInput.xboxController, 0.1); - HumanInput.Operator.joystick = new CustomCommandJoystick(Port.HumanInput.joystick, 0.05); + HumanInput.Driver.xbox = new CustomCommandXbox(Port.HumanInput.xboxController, 0.01); + HumanInput.Operator.joystick = new CustomCommandJoystick(Port.HumanInput.joystick, 0.01); // // UDP things // try { // Component.robotUDP = new RobotUDP(Port.Network.LOCAL_SOCKET_ADDRESS, Port.Network.LOCALIZATION_ADDRESS); diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java index 9eca294..025f570 100644 --- a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java @@ -11,7 +11,7 @@ public class NathanGain extends Driver { public static final double SPEED_EXP = 2; public static final double Y_SPEED_SCALE = 1; - public static final double TURN_EXP = 3.3; + public static final double TURN_EXP = 4; public static final double TURN_SPEED_SCALE = 1; @@ -19,7 +19,7 @@ public class NathanGain extends Driver { public static final double NORMAL_TURN_GAIN = 0.3; public static final double PRECISE_SPEED_SCALE = 0.4; - public static final double PRECISE_TURN_SCALE = 0.3; + public static final double PRECISE_TURN_SCALE = 0.1; // public static final double SPEEDY_TURN_GAIN = 0.7; From e23b303769d4b621b33945584d7385190a936abe Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Fri, 31 Mar 2023 16:30:15 -0700 Subject: [PATCH 10/63] Revert "Revert "oops"" This reverts commit 4b3687a26628e5c85849059d8bc3ed96ff15d70a. --- .../java/org/usfirst/frc4904/robot/Robot.java | 16 ++++++++++++++++ .../robot/subsystems/arm/ArmSubsystem.java | 2 +- 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 06482b6..36c43e1 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -124,6 +124,22 @@ public void teleopInitialize() { cmd1.setName("Intake - manual outtake activation"); RobotMap.HumanInput.Operator.joystick.button1.onTrue(cmd1); RobotMap.HumanInput.Operator.joystick.button1.onFalse(cmdnull); + + // position + place cube + RobotMap.HumanInput.Operator.joystick.button7.onTrue(RobotMap.Component.arm.c_angleCubes(3)); + RobotMap.HumanInput.Operator.joystick.button9.onTrue(RobotMap.Component.arm.c_angleCubes(2)); + RobotMap.HumanInput.Operator.joystick.button11.onTrue(RobotMap.Component.arm.c_angleCubes(1)); + + //position cone + RobotMap.HumanInput.Operator.joystick.button8.onTrue(RobotMap.Component.arm.placeCones(3)); + RobotMap.HumanInput.Operator.joystick.button10.onTrue(RobotMap.Component.arm.placeCones(2)); + RobotMap.HumanInput.Operator.joystick.button12.onTrue(RobotMap.Component.arm.placeCones(1)); + + //shelf intake + RobotMap.HumanInput.Operator.joystick.button6.onTrue(RobotMap.Component.arm.c_posIntakeShelf()); + + //ground intake + RobotMap.HumanInput.Operator.joystick.button4.onTrue(RobotMap.Component.arm.c_posIntakeGround()); } @Override diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index ecd9a98..d26d9cb 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -113,7 +113,7 @@ public Command placeCube(int shelf) { .alongWith(new WaitCommand(1).andThen(RobotMap.Component.intake.c_holdVoltage(Intake.DEFAULT_INTAKE_VOLTS)) )).withTimeout(5); //TODO: change timeout } - public Command c_angleCones(int shelf) { + public Command placeCones(int shelf) { var degreesFromHorizontal = cones.get(shelf).getFirst(); var extensionLengthMeters = cones.get(shelf).getSecond(); From b26aa7eaa3f8128cc737102ef91bcab6653b7ca6 Mon Sep 17 00:00:00 2001 From: aze12345 <88347612+aze12345@users.noreply.github.com> Date: Fri, 31 Mar 2023 16:45:08 -0700 Subject: [PATCH 11/63] Commiting a minor traffic law violation Co-authored-by: zbuster05 --- .../java/org/usfirst/frc4904/robot/Robot.java | 16 ++-- .../robot/subsystems/arm/ArmSubsystem.java | 79 +++---------------- 2 files changed, 21 insertions(+), 74 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 6159e25..e5c13cf 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -124,14 +124,14 @@ public void teleopInitialize() { RobotMap.HumanInput.Operator.joystick.button1.onFalse(cmdnull); // position + place cube - RobotMap.HumanInput.Operator.joystick.button7.onTrue(RobotMap.Component.arm.c_angleCubes(3)); - RobotMap.HumanInput.Operator.joystick.button9.onTrue(RobotMap.Component.arm.c_angleCubes(2)); - RobotMap.HumanInput.Operator.joystick.button11.onTrue(RobotMap.Component.arm.c_angleCubes(1)); - - //position cone - RobotMap.HumanInput.Operator.joystick.button8.onTrue(RobotMap.Component.arm.placeCones(3)); - RobotMap.HumanInput.Operator.joystick.button10.onTrue(RobotMap.Component.arm.placeCones(2)); - RobotMap.HumanInput.Operator.joystick.button12.onTrue(RobotMap.Component.arm.placeCones(1)); + RobotMap.HumanInput.Operator.joystick.button7.onTrue(RobotMap.Component.arm.c_shootCubes(3)); + RobotMap.HumanInput.Operator.joystick.button9.onTrue(RobotMap.Component.arm.c_shootCubes(2)); + RobotMap.HumanInput.Operator.joystick.button11.onTrue(RobotMap.Component.arm.c_shootCubes(1)); + + // //position cone + // RobotMap.HumanInput.Operator.joystick.button8.onTrue(RobotMap.Component.arm.placeCones(3)); + // RobotMap.HumanInput.Operator.joystick.button10.onTrue(RobotMap.Component.arm.placeCones(2)); + // RobotMap.HumanInput.Operator.joystick.button12.onTrue(RobotMap.Component.arm.placeCones(1)); //shelf intake RobotMap.HumanInput.Operator.joystick.button6.onTrue(RobotMap.Component.arm.c_posIntakeShelf()); diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index d26d9cb..54c83d8 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -53,89 +53,36 @@ public ArmSubsystem(ArmPivotSubsystem armPivotSubsystem, ArmExtensionSubsystem a this.armExtensionSubsystem = armExtensionSubsystem; } - public Command c_posReturnToHomeUp(boolean flippy) { - var cmd = c_holdArmPoseFlippy(otherPositions.get("homeUp"), NathanGain.isFlippy); + public Command c_posReturnToHomeUp() { + var cmd = c_holdArmPose(otherPositions.get("homeUp").getFirst(), otherPositions.get("homeUp").getSecond()).getFirst(); cmd.setName("arm position - home (up)"); return cmd; } - public Command c_posReturnToHomeDown(boolean flippy) { - var cmd = c_holdArmPoseFlippy(otherPositions.get("homeDown"), NathanGain.isFlippy); + public Command c_posReturnToHomeDown() { + var cmd = c_holdArmPose(otherPositions.get("homeDown").getFirst(), otherPositions.get("homeUp").getSecond()).getFirst(); cmd.setName("arm position - home (down)"); return cmd; } - public Command c_posIntakeGround() { - var cmd = c_holdArmPoseFlippy(otherPositions.get("intakeGround"), NathanGain.isFlippy); - cmd.setName("arm position - ground intake"); - return cmd; - } public Command c_posIntakeShelf() { // TODO: back up 14 inches -- remember to always use meters - var cmd = c_holdArmPoseFlippy(otherPositions.get("intakeShelf"), NathanGain.isFlippy); + var cmd = c_holdArmPose(otherPositions.get("intakeShelf").getFirst(), otherPositions.get("homeUp").getSecond()).getFirst(); cmd.setName("arm position - pre shelf intake"); return cmd; } - public Command c_angleCubes(int shelf) { + public Command c_shootCubes(int shelf) { var degreesFromHorizontal = cubes.get(shelf).getFirst(); var extensionLengthMeters = cubes.get(shelf).getSecond(); - if (NathanGain.isFlippy) { - degreesFromHorizontal = (degreesFromHorizontal * -1) + 180; - } - - var cmd = c_holdArmPose(degreesFromHorizontal, extensionLengthMeters); - cmd.setName("arm - c_angleCubes - " + shelf); - return cmd; - } - //for auton - public Command placeCube(int shelf, boolean flippy) { - double degreesFromHorizontal = cubes.get(shelf).getFirst(); - if (flippy) { - degreesFromHorizontal = (degreesFromHorizontal * -1) + 180; - } - double extensionLengthMeters= cubes.get(shelf).getSecond(); + Pair armMovement = c_holdArmPose(degreesFromHorizontal, extensionLengthMeters); - return ( - c_holdArmPose(degreesFromHorizontal, extensionLengthMeters) - .alongWith(new WaitCommand(1).andThen(RobotMap.Component.intake.c_holdVoltage(-Intake.DEFAULT_INTAKE_VOLTS)) - )).withTimeout(5); //TODO: change timeout + return armMovement.getFirst().withTimeout(armMovement.getSecond()) + .andThen(RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.5)) + .andThen(RobotMap.Component.intake.c_holdVoltage(0)) + .andThen(c_posReturnToHomeUp()); } - //for buttons - public Command placeCube(int shelf) { - double degreesFromHorizontal = cubes.get(shelf).getFirst(); - if (NathanGain.isFlippy) { - degreesFromHorizontal = (degreesFromHorizontal * -1) + 180; - } - double extensionLengthMeters= cubes.get(shelf).getSecond(); - return ( - c_holdArmPose(degreesFromHorizontal, extensionLengthMeters) - .alongWith(new WaitCommand(1).andThen(RobotMap.Component.intake.c_holdVoltage(Intake.DEFAULT_INTAKE_VOLTS)) - )).withTimeout(5); //TODO: change timeout - } - public Command placeCones(int shelf) { - var degreesFromHorizontal = cones.get(shelf).getFirst(); - var extensionLengthMeters = cones.get(shelf).getSecond(); - - if (NathanGain.isFlippy) { - degreesFromHorizontal = (degreesFromHorizontal * -1) + 180; - } - - var cmd = c_holdArmPose(degreesFromHorizontal, extensionLengthMeters); - cmd.setName("arm - c_angleCones - " + shelf); - return cmd; - } - - public Command c_holdArmPoseFlippy(Pair angleAndExtensionMeters, boolean flippy) { - var degreesFromHorizontal = angleAndExtensionMeters.getFirst(); - var extensionLengthMeters = angleAndExtensionMeters.getSecond(); - - if (flippy) degreesFromHorizontal = 180 - degreesFromHorizontal; - - return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters); - } - - public Command c_holdArmPose(double degreesFromHorizontal, double extensionLengthMeters) { + public Pair c_holdArmPose(double degreesFromHorizontal, double extensionLengthMeters) { // TODO: crashes // return this.runOnce(() -> System.out.println("TODO: hold arm pose crashes the code!")); Command firstCommand; @@ -167,6 +114,6 @@ public Command c_holdArmPose(double degreesFromHorizontal, double extensionLengt // secondCommand.schedule(); }); // long story. basically, parallel command group requires it's subcommands' requirements. however, we want one subcommand to be able to die wihle the other one lives, so we just do this instead and leak commands. it's fine because they'll get cleaned up when their atomic base subsystems gets taken over by new commands cmd.setName("arm - c_holdArmPose"); - return cmd; + return new Pair(cmd, pivotMovement.getSecond() + extensionMovement.getSecond()); } } From ead49eb1cb5f1efde18f2942eff040d5525a1d80 Mon Sep 17 00:00:00 2001 From: aze12345 <88347612+aze12345@users.noreply.github.com> Date: Fri, 31 Mar 2023 16:46:38 -0700 Subject: [PATCH 12/63] Trolling --- src/main/java/org/usfirst/frc4904/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index e5c13cf..68017b2 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -137,7 +137,7 @@ public void teleopInitialize() { RobotMap.HumanInput.Operator.joystick.button6.onTrue(RobotMap.Component.arm.c_posIntakeShelf()); //ground intake - RobotMap.HumanInput.Operator.joystick.button4.onTrue(RobotMap.Component.arm.c_posIntakeGround()); + // RobotMap.HumanInput.Operator.joystick.button4.onTrue(RobotMap.Component.arm.c_posIntakeGround()); } @Override From fd1c5281a8aafefe7b5cd3dfced0d1e0162773fd Mon Sep 17 00:00:00 2001 From: aze12345 <88347612+aze12345@users.noreply.github.com> Date: Sat, 1 Apr 2023 17:56:55 -0700 Subject: [PATCH 13/63] unsafe motors + actually compiles --- .../org/usfirst/frc4904/robot/RobotMap.java | 7 ++++ .../robot/seenoevil/RobotContainer2.java | 34 +++++++++---------- 2 files changed, 24 insertions(+), 17 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index cce0fa6..08ba6d2 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -181,9 +181,16 @@ public RobotMap() { *************************/ Component.backRightWheelTalon = new CANTalonFX(Port.CANMotor.RIGHT_DRIVE_A, InvertType.None); + Component.backRightWheelTalon.setSafetyEnabled(false); + Component.frontRightWheelTalon = new CANTalonFX(Port.CANMotor.RIGHT_DRIVE_B, InvertType.None); + Component.frontRightWheelTalon.setSafetyEnabled(false); + Component.backLeftWheelTalon = new CANTalonFX(Port.CANMotor.LEFT_DRIVE_A, InvertType.None); + Component.backLeftWheelTalon.setSafetyEnabled(false); + Component.frontLeftWheelTalon = new CANTalonFX(Port.CANMotor.LEFT_DRIVE_B, InvertType.None); + Component.frontLeftWheelTalon.setSafetyEnabled(false); TalonMotorSubsystem leftDriveMotors = new TalonMotorSubsystem("left drive motors", NeutralMode.Brake, 0, Component.frontLeftWheelTalon, Component.backLeftWheelTalon); TalonMotorSubsystem rightDriveMotors = new TalonMotorSubsystem("right drive motors", NeutralMode.Brake, 0, Component.frontRightWheelTalon, Component.backRightWheelTalon); diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java index d0bf1cd..e846c4b 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java @@ -367,24 +367,24 @@ public Command balanceAutonAndShootCube(Supplier w return command; } - public Command notBalanceAuton(){ - var command = new SequentialCommandGroup( - //1. Position arm to place gamepiece - RobotMap.Component.arm.placeCube(2, true) //TODO: set actual timeout - , - new ParallelCommandGroup( - //3. Retract arm - RobotMap.Component.arm.c_posReturnToHomeUp(false), - new SequentialCommandGroup( - new WaitCommand(1), //TODO: set wait time to allow arm to get started before moving? - //4. Drive forward past ramp - getAutonomousCommand(getTrajectory("past_ramp")) - ) - ) - ); + // public Command notBalanceAuton(){ + // var command = new SequentialCommandGroup( + // //1. Position arm to place gamepiece + // RobotMap.Component.arm.placeCube(2, true) //TODO: set actual timeout + // , + // new ParallelCommandGroup( + // //3. Retract arm + // RobotMap.Component.arm.c_posReturnToHomeUp(false), + // new SequentialCommandGroup( + // new WaitCommand(1), //TODO: set wait time to allow arm to get started before moving? + // //4. Drive forward past ramp + // getAutonomousCommand(getTrajectory("past_ramp")) + // ) + // ) + // ); - return command; - } + // return command; + // } public Command newAuton() { var command = new SequentialCommandGroup( From 31d423ea77c352f262217143e0869c7047da248a Mon Sep 17 00:00:00 2001 From: Exr0n Date: Sat, 1 Apr 2023 18:42:17 -0700 Subject: [PATCH 14/63] revert removing seenoevil c49dcb9 --- .../java/org/usfirst/frc4904/robot/Robot.java | 67 ++- .../robot/seenoevil/DriveSubsystem.java | 444 +++++++++--------- .../robot/seenoevil/RobotContainer2.java | 44 +- 3 files changed, 273 insertions(+), 282 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 68017b2..d9c7d15 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -11,7 +11,7 @@ import org.usfirst.frc4904.robot.humaninterface.drivers.NathanGain; import org.usfirst.frc4904.robot.humaninterface.operators.DefaultOperator; -// import org.usfirst.frc4904.robot.seenoevil.DriveSubsystem; +import org.usfirst.frc4904.robot.seenoevil.DriveSubsystem; import org.usfirst.frc4904.robot.seenoevil.RobotContainer2; import org.usfirst.frc4904.robot.subsystems.arm.ArmPivotSubsystem; import org.usfirst.frc4904.standard.CommandRobotBase; @@ -32,7 +32,7 @@ public class Robot extends CommandRobotBase { private final RobotMap map = new RobotMap(); - private final RobotContainer2 donttouchme = new RobotContainer2(RobotMap.Component.chassis, RobotMap.Component.navx); + private final RobotContainer2 donttouchme = new RobotContainer2(RobotMap.Component.frontLeftWheelTalon, RobotMap.Component.backLeftWheelTalon, RobotMap.Component.frontRightWheelTalon, RobotMap.Component.backRightWheelTalon, RobotMap.Component.navx); private final Driver driver = new NathanGain(); private final org.usfirst.frc4904.standard.humaninput.Operator operator = new DefaultOperator(); @@ -43,10 +43,10 @@ public void initialize() { @Override public void teleopInitialize() { - // if (RobotContainer2.Component.leftATalonFX != null) RobotContainer2.Component.leftATalonFX.setNeutralMode(NeutralMode.Brake); - // if (RobotContainer2.Component.leftBTalonFX != null) RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Brake); - // if (RobotContainer2.Component.rightATalonFX != null) RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Brake); - // if (RobotContainer2.Component.rightBTalonFX != null) RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Brake); + if (RobotContainer2.Component.leftATalonFX != null) RobotContainer2.Component.leftATalonFX.setNeutralMode(NeutralMode.Brake); + if (RobotContainer2.Component.leftBTalonFX != null) RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Brake); + if (RobotContainer2.Component.rightATalonFX != null) RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Brake); + if (RobotContainer2.Component.rightBTalonFX != null) RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Brake); RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.neutralOutput(); RobotMap.Component.arm.armExtensionSubsystem.motor.setBrakeOnNeutral(); @@ -58,10 +58,10 @@ public void teleopInitialize() { *************************/ // SATURDAY MORNING TEST - can you run drive train in queueline - // donttouchme.m_robotDrive.m_leftMotors = null; - // donttouchme.m_robotDrive.m_rightMotors = null; - // donttouchme.m_robotDrive.m_drive = null; - // DriveSubsystem.skuffedaf_teleop_initialized = true; + donttouchme.m_robotDrive.m_leftMotors = null; + donttouchme.m_robotDrive.m_rightMotors = null; + donttouchme.m_robotDrive.m_drive = null; + DriveSubsystem.skuffedaf_teleop_initialized = true; // donttouchme.m_robotDrive.m_leftEncoder = null; @@ -157,20 +157,17 @@ public void teleopExecute() { SmartDashboard.putNumber("arm extension length", RobotMap.Component.arm.armExtensionSubsystem.getCurrentExtensionLength()); SmartDashboard.putNumber("arm pivot angle", RobotMap.Component.arm.armPivotSubsystem.getCurrentAngleDegrees()); - // SmartDashboard.putNumber("Falcon temp", RobotContainer2.Component.leftATalonFX.getTemperature()); + SmartDashboard.putNumber("Falcon temp", RobotContainer2.Component.leftATalonFX.getTemperature()); } @Override public void autonomousInitialize() { - RobotMap.Component.chassis.leftMotors.coast(); - RobotMap.Component.chassis.rightMotors.coast(); - - // if (RobotContainer2.Component.leftATalonFX != null) RobotContainer2.Component.leftATalonFX.setNeutralMode(NeutralMode.Brake); - // if (RobotContainer2.Component.leftBTalonFX != null) RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Brake); - // if (RobotContainer2.Component.rightATalonFX != null) RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Brake); - // if (RobotContainer2.Component.rightBTalonFX != null) RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Brake); + if (RobotContainer2.Component.leftATalonFX != null) RobotContainer2.Component.leftATalonFX.setNeutralMode(NeutralMode.Brake); + if (RobotContainer2.Component.leftBTalonFX != null) RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Brake); + if (RobotContainer2.Component.rightATalonFX != null) RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Brake); + if (RobotContainer2.Component.rightBTalonFX != null) RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Brake); // if (RobotContainer2.Component.leftATalonFX != null) { RobotContainer2.Component.leftATalonFX.setNeutralMode(NeutralMode.Coast); RobotContainer2.Component.leftATalonFX.neutralOutput(); } @@ -191,8 +188,8 @@ public void autonomousInitialize() { // SATURDAY MORNING TEST: is the cube shooter auton gonna work // var commnand = donttouchme.balanceAutonAndShootCube(donttouchme.m_robotDrive::getWheelSpeeds, donttouchme.m_robotDrive::tankDriveVolts); - // var commnand = donttouchme.getAutonomousCommand(donttouchme.getTrajectory("straight_forward")); - // commnand.schedule(); + var commnand = donttouchme.getAutonomousCommand(donttouchme.getTrajectory("straight_forward")); + commnand.schedule(); } @Override @@ -201,29 +198,25 @@ public void autonomousExecute() { SmartDashboard.putBoolean("isFlipped - IMPORTANT", NathanGain.isFlippy); - // SmartDashboard.putString("pose string", donttouchme.m_robotDrive.getPose().toString()); - // SmartDashboard.putNumber("pose x", donttouchme.m_robotDrive.getPose().getX()); - // SmartDashboard.putNumber("pose y", donttouchme.m_robotDrive.getPose().getY()); - // SmartDashboard.putNumber("pose heading", donttouchme.m_robotDrive.getPose().getRotation().getDegrees()); - - SmartDashboard.putString("pose string", RobotMap.Component.chassis.getPoseMeters().toString()); - SmartDashboard.putNumber("pose x", RobotMap.Component.chassis.getPoseMeters().getX()); - SmartDashboard.putNumber("pose y", RobotMap.Component.chassis.getPoseMeters().getY()); - SmartDashboard.putNumber("pose heading", RobotMap.Component.chassis.getPoseMeters().getRotation().getDegrees()); + SmartDashboard.putString("pose string", donttouchme.m_robotDrive.getPose().toString()); + SmartDashboard.putNumber("pose x", donttouchme.m_robotDrive.getPose().getX()); + SmartDashboard.putNumber("pose y", donttouchme.m_robotDrive.getPose().getY()); + SmartDashboard.putNumber("pose heading", donttouchme.m_robotDrive.getPose().getRotation().getDegrees()); SmartDashboard.putNumber("gyroooo", RobotMap.Component.navx.getAngle()); SmartDashboard.putNumber("armV extension length", RobotMap.Component.arm.armExtensionSubsystem.getCurrentExtensionLength()); SmartDashboard.putNumber("arm pivot angle", RobotMap.Component.arm.armPivotSubsystem.getCurrentAngleDegrees()); + SmartDashboard.putNumber("Falcon temp", RobotContainer2.Component.leftATalonFX.getTemperature()); } @Override public void disabledInitialize() { - // if (RobotContainer2.Component.leftATalonFX != null) RobotContainer2.Component.leftATalonFX.setNeutralMode(NeutralMode.Brake); - // if (RobotContainer2.Component.leftBTalonFX != null) RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Brake); - // if (RobotContainer2.Component.rightATalonFX != null) RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Brake); - // if (RobotContainer2.Component.rightBTalonFX != null) RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Brake); + if (RobotContainer2.Component.leftATalonFX != null) RobotContainer2.Component.leftATalonFX.setNeutralMode(NeutralMode.Brake); + if (RobotContainer2.Component.leftBTalonFX != null) RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Brake); + if (RobotContainer2.Component.rightATalonFX != null) RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Brake); + if (RobotContainer2.Component.rightBTalonFX != null) RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Brake); RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.neutralOutput(); RobotMap.Component.arm.armExtensionSubsystem.motor.setBrakeOnNeutral(); @@ -240,10 +233,10 @@ public void disabledExecute() { @Override public void testInitialize() { - // if (RobotContainer2.Component.leftATalonFX != null) { RobotContainer2.Component.leftATalonFX.setNeutralMode(NeutralMode.Coast); RobotContainer2.Component.leftATalonFX.neutralOutput(); } - // if (RobotContainer2.Component.leftBTalonFX != null) { RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Coast); RobotContainer2.Component.leftBTalonFX.neutralOutput(); } - // if (RobotContainer2.Component.rightATalonFX != null) { RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Coast); RobotContainer2.Component.rightATalonFX.neutralOutput(); } - // if (RobotContainer2.Component.rightBTalonFX != null) { RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Coast); RobotContainer2.Component.rightBTalonFX.neutralOutput(); } + if (RobotContainer2.Component.leftATalonFX != null) { RobotContainer2.Component.leftATalonFX.setNeutralMode(NeutralMode.Coast); RobotContainer2.Component.leftATalonFX.neutralOutput(); } + if (RobotContainer2.Component.leftBTalonFX != null) { RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Coast); RobotContainer2.Component.leftBTalonFX.neutralOutput(); } + if (RobotContainer2.Component.rightATalonFX != null) { RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Coast); RobotContainer2.Component.rightATalonFX.neutralOutput(); } + if (RobotContainer2.Component.rightBTalonFX != null) { RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Coast); RobotContainer2.Component.rightBTalonFX.neutralOutput(); } RobotMap.Component.arm.armPivotSubsystem.initializeEncoderPositions(); RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.setCoastOnNeutral(); RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.neutralOutput(); diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/DriveSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/DriveSubsystem.java index 272d4f9..cfc5143 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/DriveSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/DriveSubsystem.java @@ -1,225 +1,225 @@ -// // Copyright (c) FIRST and other WPILib contributors. -// // Open Source Software; you can modify and/or share it under the terms of -// // the WPILib BSD license file in the root directory of this project. - -// package org.usfirst.frc4904.robot.seenoevil; - - -// import com.kauailabs.navx.frc.AHRS; -// import edu.wpi.first.math.geometry.Pose2d; -// import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; -// import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; -// import edu.wpi.first.wpilibj.CAN; -// import edu.wpi.first.wpilibj.Encoder; -// import edu.wpi.first.wpilibj.I2C; -// import edu.wpi.first.wpilibj.drive.DifferentialDrive; -// import edu.wpi.first.wpilibj.interfaces.Gyro; -// import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup; -// import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; -// import edu.wpi.first.wpilibj.motorcontrol.Talon; -// import edu.wpi.first.wpilibj2.command.SubsystemBase; -// import edu.wpi.first.wpilibj.SerialPort; - -// import org.usfirst.frc4904.robot.seenoevil.Constants.DriveConstants; - -// import com.ctre.phoenix.motorcontrol.FeedbackDevice; -// import com.ctre.phoenix.motorcontrol.NeutralMode; -// import com.ctre.phoenix.motorcontrol.can.TalonFX; -// import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -// import edu.wpi.first.wpilibj.motorcontrol.MotorController; - - -// public class DriveSubsystem extends SubsystemBase { -// public static boolean skuffedaf_teleop_initialized = false; -// // The motors on the left side of the drive. -// public MotorControllerGroup m_leftMotors = //motors need to be talons -// new MotorControllerGroup( -// RobotContainer2.Component.leftATalonFX, -// RobotContainer2.Component.leftBTalonFX); - -// // The motors on the right side of the drive. -// public MotorControllerGroup m_rightMotors = -// new MotorControllerGroup( -// RobotContainer2.Component.rightATalonFX, -// RobotContainer2.Component.rightBTalonFX); - -// // The robot's drive -// public DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors); - -// // The left-side drive encoder -// // private final ManualTalonEncoderLmao m_leftEncoder = -// // new ManualTalonEncoderLmao( -// // RobotContainer.Component.leftATalonFX, 1); - -// // // The right-side drive encoder -// // private final ManualTalonEncoderLmao m_rightEncoder = -// // new ManualTalonEncoderLmao( -// // RobotContainer.Component.rightATalonFX, 2); +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.usfirst.frc4904.robot.seenoevil; + + +import com.kauailabs.navx.frc.AHRS; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; +import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.wpilibj.CAN; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.I2C; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.interfaces.Gyro; +import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup; +import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import edu.wpi.first.wpilibj.motorcontrol.Talon; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj.SerialPort; + +import org.usfirst.frc4904.robot.seenoevil.Constants.DriveConstants; + +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.TalonFX; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import edu.wpi.first.wpilibj.motorcontrol.MotorController; + + +public class DriveSubsystem extends SubsystemBase { + public static boolean skuffedaf_teleop_initialized = false; + // The motors on the left side of the drive. + public MotorControllerGroup m_leftMotors = //motors need to be talons + new MotorControllerGroup( + RobotContainer2.Component.leftATalonFX, + RobotContainer2.Component.leftBTalonFX); + + // The motors on the right side of the drive. + public MotorControllerGroup m_rightMotors = + new MotorControllerGroup( + RobotContainer2.Component.rightATalonFX, + RobotContainer2.Component.rightBTalonFX); + + // The robot's drive + public DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors); + + // The left-side drive encoder + // private final ManualTalonEncoderLmao m_leftEncoder = + // new ManualTalonEncoderLmao( + // RobotContainer.Component.leftATalonFX, 1); + + // // The right-side drive encoder + // private final ManualTalonEncoderLmao m_rightEncoder = + // new ManualTalonEncoderLmao( + // RobotContainer.Component.rightATalonFX, 2); -// public Imblueeeeeee m_leftEncoder = -// new Imblueeeeeee( -// "leftWheel", RobotContainer2.Component.leftATalonFX, true, DriveConstants.kEncoderDistancePerPulse, FeedbackDevice.IntegratedSensor); - -// // The right-side drive encoder -// public Imblueeeeeee m_rightEncoder = -// new Imblueeeeeee( -// "leftWheel", RobotContainer2.Component.rightATalonFX, false, DriveConstants.kEncoderDistancePerPulse, FeedbackDevice.IntegratedSensor); - -// // The gyro sensor -// public final AHRS m_gyro; - - -// // Odometry class for tracking robot pose -// private final DifferentialDriveOdometry m_odometry; - -// /** Creates a new DriveSubsystem. */ -// public DriveSubsystem(AHRS navx) { -// m_gyro = navx; -// // We need to invert one side of the drivetrain so that positive voltages -// // result in both sides moving forward. Depending on how your robot's -// // gearbox is constructed, you might have to invert the left side instead. -// m_leftMotors.setInverted(true); -// // Sets the distance per pulse for the encoders -// m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); -// m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); - -// resetEncoders(); -// m_odometry = -// new DifferentialDriveOdometry( -// m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance()); -// } - -// @Override -// public void periodic() { -// // Update the odometry in the periodic block -// if (!skuffedaf_teleop_initialized) { -// m_odometry.update( -// m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance()); -// } -// // RobotContainer2.Component.leftATalonFX.feed(); -// // RobotContainer2.Component.leftBTalonFX.feed(); - -// // RobotContainer2.Component.rightATalonFX.feed(); -// // RobotContainer2.Component.rightBTalonFX.feed(); +public Imblueeeeeee m_leftEncoder = + new Imblueeeeeee( + "leftWheel", RobotContainer2.Component.leftATalonFX, true, DriveConstants.kEncoderDistancePerPulse, FeedbackDevice.IntegratedSensor); + +// The right-side drive encoder +public Imblueeeeeee m_rightEncoder = + new Imblueeeeeee( + "leftWheel", RobotContainer2.Component.rightATalonFX, false, DriveConstants.kEncoderDistancePerPulse, FeedbackDevice.IntegratedSensor); + + // The gyro sensor + public final AHRS m_gyro; + + + // Odometry class for tracking robot pose + private final DifferentialDriveOdometry m_odometry; + + /** Creates a new DriveSubsystem. */ + public DriveSubsystem(AHRS navx) { + m_gyro = navx; + // We need to invert one side of the drivetrain so that positive voltages + // result in both sides moving forward. Depending on how your robot's + // gearbox is constructed, you might have to invert the left side instead. + m_leftMotors.setInverted(true); + // Sets the distance per pulse for the encoders + m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); + m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); + + resetEncoders(); + m_odometry = + new DifferentialDriveOdometry( + m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance()); + } + + @Override + public void periodic() { + // Update the odometry in the periodic block + if (!skuffedaf_teleop_initialized) { + m_odometry.update( + m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance()); + } + // RobotContainer2.Component.leftATalonFX.feed(); + // RobotContainer2.Component.leftBTalonFX.feed(); + + // RobotContainer2.Component.rightATalonFX.feed(); + // RobotContainer2.Component.rightBTalonFX.feed(); -// // m_leftEncoder.debug(); -// // m_rightEncoder.debug(); -// } - -// /** -// * Returns the currently-estimated pose of the robot. -// * -// * @return The pose. -// */ -// public Pose2d getPose() { -// return m_odometry.getPoseMeters(); -// } - -// /** -// * Returns the current wheel speeds of the robot. -// * -// * @return The current wheel speeds. -// */ -// public DifferentialDriveWheelSpeeds getWheelSpeeds() { -// return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate()); -// } - -// /** -// * Resets the odometry to the specified pose. -// * -// * @param pose The pose to which to set the odometry. -// */ -// public void resetOdometry(Pose2d pose) { -// resetEncoders(); -// m_odometry.resetPosition( -// m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance(), pose); -// } - -// /** -// * Drives the robot using arcade controls. -// * -// * @param fwd the commanded forward movement -// * @param rot the commanded rotation -// */ -// public void arcadeDrive(double fwd, double rot) { -// m_drive.arcadeDrive(fwd, rot); -// } - -// /** -// * Controls the left and right sides of the drive directly with voltages. -// * -// * @param leftVolts the commanded left output -// * @param rightVolts the commanded right output -// */ -// public void tankDriveVolts(double leftVolts, double rightVolts) { -// m_leftMotors.setVoltage(leftVolts); -// m_rightMotors.setVoltage(rightVolts); -// m_drive.feed(); -// } - -// /** Resets the drive encoders to currently read a position of 0. */ -// public void resetEncoders() { -// m_leftEncoder.reset(); -// m_rightEncoder.reset(); -// } - -// /** -// * Gets the average distance of the two encoders. -// * -// * @return the average of the two encoder readings -// */ -// public double getAverageEncoderDistance() { -// return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0; -// } - -// /** -// * Gets the left drive encoder. -// * -// * @return the left drive encoder -// */ -// public Imblueeeeeee getLeftEncoder() { -// return m_leftEncoder; -// } - -// /** -// * Gets the right drive encoder. -// *c -// * @return the right drive encoder -// */ -// public Imblueeeeeee getRightEncoder() { -// return m_rightEncoder; -// } - -// /** -// * Sets the max output of the drive. Useful for scaling the drive to drive more slowly. -// * -// * @param maxOutput the maximum output to which the drive will be constrained -// */ -// public void setMaxOutput(double maxOutput) { -// m_drive.setMaxOutput(maxOutput); -// } - -// /** Zeroes the heading of the robot. */ -// public void zeroHeading() { -// m_gyro.reset(); -// } - -// /** -// * Returns the heading of the robot. -// * -// * @return the robot's heading in degrees, from -180 to 180 -// */ -// public double getHeading() { -// return m_gyro.getRotation2d().getDegrees(); -// } - -// /** -// * Returns the turn rate of the robot. -// * -// * @return The turn rate of the robot, in degrees per second -// */ -// public double getTurnRate() { -// return -m_gyro.getRate(); -// } -// } + // m_leftEncoder.debug(); + // m_rightEncoder.debug(); + } + + /** + * Returns the currently-estimated pose of the robot. + * + * @return The pose. + */ + public Pose2d getPose() { + return m_odometry.getPoseMeters(); + } + + /** + * Returns the current wheel speeds of the robot. + * + * @return The current wheel speeds. + */ + public DifferentialDriveWheelSpeeds getWheelSpeeds() { + return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate()); + } + + /** + * Resets the odometry to the specified pose. + * + * @param pose The pose to which to set the odometry. + */ + public void resetOdometry(Pose2d pose) { + resetEncoders(); + m_odometry.resetPosition( + m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance(), pose); + } + + /** + * Drives the robot using arcade controls. + * + * @param fwd the commanded forward movement + * @param rot the commanded rotation + */ + public void arcadeDrive(double fwd, double rot) { + m_drive.arcadeDrive(fwd, rot); + } + + /** + * Controls the left and right sides of the drive directly with voltages. + * + * @param leftVolts the commanded left output + * @param rightVolts the commanded right output + */ + public void tankDriveVolts(double leftVolts, double rightVolts) { + m_leftMotors.setVoltage(leftVolts); + m_rightMotors.setVoltage(rightVolts); + m_drive.feed(); + } + + /** Resets the drive encoders to currently read a position of 0. */ + public void resetEncoders() { + m_leftEncoder.reset(); + m_rightEncoder.reset(); + } + + /** + * Gets the average distance of the two encoders. + * + * @return the average of the two encoder readings + */ + public double getAverageEncoderDistance() { + return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0; + } + + /** + * Gets the left drive encoder. + * + * @return the left drive encoder + */ + public Imblueeeeeee getLeftEncoder() { + return m_leftEncoder; + } + + /** + * Gets the right drive encoder. + *c + * @return the right drive encoder + */ + public Imblueeeeeee getRightEncoder() { + return m_rightEncoder; + } + + /** + * Sets the max output of the drive. Useful for scaling the drive to drive more slowly. + * + * @param maxOutput the maximum output to which the drive will be constrained + */ + public void setMaxOutput(double maxOutput) { + m_drive.setMaxOutput(maxOutput); + } + + /** Zeroes the heading of the robot. */ + public void zeroHeading() { + m_gyro.reset(); + } + + /** + * Returns the heading of the robot. + * + * @return the robot's heading in degrees, from -180 to 180 + */ + public double getHeading() { + return m_gyro.getRotation2d().getDegrees(); + } + + /** + * Returns the turn rate of the robot. + * + * @return The turn rate of the robot, in degrees per second + */ + public double getTurnRate() { + return -m_gyro.getRate(); + } +} diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java index e846c4b..f9f2df2 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java @@ -8,7 +8,6 @@ import static edu.wpi.first.wpilibj.XboxController.Button; -import edu.wpi.first.math.controller.DifferentialDriveWheelVoltages; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.RamseteController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -43,7 +42,6 @@ import org.usfirst.frc4904.robot.seenoevil.Constants.AutoConstants; import org.usfirst.frc4904.robot.seenoevil.Constants.DriveConstants; import org.usfirst.frc4904.standard.custom.sensors.NavX; -import org.usfirst.frc4904.standard.subsystems.chassis.WestCoastDrive; import org.usfirst.frc4904.robot.Robot; import org.usfirst.frc4904.robot.RobotMap; import org.usfirst.frc4904.robot.seenoevil.Balance; @@ -193,44 +191,44 @@ public class RobotContainer2 { ); public static class Component { - // public static WPI_TalonFX leftATalonFX; - // public static WPI_TalonFX leftBTalonFX; - // public static WPI_TalonFX rightATalonFX; - // public static WPI_TalonFX rightBTalonFX; - // public static WPI_TalonFX testTalon; + public static WPI_TalonFX leftATalonFX; + public static WPI_TalonFX leftBTalonFX; + public static WPI_TalonFX rightATalonFX; + public static WPI_TalonFX rightBTalonFX; + public static WPI_TalonFX testTalon; } // The robot's subsystems // motors - public final WestCoastDrive m_robotDrive; + public final DriveSubsystem m_robotDrive; // public final XboxController m_driverController; /** * The container for the robot. Contains subsystems, OI devices, and commands. */ - public RobotContainer2(WestCoastDrive drive, AHRS navx) { + public RobotContainer2(WPI_TalonFX leftATalonFX, WPI_TalonFX leftBTalonFX, WPI_TalonFX rightATalonFX, WPI_TalonFX rightBTalonFX, AHRS navx) { // The driver's controller // m_driverController = new XboxController(OIConstants.kDriverControllerPort); // Configure the button bindings - // Component.leftATalonFX = leftATalonFX; - // Component.leftBTalonFX = leftBTalonFX; - // Component.rightATalonFX = rightATalonFX; - // Component.rightBTalonFX = rightBTalonFX; + Component.leftATalonFX = leftATalonFX; + Component.leftBTalonFX = leftBTalonFX; + Component.rightATalonFX = rightATalonFX; + Component.rightBTalonFX = rightBTalonFX; - // RobotContainer2.Component.leftATalonFX.setNeutralMode(NeutralMode.Coast); - // RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Coast); - // RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Coast); - // RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Coast); + RobotContainer2.Component.leftATalonFX.setNeutralMode(NeutralMode.Coast); + RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Coast); + RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Coast); + RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Coast); - // Component.testTalon = new WPI_TalonFX(1); + Component.testTalon = new WPI_TalonFX(1); // Component.leftATalonFX.setInverted(true); // Component.leftBTalonFX.setInverted(true); - this.m_robotDrive = drive; + this.m_robotDrive = new DriveSubsystem(navx); // Configure default commands // Set the default drive command to split-stick arcade drive @@ -248,7 +246,7 @@ public Command getAutonomousCommand(Trajectory trajectory) { EEEE.setEnabled(false); RamseteCommand ramseteCommand = new RamseteCommand( trajectory, - m_robotDrive::getPoseMeters, + m_robotDrive::getPose, EEEE, new SimpleMotorFeedforward( DriveConstants.ksVolts, @@ -259,7 +257,7 @@ public Command getAutonomousCommand(Trajectory trajectory) { // new PIDController(DriveConstants.kPDriveVel, 0, 0), new PIDController(DriveConstants.kPDriveVel, 0, 0), new PIDController(0, 0, 0), new PIDController(0, 0, 0), // RamseteCommand passes volts to the callback - m_robotDrive::setWheelVoltages, + m_robotDrive::tankDriveVolts, m_robotDrive); // Reset odometry to the starting pose of the trajectory. @@ -270,9 +268,9 @@ public Command getAutonomousCommand(Trajectory trajectory) { // return Commands.run(() -> m_robotDrive.tankDriveVolts(1, 1), m_robotDrive); //return Commands.runOnce(() -> m_robotDrive.arcadeDrive(0.5, 0), m_robotDrive); //return Commands.runOnce(() -> Component.testTalon.setVoltage(6)); - return Commands.runOnce(() -> m_robotDrive.resetPoseMeters(trajectory.getInitialPose()) + return Commands.runOnce(() -> m_robotDrive.resetOdometry(trajectory.getInitialPose()) ) .andThen( ramseteCommand) - .andThen(() -> m_robotDrive.setWheelVoltages(0, 0)); + .andThen(() -> m_robotDrive.tankDriveVolts(0, 0)); } /** From d300dadfccbf8aad4226ded7932ea02dc7cb3726 Mon Sep 17 00:00:00 2001 From: zbuster05 Date: Sat, 1 Apr 2023 19:06:01 -0700 Subject: [PATCH 15/63] Triplet stuff etc --- .../java/org/usfirst/frc4904/robot/Robot.java | 2 +- .../robot/seenoevil/RobotContainer2.java | 34 +++++++++---------- .../robot/subsystems/arm/ArmSubsystem.java | 26 +++++++++----- src/main/java/org/usfirst/frc4904/standard | 2 +- 4 files changed, 36 insertions(+), 28 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index f6dab45..0fde362 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -139,7 +139,7 @@ public void teleopInitialize() { RobotMap.HumanInput.Operator.joystick.button6.onTrue(RobotMap.Component.arm.c_posIntakeShelf()); //ground intake - RobotMap.HumanInput.Operator.joystick.button4.onTrue(RobotMap.Component.arm.c_posIntakeGround()); + // RobotMap.HumanInput.Operator.joystick.button4.onTrue(RobotMap.Component.arm.c_posIntakeGround()); } @Override diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java index d0bf1cd..e846c4b 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java @@ -367,24 +367,24 @@ public Command balanceAutonAndShootCube(Supplier w return command; } - public Command notBalanceAuton(){ - var command = new SequentialCommandGroup( - //1. Position arm to place gamepiece - RobotMap.Component.arm.placeCube(2, true) //TODO: set actual timeout - , - new ParallelCommandGroup( - //3. Retract arm - RobotMap.Component.arm.c_posReturnToHomeUp(false), - new SequentialCommandGroup( - new WaitCommand(1), //TODO: set wait time to allow arm to get started before moving? - //4. Drive forward past ramp - getAutonomousCommand(getTrajectory("past_ramp")) - ) - ) - ); + // public Command notBalanceAuton(){ + // var command = new SequentialCommandGroup( + // //1. Position arm to place gamepiece + // RobotMap.Component.arm.placeCube(2, true) //TODO: set actual timeout + // , + // new ParallelCommandGroup( + // //3. Retract arm + // RobotMap.Component.arm.c_posReturnToHomeUp(false), + // new SequentialCommandGroup( + // new WaitCommand(1), //TODO: set wait time to allow arm to get started before moving? + // //4. Drive forward past ramp + // getAutonomousCommand(getTrajectory("past_ramp")) + // ) + // ) + // ); - return command; - } + // return command; + // } public Command newAuton() { var command = new SequentialCommandGroup( diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index 54c83d8..3867ac3 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -1,9 +1,12 @@ package org.usfirst.frc4904.robot.subsystems.arm; +import java.util.ArrayList; import java.util.HashMap; +import java.util.List; import org.usfirst.frc4904.robot.RobotMap; import org.usfirst.frc4904.robot.subsystems.Intake; +import org.usfirst.frc4904.standard.custom.Triple; import org.usfirst.frc4904.robot.humaninterface.drivers.NathanGain; import edu.wpi.first.math.Pair; @@ -13,6 +16,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.WaitCommand; +import org.opencv.core.MatOfDouble; public class ArmSubsystem extends SubsystemBase { @@ -26,17 +30,18 @@ public class ArmSubsystem extends SubsystemBase { public static final double MAX_ACCEL_PIVOT = 200; - public static final HashMap> cones = new HashMap<>(); //in degrees, meters + public static final HashMap> cones = new HashMap<>(); //in degrees, meters static { - cones.put(1, new Pair<>(-19.,Units.inchesToMeters(0.))); - cones.put(2, new Pair<>(29.,Units.inchesToMeters(18.))); - cones.put(3, new Pair<>(31.,Units.inchesToMeters(38.))); + cones.put(1, new Triple<>(-19., Units.inchesToMeters(0), 3.)); + cones.put(2, new Triple<>(29., Units.inchesToMeters(18), 3.)); + cones.put(3, new Triple<>(31., Units.inchesToMeters(38.), 3.)); } - public static final HashMap> cubes = new HashMap>(); //in degrees, meters + + public static final HashMap> cubes = new HashMap<>(); //in degrees, meters static { - cubes.put(1, new Pair<>(-33.,Units.inchesToMeters(0.))); - cubes.put(2, new Pair<>(14.,Units.inchesToMeters(6.))); - cubes.put(3, new Pair<>(22.,Units.inchesToMeters(28.))); + cubes.put(1, new Triple<>(-33., Units.inchesToMeters(0), 3.)); + cubes.put(2, new Triple<>(14., Units.inchesToMeters(6), 4.)); + cubes.put(3, new Triple<>(22., Units.inchesToMeters(28.), 4.5)); } public static final HashMap> otherPositions = new HashMap<>(); @@ -69,6 +74,10 @@ public Command c_posIntakeShelf() { cmd.setName("arm position - pre shelf intake"); return cmd; } + + public Command c_angleCubes(int shelf) { + + } public Command c_shootCubes(int shelf) { var degreesFromHorizontal = cubes.get(shelf).getFirst(); @@ -83,7 +92,6 @@ public Command c_shootCubes(int shelf) { } public Pair c_holdArmPose(double degreesFromHorizontal, double extensionLengthMeters) { - // TODO: crashes // return this.runOnce(() -> System.out.println("TODO: hold arm pose crashes the code!")); Command firstCommand; Command secondCommand; diff --git a/src/main/java/org/usfirst/frc4904/standard b/src/main/java/org/usfirst/frc4904/standard index 5173578..f8afa81 160000 --- a/src/main/java/org/usfirst/frc4904/standard +++ b/src/main/java/org/usfirst/frc4904/standard @@ -1 +1 @@ -Subproject commit 51735784ae2cc3ee6f420282d516eea222492983 +Subproject commit f8afa8106e399f23d923d7278cc2b4ab7c740c5d From 1333dd4c61f352eabb62888d813bf1ac341cdc31 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Sat, 1 Apr 2023 19:09:52 -0700 Subject: [PATCH 16/63] Uncommited --- .../usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index 3867ac3..469e4ad 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -75,8 +75,11 @@ public Command c_posIntakeShelf() { return cmd; } - public Command c_angleCubes(int shelf) { + public Pair c_angleCubes(int shelf) { + var degreesFromHorizontal = cubes.get(shelf).getFirst(); + var extensionLengthMeters = cubes.get(shelf).getSecond(); + return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters); } public Command c_shootCubes(int shelf) { From 325556aa856594e2ac1623f2a25ac2d46f2506e8 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Sat, 1 Apr 2023 19:18:49 -0700 Subject: [PATCH 17/63] undo motor saftey (revert fd1c528) --- src/main/java/org/usfirst/frc4904/robot/RobotMap.java | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index 08ba6d2..fafa401 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -181,15 +181,13 @@ public RobotMap() { *************************/ Component.backRightWheelTalon = new CANTalonFX(Port.CANMotor.RIGHT_DRIVE_A, InvertType.None); - Component.backRightWheelTalon.setSafetyEnabled(false); - Component.frontRightWheelTalon = new CANTalonFX(Port.CANMotor.RIGHT_DRIVE_B, InvertType.None); - Component.frontRightWheelTalon.setSafetyEnabled(false); - Component.backLeftWheelTalon = new CANTalonFX(Port.CANMotor.LEFT_DRIVE_A, InvertType.None); - Component.backLeftWheelTalon.setSafetyEnabled(false); - Component.frontLeftWheelTalon = new CANTalonFX(Port.CANMotor.LEFT_DRIVE_B, InvertType.None); + + Component.backRightWheelTalon.setSafetyEnabled(false); + Component.frontRightWheelTalon.setSafetyEnabled(false); + Component.backLeftWheelTalon.setSafetyEnabled(false); Component.frontLeftWheelTalon.setSafetyEnabled(false); TalonMotorSubsystem leftDriveMotors = new TalonMotorSubsystem("left drive motors", NeutralMode.Brake, 0, Component.frontLeftWheelTalon, Component.backLeftWheelTalon); From 0b0a196ef7b1589f8d32842ba1b23eb7d2800250 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Sat, 1 Apr 2023 19:34:40 -0700 Subject: [PATCH 18/63] Added angle cones and shoot cones command --- .../robot/subsystems/arm/ArmSubsystem.java | 23 ++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index 469e4ad..cfc7c56 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -74,6 +74,26 @@ public Command c_posIntakeShelf() { cmd.setName("arm position - pre shelf intake"); return cmd; } + + public Pair c_angleCones(int shelf) { + var degreesFromHorizontal = cones.get(shelf).getFirst(); + var extensionLengthMeters = cones.get(shelf).getSecond(); + + return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters); + } + + public Command c_shootCones(int shelf) { + var degreesFromHorizontal = cones.get(shelf).getFirst(); + var extensionLengthMeters = cones.get(shelf).getSecond(); + var voltage = cones.get(shelf).getThird(); + + Pair armMovement = c_holdArmPose(degreesFromHorizontal, extensionLengthMeters); + + return armMovement.getFirst().withTimeout(armMovement.getSecond()) + .andThen(RobotMap.Component.intake.c_holdVoltage(voltage).withTimeout(0.5)) + .andThen(RobotMap.Component.intake.c_holdVoltage(0)) + .andThen(c_posReturnToHomeUp()); + } public Pair c_angleCubes(int shelf) { var degreesFromHorizontal = cubes.get(shelf).getFirst(); @@ -85,11 +105,12 @@ public Pair c_angleCubes(int shelf) { public Command c_shootCubes(int shelf) { var degreesFromHorizontal = cubes.get(shelf).getFirst(); var extensionLengthMeters = cubes.get(shelf).getSecond(); + var voltage = cubes.get(shelf).getThird(); Pair armMovement = c_holdArmPose(degreesFromHorizontal, extensionLengthMeters); return armMovement.getFirst().withTimeout(armMovement.getSecond()) - .andThen(RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.5)) + .andThen(RobotMap.Component.intake.c_holdVoltage(voltage).withTimeout(0.5)) .andThen(RobotMap.Component.intake.c_holdVoltage(0)) .andThen(c_posReturnToHomeUp()); } From 594aed14cdf89f834452338b8a72b899cc579ce3 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Sat, 1 Apr 2023 20:20:46 -0700 Subject: [PATCH 19/63] remove motorsaftey disable --- src/main/java/org/usfirst/frc4904/robot/Robot.java | 5 ----- src/main/java/org/usfirst/frc4904/robot/RobotMap.java | 8 ++++---- .../frc4904/robot/subsystems/arm/ArmSubsystem.java | 3 --- 3 files changed, 4 insertions(+), 12 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index b2ec650..554f6cd 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -259,11 +259,6 @@ public void testExecute() { public void alwaysExecute() { // SmartDashboard.putNumber("Arm angle", RobotMap.Component.arm.armPivotSubsystem.getCurrentAngleDegrees()); // SmartDashboard.putNumber("gyroooo", RobotMap.Component.navx.getAngle()); - SmartDashboard.putNumber("joystick left T", RobotMap.HumanInput.Driver.xbox.getLeftTriggerAxis()); - SmartDashboard.putNumber("joystick right T", RobotMap.HumanInput.Driver.xbox.getRightTriggerAxis()); - - - } } diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index df14933..1cb5604 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -185,10 +185,10 @@ public RobotMap() { Component.backLeftWheelTalon = new CANTalonFX(Port.CANMotor.LEFT_DRIVE_A, InvertType.None); Component.frontLeftWheelTalon = new CANTalonFX(Port.CANMotor.LEFT_DRIVE_B, InvertType.None); - Component.backRightWheelTalon.setSafetyEnabled(false); - Component.frontRightWheelTalon.setSafetyEnabled(false); - Component.backLeftWheelTalon.setSafetyEnabled(false); - Component.frontLeftWheelTalon.setSafetyEnabled(false); + // Component.backRightWheelTalon.setSafetyEnabled(false); + // Component.frontRightWheelTalon.setSafetyEnabled(false); + // Component.backLeftWheelTalon.setSafetyEnabled(false); + // Component.frontLeftWheelTalon.setSafetyEnabled(false); TalonMotorSubsystem leftDriveMotors = new TalonMotorSubsystem("left drive motors", NeutralMode.Brake, 0, Component.frontLeftWheelTalon, Component.backLeftWheelTalon); TalonMotorSubsystem rightDriveMotors = new TalonMotorSubsystem("right drive motors", NeutralMode.Brake, 0, Component.frontRightWheelTalon, Component.backRightWheelTalon); diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index 469e4ad..2c0a204 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -1,8 +1,6 @@ package org.usfirst.frc4904.robot.subsystems.arm; -import java.util.ArrayList; import java.util.HashMap; -import java.util.List; import org.usfirst.frc4904.robot.RobotMap; import org.usfirst.frc4904.robot.subsystems.Intake; @@ -16,7 +14,6 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.WaitCommand; -import org.opencv.core.MatOfDouble; public class ArmSubsystem extends SubsystemBase { From cc66125117c57726b89f197cb0ba5f2fd946fbcd Mon Sep 17 00:00:00 2001 From: zbuster05 Date: Sat, 1 Apr 2023 21:27:05 -0700 Subject: [PATCH 20/63] Fixed kerchunk to use the technical term --- src/main/java/org/usfirst/frc4904/robot/Robot.java | 12 +++++------- .../java/org/usfirst/frc4904/robot/RobotMap.java | 1 + .../robot/humaninterface/drivers/NathanGain.java | 4 ++-- .../frc4904/robot/seenoevil/DriveSubsystem.java | 2 +- .../frc4904/robot/subsystems/arm/ArmSubsystem.java | 5 +++-- 5 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 554f6cd..2e1baf6 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -47,11 +47,9 @@ public void teleopInitialize() { if (RobotContainer2.Component.leftBTalonFX != null) RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Brake); if (RobotContainer2.Component.rightATalonFX != null) RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Brake); if (RobotContainer2.Component.rightBTalonFX != null) RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Brake); - RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.neutralOutput(); - RobotMap.Component.arm.armExtensionSubsystem.motor.setBrakeOnNeutral(); + RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.brake(); - RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.setBrakeOnNeutral(); - RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.neutralOutput(); + RobotMap.Component.arm.armExtensionSubsystem.motor.brake(); driver.bindCommands(); @@ -60,9 +58,9 @@ public void teleopInitialize() { *************************/ // SATURDAY MORNING TEST - can you run drive train in queueline - donttouchme.m_robotDrive.m_leftMotors = null; - donttouchme.m_robotDrive.m_rightMotors = null; - donttouchme.m_robotDrive.m_drive = null; + // donttouchme.m_robotDrive.m_leftMotors = null; + // donttouchme.m_robotDrive.m_rightMotors = null; + // donttouchme.m_robotDrive.m_drive = null; DriveSubsystem.skuffedaf_teleop_initialized = true; diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index 1cb5604..bc63c9f 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -28,6 +28,7 @@ import org.usfirst.frc4904.standard.subsystems.chassis.WestCoastDrive; import org.usfirst.frc4904.standard.subsystems.motor.TalonMotorSubsystem; import edu.wpi.first.wpilibj.SerialPort; +import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; public class RobotMap { diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java index 8d37997..076809d 100644 --- a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java @@ -21,7 +21,7 @@ public class NathanGain extends Driver { public static final double PRECISE_SPEED_SCALE = 0.4; public static final double PRECISE_TURN_SCALE = 0.1; - public static final double TURN_CORRECTION = 0.05; + // public static final double TURN_CORRECTION = 0.05; // public static final double SPEEDY_TURN_GAIN = 0.7; @@ -83,6 +83,6 @@ public double getTurnSpeed() { // double operatorControlTurnSpeed = scaleGain(RobotMap.HumanInput.Operator.joystick.getAxis(0), 0.2, 1.5); if (NathanGain.isFlippy) return (turnSpeed) * -1; - return turnSpeed + TURN_CORRECTION;// + precisionTurnSpeed; + return turnSpeed;// + precisionTurnSpeed; } } diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/DriveSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/DriveSubsystem.java index cfc5143..d7c5317 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/DriveSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/DriveSubsystem.java @@ -102,7 +102,7 @@ public void periodic() { // RobotContainer2.Component.rightATalonFX.feed(); // RobotContainer2.Component.rightBTalonFX.feed(); - + m_drive.feed(); // m_leftEncoder.debug(); // m_rightEncoder.debug(); } diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index 6a62423..3243d0a 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -47,6 +47,7 @@ public class ArmSubsystem extends SubsystemBase { otherPositions.put("homeUp", new Pair<>(70., Units.inchesToMeters(0.))); // TODO: get number @thomasrimer otherPositions.put("homeDown", new Pair<>(-37., Units.inchesToMeters(0.))); otherPositions.put("intakeGround", new Pair<>(-37., Units.inchesToMeters(4.))); + otherPositions.put("intakeShelf", new Pair<>(-37., Units.inchesToMeters(4.))); // TODO WRONG AND BAD } @@ -61,13 +62,13 @@ public Command c_posReturnToHomeUp() { return cmd; } public Command c_posReturnToHomeDown() { - var cmd = c_holdArmPose(otherPositions.get("homeDown").getFirst(), otherPositions.get("homeUp").getSecond()).getFirst(); + var cmd = c_holdArmPose(otherPositions.get("homeDown").getFirst(), otherPositions.get("homeDown").getSecond()).getFirst(); cmd.setName("arm position - home (down)"); return cmd; } public Command c_posIntakeShelf() { // TODO: back up 14 inches -- remember to always use meters - var cmd = c_holdArmPose(otherPositions.get("intakeShelf").getFirst(), otherPositions.get("homeUp").getSecond()).getFirst(); + var cmd = c_holdArmPose(otherPositions.get("intakeShelf").getFirst(), otherPositions.get("intakeShelf").getSecond()).getFirst(); cmd.setName("arm position - pre shelf intake"); return cmd; } From a67baefac7a62b19cd6f3308dd700ce60731ef35 Mon Sep 17 00:00:00 2001 From: Ethan Date: Sun, 2 Apr 2023 11:34:08 -0700 Subject: [PATCH 21/63] added some stuff --- .../robot/seenoevil/RobotContainer2.java | 43 ++++++++++++++++++- 1 file changed, 42 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java index f9f2df2..ac6e449 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java @@ -186,7 +186,12 @@ public class RobotContainer2 { List.of(), new Pose2d(Units.inchesToMeters(53.5),0,new Rotation2d(Math.PI)), trajectoryConfigReversed - )) + )), + entry("go_to_pickup_next", TrajectoryGenerator.generateTrajectory( + new Pose2d(0,0,new Rotation2d(0)), + List.of(), + new Pose2d(4.44,0,new Rotation2d(0)), + trajectoryConfig)) ); @@ -419,4 +424,40 @@ public Command newAuton() { return command; } + + public Command shootCubeAndCross() { + var command = new SequentialCommandGroup( + //1. Position arm to place gamepiece + // TODO: options: either place the game picee, or try to flip over, shoot, and then come back so that we are in the same state + + // implement going over and shooting a cone? + + new ParallelCommandGroup( + //3. Retract arm + // RobotMap.Component.arm.c_posReturnToHomeDown(false), + RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getFirst() + .withTimeout(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()) + .andThen(new WaitCommand(0.8)) + .andThen(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getFirst().withTimeout(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getSecond()).andThen(new InstantCommand(() -> RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.setVoltage(0))) + ), + new SequentialCommandGroup( + new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 100).getSecond()), + RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.8).andThen(RobotMap.Component.intake.c_holdVoltage(0)) + ), + new SequentialCommandGroup( + new WaitCommand(2.5), //TODO: set wait time to allow arm to get started before moving? + //4. Drive out of the community and stop right in front of the next game piece. + getAutonomousCommand(getTrajectory("go_to_pickup_next")) + + ) + ) + // new Balance(RobotMap.Component.navx, wheelSpeeds, outputVolts, 1, -0.1) + //6. balance code here + ); + + return command; + } + + + } From a35d47290d1a7cae9ab80abe37b7ac4729687dde Mon Sep 17 00:00:00 2001 From: jac0rr Date: Sun, 2 Apr 2023 11:44:44 -0700 Subject: [PATCH 22/63] add 2shoot no balance auto --- .../robot/seenoevil/RobotContainer2.java | 30 ++++++++++++++++--- .../robot/subsystems/arm/ArmSubsystem.java | 1 + 2 files changed, 27 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java index ac6e449..052adc5 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java @@ -191,8 +191,12 @@ public class RobotContainer2 { new Pose2d(0,0,new Rotation2d(0)), List.of(), new Pose2d(4.44,0,new Rotation2d(0)), - trajectoryConfig)) - + trajectoryConfig)), + entry("from_pickup_to_place", TrajectoryGenerator.generateTrajectory( + new Pose2d(0,0,new Rotation2d(Math.PI)), + List.of(), + new Pose2d(4.44,0,new Rotation2d(Math.PI)), + trajectoryConfigReversed)) ); public static class Component { @@ -457,7 +461,25 @@ public Command shootCubeAndCross() { return command; } - + public Command shoot2auto(){ //shoot cone, grab cube, shoot cube, doesn't balance + var command = new SequentialCommandGroup( + RobotMap.Component.arm.c_shootCones(4), + new ParallelCommandGroup( + RobotMap.Component.arm.c_posReturnToHomeDown(), + getAutonomousCommand(getTrajectory("go_to_pickup_next")) + ), + RobotMap.Component.intake.c_holdVoltage(-8).withTimeout(0.5), //TODO: for picking up cone tune to make sure it works + getAutonomousCommand(getTrajectory("go_to_pickup_next")), + new ParallelCommandGroup( // shootign cube + RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getFirst(), + new SequentialCommandGroup( + new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()), + RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.5), + RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getFirst().withTimeout(0.8) + )) + + ); + return command; + } - } diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index 3243d0a..2768101 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -32,6 +32,7 @@ public class ArmSubsystem extends SubsystemBase { cones.put(1, new Triple<>(-19., Units.inchesToMeters(0), 3.)); cones.put(2, new Triple<>(29., Units.inchesToMeters(18), 3.)); cones.put(3, new Triple<>(31., Units.inchesToMeters(38.), 3.)); + cones.put(4, new Triple<>(180+31.,Units.inchesToMeters(38),3.));//placehoders, need a lot of tuning } public static final HashMap> cubes = new HashMap<>(); //in degrees, meters From 1cffe3033792af4e39ef20041ee3bec1bba00584 Mon Sep 17 00:00:00 2001 From: zbuster05 Date: Sun, 2 Apr 2023 11:47:45 -0700 Subject: [PATCH 23/63] ok gaming --- .../java/org/usfirst/frc4904/robot/Robot.java | 20 +++++-------- .../humaninterface/drivers/NathanGain.java | 2 -- .../operators/DefaultOperator.java | 6 ++++ .../robot/subsystems/arm/ArmSubsystem.java | 28 +++++++++++++------ 4 files changed, 32 insertions(+), 24 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 2e1baf6..9a8f9f9 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -52,6 +52,7 @@ public void teleopInitialize() { RobotMap.Component.arm.armExtensionSubsystem.motor.brake(); driver.bindCommands(); + operator.bindCommands(); /*********************** * HAZMAT BLOCK START @@ -98,13 +99,6 @@ public void teleopInitialize() { ) ); - RobotMap.HumanInput.Operator.joystick.button3.onTrue(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> -0.3)); - RobotMap.HumanInput.Operator.joystick.button3.onFalse(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> 0)); - - RobotMap.HumanInput.Operator.joystick.button5.onTrue(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> 0.3)); - RobotMap.HumanInput.Operator.joystick.button5.onFalse(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> 0)); - - // Intake // FIXME: use nameCommand to make it cleaner with expresions (no varibales) var cmdnull = RobotMap.Component.intake.c_holdVoltage(0); @@ -118,7 +112,7 @@ public void teleopInitialize() { RobotMap.HumanInput.Operator.joystick.button2.onFalse(cmdhold); // Outtake - var cmd1 = RobotMap.Component.intake.c_holdVoltage(5); + var cmd1 = RobotMap.Component.intake.c_holdVoltage(3); cmd1.setName("Intake - manual outtake activation"); RobotMap.HumanInput.Operator.joystick.button1.onTrue(cmd1); RobotMap.HumanInput.Operator.joystick.button1.onFalse(cmdnull); @@ -126,15 +120,15 @@ public void teleopInitialize() { // position + place cube RobotMap.HumanInput.Operator.joystick.button7.onTrue(RobotMap.Component.arm.c_shootCubes(3)); RobotMap.HumanInput.Operator.joystick.button9.onTrue(RobotMap.Component.arm.c_shootCubes(2)); - RobotMap.HumanInput.Operator.joystick.button11.onTrue(RobotMap.Component.arm.c_shootCubes(1)); - // //position cone - // RobotMap.HumanInput.Operator.joystick.button8.onTrue(RobotMap.Component.arm.placeCones(3)); - // RobotMap.HumanInput.Operator.joystick.button10.onTrue(RobotMap.Component.arm.placeCones(2)); - // RobotMap.HumanInput.Operator.joystick.button12.onTrue(RobotMap.Component.arm.placeCones(1)); + //position cone + RobotMap.HumanInput.Operator.joystick.button8.onTrue(RobotMap.Component.arm.c_shootCones(3)); + RobotMap.HumanInput.Operator.joystick.button10.onTrue(RobotMap.Component.arm.c_shootCones(2)); //shelf intake RobotMap.HumanInput.Operator.joystick.button6.onTrue(RobotMap.Component.arm.c_posIntakeShelf()); + RobotMap.HumanInput.Operator.joystick.button4.onTrue(RobotMap.Component.arm.c_posReturnToHomeUp()); + //ground intake // RobotMap.HumanInput.Operator.joystick.button4.onTrue(RobotMap.Component.arm.c_posIntakeGround()); diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java index 076809d..025f570 100644 --- a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java @@ -21,8 +21,6 @@ public class NathanGain extends Driver { public static final double PRECISE_SPEED_SCALE = 0.4; public static final double PRECISE_TURN_SCALE = 0.1; - // public static final double TURN_CORRECTION = 0.05; - // public static final double SPEEDY_TURN_GAIN = 0.7; public static double precisionScaleY = NORMAL_SPEED_GAIN; diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java index 4893d54..3e0e527 100644 --- a/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java @@ -18,6 +18,12 @@ public DefaultOperator(String name) { @Override public void bindCommands() { + RobotMap.HumanInput.Operator.joystick.button3.onTrue(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> -0.3)); + RobotMap.HumanInput.Operator.joystick.button3.onFalse(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> 0)); + + RobotMap.HumanInput.Operator.joystick.button5.onTrue(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> 0.3)); + RobotMap.HumanInput.Operator.joystick.button5.onFalse(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> 0)); + // Flippy button // RobotMap.HumanInput.Operator.joystick.button12.onTrue(new InstantCommand( diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index 3243d0a..7eacbe0 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -26,19 +26,30 @@ public class ArmSubsystem extends SubsystemBase { public static final double MAX_VELOCITY_PIVOT = 150; public static final double MAX_ACCEL_PIVOT = 200; + public static final double FUNNY_ANGLE_CORRECTION = 6; + public static final double FUNNY_EXTENSION_CORRECTION = 0; + + public static final HashMap> shelfCones = new HashMap<>(); //in degrees, meters + static { + // cones.put(1, new Triple<>(-19., Units.inchesToMeters(0), 3.)); + shelfCones.put(2, new Triple<>(29. + FUNNY_ANGLE_CORRECTION, Units.inchesToMeters(19) + FUNNY_EXTENSION_CORRECTION, 3.2)); + shelfCones.put(3, new Triple<>(41. + FUNNY_ANGLE_CORRECTION, ArmExtensionSubsystem.MAX_EXTENSION_M + FUNNY_EXTENSION_CORRECTION, 3.2)); + } - public static final HashMap> cones = new HashMap<>(); //in degrees, meters + public static final HashMap> floorCones = new HashMap<>(); //in degrees, meters static { - cones.put(1, new Triple<>(-19., Units.inchesToMeters(0), 3.)); - cones.put(2, new Triple<>(29., Units.inchesToMeters(18), 3.)); - cones.put(3, new Triple<>(31., Units.inchesToMeters(38.), 3.)); + // cones.put(1, new Triple<>(-19., Units.inchesToMeters(0), 3.)); + floorCones.put(2, new Triple<>(29. + FUNNY_ANGLE_CORRECTION, Units.inchesToMeters(16) + FUNNY_EXTENSION_CORRECTION, 3.)); + floorCones.put(3, new Triple<>(31. + FUNNY_ANGLE_CORRECTION, ArmExtensionSubsystem.MAX_EXTENSION_M + FUNNY_EXTENSION_CORRECTION, 3.)); } + public static HashMap> cones = floorCones; + public static final HashMap> cubes = new HashMap<>(); //in degrees, meters static { - cubes.put(1, new Triple<>(-33., Units.inchesToMeters(0), 3.)); - cubes.put(2, new Triple<>(14., Units.inchesToMeters(6), 4.)); - cubes.put(3, new Triple<>(22., Units.inchesToMeters(28.), 4.5)); + // cubes.put(1, new Triple<>(-33., Units.inchesToMeters(0), 3.)); + cubes.put(2, new Triple<>(15. + FUNNY_ANGLE_CORRECTION, Units.inchesToMeters(0), 4.5)); + cubes.put(3, new Triple<>(22. + FUNNY_ANGLE_CORRECTION, Units.inchesToMeters(0), 4.5)); } public static final HashMap> otherPositions = new HashMap<>(); @@ -46,8 +57,7 @@ public class ArmSubsystem extends SubsystemBase { // https://docs.google.com/spreadsheets/d/1B7Ie4efOpuZb4UQsk8lHycGvi6BspnF74DUMLmiKGUM/edit#gid=0 in degrees, meters otherPositions.put("homeUp", new Pair<>(70., Units.inchesToMeters(0.))); // TODO: get number @thomasrimer otherPositions.put("homeDown", new Pair<>(-37., Units.inchesToMeters(0.))); - otherPositions.put("intakeGround", new Pair<>(-37., Units.inchesToMeters(4.))); - otherPositions.put("intakeShelf", new Pair<>(-37., Units.inchesToMeters(4.))); // TODO WRONG AND BAD + otherPositions.put("intakeShelf", new Pair<>(31., Units.inchesToMeters(20.))); } From abf8afdf92c1c049fee1f4b791005713eea198c2 Mon Sep 17 00:00:00 2001 From: Ethan Date: Sun, 2 Apr 2023 11:48:30 -0700 Subject: [PATCH 24/63] fixed one thing, time on rotation needs tuning --- .../robot/seenoevil/RobotContainer2.java | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java index 052adc5..ed2a19b 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java @@ -469,14 +469,16 @@ public Command shoot2auto(){ //shoot cone, grab cube, shoot cube, doesn't balanc getAutonomousCommand(getTrajectory("go_to_pickup_next")) ), RobotMap.Component.intake.c_holdVoltage(-8).withTimeout(0.5), //TODO: for picking up cone tune to make sure it works - getAutonomousCommand(getTrajectory("go_to_pickup_next")), - new ParallelCommandGroup( // shootign cube - RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getFirst(), - new SequentialCommandGroup( - new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()), - RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.5), - RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getFirst().withTimeout(0.8) - )) + new ParallelCommandGroup( + RobotMap.Component.intake.c_holdVoltage(-1).withTimeout(2.5), //TODO: Tune the time after testing + getAutonomousCommand(getTrajectory("go_to_pickup_next")), + new ParallelCommandGroup( // shootign cube + RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getFirst(), + new SequentialCommandGroup( + new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()), + RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.5), + RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getFirst().withTimeout(0.8) + ))) ); return command; From cc6d2f0351547766e599254e912d445a476328d1 Mon Sep 17 00:00:00 2001 From: Ethan Date: Sun, 2 Apr 2023 12:21:18 -0700 Subject: [PATCH 25/63] finished shoot2auto --- .../robot/seenoevil/RobotContainer2.java | 23 ++++++++++--------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java index ed2a19b..18d74eb 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java @@ -27,6 +27,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import com.ctre.phoenix.motorcontrol.NeutralMode; @@ -469,17 +470,17 @@ public Command shoot2auto(){ //shoot cone, grab cube, shoot cube, doesn't balanc getAutonomousCommand(getTrajectory("go_to_pickup_next")) ), RobotMap.Component.intake.c_holdVoltage(-8).withTimeout(0.5), //TODO: for picking up cone tune to make sure it works - new ParallelCommandGroup( - RobotMap.Component.intake.c_holdVoltage(-1).withTimeout(2.5), //TODO: Tune the time after testing - getAutonomousCommand(getTrajectory("go_to_pickup_next")), - new ParallelCommandGroup( // shootign cube - RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getFirst(), - new SequentialCommandGroup( - new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()), - RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.5), - RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getFirst().withTimeout(0.8) - ))) - + new ParallelRaceGroup( + RobotMap.Component.intake.c_holdVoltage(-1), + getAutonomousCommand(getTrajectory("from_pickup_to_place")) + ), + new ParallelCommandGroup( // shootign cube + RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getFirst(), + new SequentialCommandGroup( + new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()), + RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.5), + RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getFirst().withTimeout(0.8) + )) ); return command; } From 6e9748addc3b25db6c360c8b162e28cef276565c Mon Sep 17 00:00:00 2001 From: Ethan Date: Sun, 2 Apr 2023 12:31:48 -0700 Subject: [PATCH 26/63] added 4th mode for shelfcones --- .../org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index 7eacbe0..12bffcb 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -34,6 +34,7 @@ public class ArmSubsystem extends SubsystemBase { // cones.put(1, new Triple<>(-19., Units.inchesToMeters(0), 3.)); shelfCones.put(2, new Triple<>(29. + FUNNY_ANGLE_CORRECTION, Units.inchesToMeters(19) + FUNNY_EXTENSION_CORRECTION, 3.2)); shelfCones.put(3, new Triple<>(41. + FUNNY_ANGLE_CORRECTION, ArmExtensionSubsystem.MAX_EXTENSION_M + FUNNY_EXTENSION_CORRECTION, 3.2)); + shelfCones.put(4, new Triple<>(180-(41+FUNNY_ANGLE_CORRECTION), ArmExtensionSubsystem.MAX_EXTENSION_M + FUNNY_EXTENSION_CORRECTION, 4.)); } public static final HashMap> floorCones = new HashMap<>(); //in degrees, meters From c24f3e8d76ec6b74303e49b48b5727317f77cec8 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Sun, 2 Apr 2023 13:18:11 -0700 Subject: [PATCH 27/63] UT: hold arm pose --- .../java/org/usfirst/frc4904/robot/Robot.java | 32 ---------- .../operators/DefaultOperator.java | 62 +++++++++++-------- .../robot/subsystems/arm/ArmSubsystem.java | 7 +++ 3 files changed, 42 insertions(+), 59 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 9a8f9f9..92e3e81 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -99,39 +99,7 @@ public void teleopInitialize() { ) ); - // Intake - // FIXME: use nameCommand to make it cleaner with expresions (no varibales) - var cmdnull = RobotMap.Component.intake.c_holdVoltage(0); - var cmd2 = RobotMap.Component.intake.c_holdVoltage(-8); - cmd2.setName("Intake - manual intake activation"); - var cmdhold = RobotMap.Component.intake.c_holdVoltage(-2).withTimeout(0.5).andThen(RobotMap.Component.intake.c_holdVoltage(-1)); - - cmdnull.setName("Intake - deactivated"); - RobotMap.HumanInput.Operator.joystick.button2.onTrue(cmd2); - RobotMap.HumanInput.Operator.joystick.button2.onFalse(cmdhold); - - // Outtake - var cmd1 = RobotMap.Component.intake.c_holdVoltage(3); - cmd1.setName("Intake - manual outtake activation"); - RobotMap.HumanInput.Operator.joystick.button1.onTrue(cmd1); - RobotMap.HumanInput.Operator.joystick.button1.onFalse(cmdnull); - - // position + place cube - RobotMap.HumanInput.Operator.joystick.button7.onTrue(RobotMap.Component.arm.c_shootCubes(3)); - RobotMap.HumanInput.Operator.joystick.button9.onTrue(RobotMap.Component.arm.c_shootCubes(2)); - - //position cone - RobotMap.HumanInput.Operator.joystick.button8.onTrue(RobotMap.Component.arm.c_shootCones(3)); - RobotMap.HumanInput.Operator.joystick.button10.onTrue(RobotMap.Component.arm.c_shootCones(2)); - - //shelf intake - RobotMap.HumanInput.Operator.joystick.button6.onTrue(RobotMap.Component.arm.c_posIntakeShelf()); - RobotMap.HumanInput.Operator.joystick.button4.onTrue(RobotMap.Component.arm.c_posReturnToHomeUp()); - - - //ground intake - // RobotMap.HumanInput.Operator.joystick.button4.onTrue(RobotMap.Component.arm.c_posIntakeGround()); } @Override diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java index 3e0e527..f8bfa16 100644 --- a/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java @@ -17,37 +17,45 @@ public DefaultOperator(String name) { @Override public void bindCommands() { + var joystick = RobotMap.HumanInput.Operator.joystick; - RobotMap.HumanInput.Operator.joystick.button3.onTrue(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> -0.3)); - RobotMap.HumanInput.Operator.joystick.button3.onFalse(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> 0)); + joystick.button3.onTrue(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> -0.3)); + joystick.button3.onFalse(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> 0)); - RobotMap.HumanInput.Operator.joystick.button5.onTrue(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> 0.3)); - RobotMap.HumanInput.Operator.joystick.button5.onFalse(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> 0)); - + joystick.button5.onTrue(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> 0.3)); + joystick.button5.onFalse(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> 0)); - // Flippy button - // RobotMap.HumanInput.Operator.joystick.button12.onTrue(new InstantCommand( - // () -> { - // NathanGain.isFlippy = !NathanGain.isFlippy; - // } - // )); - // don't use flippy because confusion - - // // position + place cube - // RobotMap.HumanInput.Operator.joystick.button7.onTrue(RobotMap.Component.arm.placeCube(3)); - // RobotMap.HumanInput.Operator.joystick.button9.onTrue(RobotMap.Component.arm.placeCube(2)); - // RobotMap.HumanInput.Operator.joystick.button11.onTrue(RobotMap.Component.arm.placeCube(1)); - - // //position cone - // RobotMap.HumanInput.Operator.joystick.button8.onTrue(RobotMap.Component.arm.c_angleCones(3)); - // RobotMap.HumanInput.Operator.joystick.button10.onTrue(RobotMap.Component.arm.c_angleCones(2)); - // RobotMap.HumanInput.Operator.joystick.button12.onTrue(RobotMap.Component.arm.c_angleCones(1)); + // Intake + // FIXME: use nameCommand to make it cleaner with expresions (no varibales) + var zeroIntake = RobotMap.Component.intake.c_holdVoltage(0); + var holdPiece = RobotMap.Component.intake.c_holdVoltage(-2).withTimeout(0.5).andThen(RobotMap.Component.intake.c_holdVoltage(-1)); + var runIntake = RobotMap.Component.intake.c_holdVoltage(-8); + var runOuttake = RobotMap.Component.intake.c_holdVoltage(3); - // //shelf intake - // RobotMap.HumanInput.Operator.joystick.button6.onTrue(RobotMap.Component.arm.c_posIntakeShelf()); - - // //ground intake - // RobotMap.HumanInput.Operator.joystick.button4.onTrue(RobotMap.Component.arm.c_posIntakeGround()); + // intake + joystick.button2.onTrue(runIntake); + joystick.button2.onFalse(holdPiece); + + // outtake + joystick.button1.onTrue(runOuttake); + joystick.button1.onFalse(zeroIntake); + + + // position + place cube + joystick.button7.onTrue(RobotMap.Component.arm.c_shootCubes(3)); + joystick.button9.onTrue(RobotMap.Component.arm.c_shootCubes(2)); + + // position cone + joystick.button8.onTrue(RobotMap.Component.arm.c_shootCones(3)); + joystick.button10.onTrue(RobotMap.Component.arm.c_shootCones(2)); + + // intake positions + joystick.button6.onTrue(RobotMap.Component.arm.c_posIntakeShelf()); + joystick.button4.onTrue(RobotMap.Component.arm.c_posIntakeFloor()); + + // stow positions + joystick.button11.onTrue(RobotMap.Component.arm.c_posReturnToHomeDown()); + joystick.button12.onTrue(RobotMap.Component.arm.c_posReturnToHomeUp()); } } diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index 12bffcb..caed155 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -79,10 +79,17 @@ public Command c_posReturnToHomeDown() { } public Command c_posIntakeShelf() { // TODO: back up 14 inches -- remember to always use meters + cones = floorCones; var cmd = c_holdArmPose(otherPositions.get("intakeShelf").getFirst(), otherPositions.get("intakeShelf").getSecond()).getFirst(); cmd.setName("arm position - pre shelf intake"); return cmd; } + public Command c_posIntakeFloor() { + cones = floorCones; + var cmd = c_holdArmPose(otherPositions.get("homeDown").getFirst(), otherPositions.get("homeDown").getSecond()).getFirst(); + cmd.setName("arm position - pre floor intake"); + return cmd; + } public Pair c_angleCones(int shelf) { var degreesFromHorizontal = cones.get(shelf).getFirst(); From f0e58d7323f86173489cda58f6bda3bcce34a8ea Mon Sep 17 00:00:00 2001 From: zbuster05 Date: Sun, 2 Apr 2023 13:47:00 -0700 Subject: [PATCH 28/63] Constants tuning --- src/main/java/org/usfirst/frc4904/robot/Robot.java | 9 ++++++--- .../frc4904/robot/humaninterface/drivers/NathanGain.java | 8 ++++---- .../frc4904/robot/subsystems/arm/ArmPivotSubsystem.java | 2 +- 3 files changed, 11 insertions(+), 8 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 9a8f9f9..bf5204f 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -99,6 +99,7 @@ public void teleopInitialize() { ) ); + // Intake // FIXME: use nameCommand to make it cleaner with expresions (no varibales) var cmdnull = RobotMap.Component.intake.c_holdVoltage(0); @@ -146,12 +147,12 @@ public void teleopExecute() { // SmartDashboard.putNumber("Driver out", driver.getTurnSpeed()); - SmartDashboard.putBoolean("isFlipped - IMPORTANT", NathanGain.isFlippy); + // SmartDashboard.putBoolean("isFlipped - IMPORTANT", NathanGain.isFlippy); SmartDashboard.putNumber("gyroooo", RobotMap.Component.navx.getAngle()); SmartDashboard.putNumber("arm extension length", RobotMap.Component.arm.armExtensionSubsystem.getCurrentExtensionLength()); SmartDashboard.putNumber("arm pivot angle", RobotMap.Component.arm.armPivotSubsystem.getCurrentAngleDegrees()); - SmartDashboard.putNumber("Falcon temp", RobotContainer2.Component.leftATalonFX.getTemperature()); + // SmartDashboard.putNumber("Falcon temp", RobotContainer2.Component.leftATalonFX.getTemperature()); } @@ -249,7 +250,9 @@ public void testExecute() { @Override public void alwaysExecute() { - // SmartDashboard.putNumber("Arm angle", RobotMap.Component.arm.armPivotSubsystem.getCurrentAngleDegrees()); + SmartDashboard.putNumber("Arm angle", RobotMap.Component.arm.armPivotSubsystem.getCurrentAngleDegrees()); + SmartDashboard.putNumber("arm extension length", RobotMap.Component.arm.armExtensionSubsystem.getCurrentExtensionLength()); + // SmartDashboard.putNumber("gyroooo", RobotMap.Component.navx.getAngle()); } diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java index 025f570..3bdf34a 100644 --- a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java @@ -11,14 +11,14 @@ public class NathanGain extends Driver { public static final double SPEED_EXP = 2; public static final double Y_SPEED_SCALE = 1; - public static final double TURN_EXP = 4; + public static final double TURN_EXP = 3.5; public static final double TURN_SPEED_SCALE = 1; public static final double NORMAL_SPEED_GAIN = 0.5; // TODO TUNE public static final double NORMAL_TURN_GAIN = 0.3; - public static final double PRECISE_SPEED_SCALE = 0.4; + public static final double PRECISE_SPEED_SCALE = 0.2; public static final double PRECISE_TURN_SCALE = 0.1; // public static final double SPEEDY_TURN_GAIN = 0.7; @@ -36,13 +36,13 @@ protected double scaleGain(double input, double gain, double exp) { @Override public void bindCommands() { - RobotMap.HumanInput.Driver.xbox.leftBumper().onTrue(new InstantCommand( + RobotMap.HumanInput.Driver.xbox.x().onTrue(new InstantCommand( () -> { NathanGain.precisionScaleY = PRECISE_SPEED_SCALE; NathanGain.precisionScaleTurn = PRECISE_TURN_SCALE; } )); - RobotMap.HumanInput.Driver.xbox.leftBumper().onFalse(new InstantCommand(( + RobotMap.HumanInput.Driver.xbox.x().onFalse(new InstantCommand(( ) -> { NathanGain.precisionScaleY = NORMAL_SPEED_GAIN; NathanGain.precisionScaleTurn = NORMAL_TURN_GAIN; diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java index b60ee7c..d5f3f09 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java @@ -56,7 +56,7 @@ public class ArmPivotSubsystem extends SubsystemBase { // public static final double kA_extended = .12082; public static final double kG_retracted = 0.32; - public static final double kG_extended = 0.7; + public static final double kG_extended = 0.6; // TODO: tune public static final double kP = 0;//0.04; //extended: .36915 retracted: .01464 From 2568941c56d4a0f12f07f074c0131cb9f281cf02 Mon Sep 17 00:00:00 2001 From: aze12345 <88347612+aze12345@users.noreply.github.com> Date: Sun, 2 Apr 2023 13:51:36 -0700 Subject: [PATCH 29/63] add linear error correction to arm subsystems --- .../subsystems/arm/ArmExtensionSubsystem.java | 2 +- .../robot/subsystems/arm/ArmPivotSubsystem.java | 2 +- .../robot/subsystems/arm/ArmSubsystem.java | 17 +++++++---------- 3 files changed, 9 insertions(+), 12 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java index 2ad39a4..72aedd8 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java @@ -56,7 +56,7 @@ public void initializeEncoderPositions(double meters) { } public double getCurrentExtensionLength() { - return revsToExtensionLength(motor.getSensorPositionRotations()); + return revsToExtensionLength(motor.getSensorPositionRotations()) * 0.968 -0.0853; } public void setVoltageSafely(double voltage) { diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java index b60ee7c..7e5f999 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java @@ -82,7 +82,7 @@ public ArmPivotSubsystem(TalonMotorSubsystem armMotorGroup, DoubleSupplier exten public double getCurrentAngleDegrees() { // return slackyEncoder.getRealPosition(); - return motorRevsToAngle(armMotorGroup.getSensorPositionRotations()); + return motorRevsToAngle(armMotorGroup.getSensorPositionRotations()) * 0.911 - 6.3; } diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index caed155..223cd2f 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -26,22 +26,19 @@ public class ArmSubsystem extends SubsystemBase { public static final double MAX_VELOCITY_PIVOT = 150; public static final double MAX_ACCEL_PIVOT = 200; - public static final double FUNNY_ANGLE_CORRECTION = 6; - public static final double FUNNY_EXTENSION_CORRECTION = 0; - public static final HashMap> shelfCones = new HashMap<>(); //in degrees, meters static { // cones.put(1, new Triple<>(-19., Units.inchesToMeters(0), 3.)); - shelfCones.put(2, new Triple<>(29. + FUNNY_ANGLE_CORRECTION, Units.inchesToMeters(19) + FUNNY_EXTENSION_CORRECTION, 3.2)); - shelfCones.put(3, new Triple<>(41. + FUNNY_ANGLE_CORRECTION, ArmExtensionSubsystem.MAX_EXTENSION_M + FUNNY_EXTENSION_CORRECTION, 3.2)); - shelfCones.put(4, new Triple<>(180-(41+FUNNY_ANGLE_CORRECTION), ArmExtensionSubsystem.MAX_EXTENSION_M + FUNNY_EXTENSION_CORRECTION, 4.)); + shelfCones.put(2, new Triple<>(29., Units.inchesToMeters(19), 3.2)); + shelfCones.put(3, new Triple<>(41., ArmExtensionSubsystem.MAX_EXTENSION_M, 3.2)); + shelfCones.put(4, new Triple<>(180.0-41, ArmExtensionSubsystem.MAX_EXTENSION_M, 4.)); } public static final HashMap> floorCones = new HashMap<>(); //in degrees, meters static { // cones.put(1, new Triple<>(-19., Units.inchesToMeters(0), 3.)); - floorCones.put(2, new Triple<>(29. + FUNNY_ANGLE_CORRECTION, Units.inchesToMeters(16) + FUNNY_EXTENSION_CORRECTION, 3.)); - floorCones.put(3, new Triple<>(31. + FUNNY_ANGLE_CORRECTION, ArmExtensionSubsystem.MAX_EXTENSION_M + FUNNY_EXTENSION_CORRECTION, 3.)); + floorCones.put(2, new Triple<>(29., Units.inchesToMeters(16), 3.)); + floorCones.put(3, new Triple<>(31., ArmExtensionSubsystem.MAX_EXTENSION_M, 3.)); } public static HashMap> cones = floorCones; @@ -49,8 +46,8 @@ public class ArmSubsystem extends SubsystemBase { public static final HashMap> cubes = new HashMap<>(); //in degrees, meters static { // cubes.put(1, new Triple<>(-33., Units.inchesToMeters(0), 3.)); - cubes.put(2, new Triple<>(15. + FUNNY_ANGLE_CORRECTION, Units.inchesToMeters(0), 4.5)); - cubes.put(3, new Triple<>(22. + FUNNY_ANGLE_CORRECTION, Units.inchesToMeters(0), 4.5)); + cubes.put(2, new Triple<>(15., Units.inchesToMeters(0), 4.5)); + cubes.put(3, new Triple<>(22., Units.inchesToMeters(0), 4.5)); } public static final HashMap> otherPositions = new HashMap<>(); From c47c72cf8025a07e95972fafb410f09711e19bd5 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Sun, 2 Apr 2023 14:08:57 -0700 Subject: [PATCH 30/63] merge or something? --- src/main/java/org/usfirst/frc4904/robot/Robot.java | 3 --- .../frc4904/robot/humaninterface/drivers/NathanGain.java | 2 +- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 5d1c336..b224d5d 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -99,13 +99,10 @@ public void teleopInitialize() { ) ); -<<<<<<< HEAD // Intake // FIXME: use nameCommand to make it cleaner with expresions (no varibales) var cmdnull = RobotMap.Component.intake.c_holdVoltage(0); -======= ->>>>>>> c24f3e8d76ec6b74303e49b48b5727317f77cec8 } diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java index 3bdf34a..d8b7e3a 100644 --- a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java @@ -11,7 +11,7 @@ public class NathanGain extends Driver { public static final double SPEED_EXP = 2; public static final double Y_SPEED_SCALE = 1; - public static final double TURN_EXP = 3.5; + public static final double TURN_EXP = 4; public static final double TURN_SPEED_SCALE = 1; From 2b760f45a4c28f5091be39e069b9391b1bfec258 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Sun, 2 Apr 2023 14:45:01 -0700 Subject: [PATCH 31/63] baseding armSubsystem --- .../robot/subsystems/arm/ArmSubsystem.java | 100 ++++++++++-------- 1 file changed, 53 insertions(+), 47 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index 223cd2f..f27e790 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -65,30 +65,30 @@ public ArmSubsystem(ArmPivotSubsystem armPivotSubsystem, ArmExtensionSubsystem a } public Command c_posReturnToHomeUp() { - var cmd = c_holdArmPose(otherPositions.get("homeUp").getFirst(), otherPositions.get("homeUp").getSecond()).getFirst(); + var cmd = c_holdArmPose(otherPositions.get("homeUp")); cmd.setName("arm position - home (up)"); return cmd; } public Command c_posReturnToHomeDown() { - var cmd = c_holdArmPose(otherPositions.get("homeDown").getFirst(), otherPositions.get("homeDown").getSecond()).getFirst(); + var cmd = c_holdArmPose(otherPositions.get("homeDown")); cmd.setName("arm position - home (down)"); return cmd; } public Command c_posIntakeShelf() { // TODO: back up 14 inches -- remember to always use meters cones = floorCones; - var cmd = c_holdArmPose(otherPositions.get("intakeShelf").getFirst(), otherPositions.get("intakeShelf").getSecond()).getFirst(); + var cmd = c_holdArmPose(otherPositions.get("intakeShelf")); cmd.setName("arm position - pre shelf intake"); return cmd; } public Command c_posIntakeFloor() { cones = floorCones; - var cmd = c_holdArmPose(otherPositions.get("homeDown").getFirst(), otherPositions.get("homeDown").getSecond()).getFirst(); + var cmd = c_holdArmPose(otherPositions.get("homeDown")); cmd.setName("arm position - pre floor intake"); return cmd; } - public Pair c_angleCones(int shelf) { + public Command c_angleCones(int shelf) { var degreesFromHorizontal = cones.get(shelf).getFirst(); var extensionLengthMeters = cones.get(shelf).getSecond(); @@ -100,19 +100,18 @@ public Command c_shootCones(int shelf) { var extensionLengthMeters = cones.get(shelf).getSecond(); var voltage = cones.get(shelf).getThird(); - Pair armMovement = c_holdArmPose(degreesFromHorizontal, extensionLengthMeters); - - return armMovement.getFirst().withTimeout(armMovement.getSecond()) - .andThen(RobotMap.Component.intake.c_holdVoltage(voltage).withTimeout(0.5)) - .andThen(RobotMap.Component.intake.c_holdVoltage(0)) - .andThen(c_posReturnToHomeUp()); + return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters, + RobotMap.Component.intake.c_holdVoltage(voltage).withTimeout(0.5) + .andThen(RobotMap.Component.intake.c_holdVoltage(0)) + .andThen(c_posReturnToHomeUp()) + ); } - public Pair c_angleCubes(int shelf) { + public Command c_angleCubes(int shelf) { var degreesFromHorizontal = cubes.get(shelf).getFirst(); var extensionLengthMeters = cubes.get(shelf).getSecond(); - return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters); + return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters, null); } public Command c_shootCubes(int shelf) { @@ -120,45 +119,52 @@ public Command c_shootCubes(int shelf) { var extensionLengthMeters = cubes.get(shelf).getSecond(); var voltage = cubes.get(shelf).getThird(); - Pair armMovement = c_holdArmPose(degreesFromHorizontal, extensionLengthMeters); - - return armMovement.getFirst().withTimeout(armMovement.getSecond()) - .andThen(RobotMap.Component.intake.c_holdVoltage(voltage).withTimeout(0.5)) - .andThen(RobotMap.Component.intake.c_holdVoltage(0)) - .andThen(c_posReturnToHomeUp()); + return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters, + RobotMap.Component.intake.c_holdVoltage(voltage).withTimeout(0.5) + .andThen(RobotMap.Component.intake.c_holdVoltage(0)) + .andThen(c_posReturnToHomeUp()) + ); } - public Pair c_holdArmPose(double degreesFromHorizontal, double extensionLengthMeters) { - // return this.runOnce(() -> System.out.println("TODO: hold arm pose crashes the code!")); - Command firstCommand; - Command secondCommand; - double wait; - - Pair pivotMovement = armPivotSubsystem.c_holdRotation(degreesFromHorizontal, MAX_VELOCITY_PIVOT, MAX_ACCEL_PIVOT); - Pair extensionMovement = armExtensionSubsystem.c_holdExtension(extensionLengthMeters, MAX_VELOCITY_EXTENSION, MAX_ACCEL_EXTENSION); + public Command c_holdArmPose(Pair emacs) { + return c_holdArmPose(emacs.getFirst(), emacs.getSecond()); + } + public Command c_holdArmPose(Pair emacs, Command onArrivalCommand) { + return c_holdArmPose(emacs.getFirst(), emacs.getSecond(), onArrivalCommand); + } + public Command c_holdArmPose(double degreesFromHorizontal, double extensionLengthMeters) { + return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters, null); + } - if ((extensionLengthMeters - armExtensionSubsystem.getCurrentExtensionLength()) > 0) { - firstCommand = pivotMovement.getFirst(); - wait = pivotMovement.getSecond(); - secondCommand = extensionMovement.getFirst(); - } else { - firstCommand = extensionMovement.getFirst(); - wait = extensionMovement.getSecond(); - secondCommand = pivotMovement.getFirst(); - } + public Command c_holdArmPose(double degreesFromHorizontal, double extensionLengthMeters, Command onArrivalCommand) { var cmd = this.runOnce(() -> { - firstCommand.schedule(); - // (Commands.run(() -> System.out.println( - // "1: " + String.valueOf(firstCommand.isScheduled()) - // + " ... 2: " + String.valueOf(secondCommand.isScheduled()) - // + " ... cur : " + armExtensionSubsystem.getCurrentCommand().getName() - // + " ... joystick: " + String.valueOf(RobotMap.HumanInput.Operator.joystick.getAxis(3)) - // ))).schedule(); - (new SequentialCommandGroup(new WaitCommand(wait), secondCommand)).schedule(); - // ((new WaitCommand(1)).andThen(secondCommand)).schedule(); - // secondCommand.schedule(); + Command firstCommand; + Command secondCommand; + double wait; + double secondWait; + + Pair pivotMovement = armPivotSubsystem.c_holdRotation(degreesFromHorizontal, + MAX_VELOCITY_PIVOT, MAX_ACCEL_PIVOT); + Pair extensionMovement = armExtensionSubsystem.c_holdExtension(extensionLengthMeters, + MAX_VELOCITY_EXTENSION, MAX_ACCEL_EXTENSION); + + if ((extensionLengthMeters - armExtensionSubsystem.getCurrentExtensionLength()) > 0) { + firstCommand = pivotMovement.getFirst(); + wait = pivotMovement.getSecond(); + secondCommand = extensionMovement.getFirst(); + secondWait = wait + extensionMovement.getSecond(); + } else { + firstCommand = extensionMovement.getFirst(); + wait = extensionMovement.getSecond(); + secondCommand = pivotMovement.getFirst(); + secondWait = wait + pivotMovement.getSecond(); + } + + firstCommand.schedule(); + (new SequentialCommandGroup(new WaitCommand(wait), secondCommand)).schedule(); + if (onArrivalCommand != null) (new SequentialCommandGroup(new WaitCommand(secondWait), onArrivalCommand)).schedule(); }); // long story. basically, parallel command group requires it's subcommands' requirements. however, we want one subcommand to be able to die wihle the other one lives, so we just do this instead and leak commands. it's fine because they'll get cleaned up when their atomic base subsystems gets taken over by new commands cmd.setName("arm - c_holdArmPose"); - return new Pair(cmd, pivotMovement.getSecond() + extensionMovement.getSecond()); + return cmd; } } From f3ad68c88ffc405a5786b210c32083c76a0f67d1 Mon Sep 17 00:00:00 2001 From: aze12345 <88347612+aze12345@users.noreply.github.com> Date: Sun, 2 Apr 2023 15:31:54 -0700 Subject: [PATCH 32/63] Lies to feedforward --- src/main/java/org/usfirst/frc4904/robot/RobotMap.java | 2 +- .../robot/subsystems/arm/ArmExtensionSubsystem.java | 4 ++-- .../frc4904/robot/subsystems/arm/ArmPivotSubsystem.java | 8 ++++---- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index bc63c9f..5766e57 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -211,7 +211,7 @@ public RobotMap() { TalonMotorSubsystem armRotationMotors = new TalonMotorSubsystem("Arm Pivot Subsystem", NeutralMode.Brake, 0, leftPivotMotor, rightPivotMotor); ArmExtensionSubsystem armExtensionSubsystem = new ArmExtensionSubsystem( new TalonMotorSubsystem("Arm Extension Subsystem", NeutralMode.Brake, 0, armExtensionMotor), - () -> ArmPivotSubsystem.motorRevsToAngle(armRotationMotors.getSensorPositionRotations()) + () -> ArmPivotSubsystem.motorRevsToAngle(armRotationMotors.getSensorPositionRotations() * 0.911 - 6.3) ); // Autonomous.autonCommand = Component.chassis.c_buildPathPlannerAuto( // PID.Drive.kS, PID.Drive.kV, PID.Drive.kA, diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java index 72aedd8..1c29919 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java @@ -103,8 +103,8 @@ public Pair c_holdExtension(double extensionLengthMeters, doubl )); TrapezoidProfile profile = new TrapezoidProfile(new TrapezoidProfile.Constraints(maxVelocity, maxAcceleration), - new TrapezoidProfile.State(extensionLengthMeters, 0), - new TrapezoidProfile.State(getCurrentExtensionLength(), 0)); + new TrapezoidProfile.State((extensionLengthMeters - 0.0853)/0.968, 0), + new TrapezoidProfile.State((getCurrentExtensionLength() - 0.0853)/0.968, 0)); var cmd = new ezMotion(controller, this::getCurrentExtensionLength, motor::setVoltage, (double t) -> new Tuple2(profile.calculate(t).position, profile.calculate(t).velocity), this, motor); cmd.setName("arm - c_holdExtension"); return new Pair(cmd, profile.totalTime()); diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java index c7c4680..132a1e0 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java @@ -86,7 +86,7 @@ public double getCurrentAngleDegrees() { } - /** + /**[p] * Expects sensors to be zeroed at forward hard-stop. */ public void initializeEncoderPositions() { @@ -130,7 +130,7 @@ public Pair c_holdRotation(double degreesFromHorizontal, double (position, velocityDegPerSec) -> { double brr = this.feedforward.calculate( extensionDealerMeters.getAsDouble()/ArmExtensionSubsystem.MAX_EXTENSION_M, - Units.degreesToRadians(getCurrentAngleDegrees()), + Units.degreesToRadians((getCurrentAngleDegrees() + 6.3)/0.911), Units.degreesToRadians(velocityDegPerSec), 0 ); @@ -141,8 +141,8 @@ public Pair c_holdRotation(double degreesFromHorizontal, double TrapezoidProfile profile = new TrapezoidProfile( new TrapezoidProfile.Constraints(maxVelDegPerSec, maxAccelDegPerSecSquare), - new TrapezoidProfile.State(degreesFromHorizontal, 0), - new TrapezoidProfile.State(getCurrentAngleDegrees(), 0) + new TrapezoidProfile.State((degreesFromHorizontal + 6.3)/0.911, 0), + new TrapezoidProfile.State((getCurrentAngleDegrees() + 6.3)/0.911, 0) ); var cmd = new ezMotion( controller, From 11f10461eb695428e72259d736e966df9bf8176f Mon Sep 17 00:00:00 2001 From: Exr0n Date: Sun, 2 Apr 2023 17:41:16 -0700 Subject: [PATCH 33/63] UT: onArrivalCommandDealer --- .../frc4904/robot/subsystems/arm/ArmSubsystem.java | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index f27e790..847e6bb 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -1,6 +1,7 @@ package org.usfirst.frc4904.robot.subsystems.arm; import java.util.HashMap; +import java.util.function.Supplier; import org.usfirst.frc4904.robot.RobotMap; import org.usfirst.frc4904.robot.subsystems.Intake; @@ -101,7 +102,7 @@ public Command c_shootCones(int shelf) { var voltage = cones.get(shelf).getThird(); return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters, - RobotMap.Component.intake.c_holdVoltage(voltage).withTimeout(0.5) + () -> RobotMap.Component.intake.c_holdVoltage(voltage).withTimeout(0.5) .andThen(RobotMap.Component.intake.c_holdVoltage(0)) .andThen(c_posReturnToHomeUp()) ); @@ -120,7 +121,7 @@ public Command c_shootCubes(int shelf) { var voltage = cubes.get(shelf).getThird(); return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters, - RobotMap.Component.intake.c_holdVoltage(voltage).withTimeout(0.5) + () -> RobotMap.Component.intake.c_holdVoltage(voltage).withTimeout(0.5) .andThen(RobotMap.Component.intake.c_holdVoltage(0)) .andThen(c_posReturnToHomeUp()) ); @@ -129,14 +130,14 @@ public Command c_shootCubes(int shelf) { public Command c_holdArmPose(Pair emacs) { return c_holdArmPose(emacs.getFirst(), emacs.getSecond()); } - public Command c_holdArmPose(Pair emacs, Command onArrivalCommand) { - return c_holdArmPose(emacs.getFirst(), emacs.getSecond(), onArrivalCommand); + public Command c_holdArmPose(Pair emacs, Supplier onArrivalCommandDealer) { + return c_holdArmPose(emacs.getFirst(), emacs.getSecond(), onArrivalCommandDealer); } public Command c_holdArmPose(double degreesFromHorizontal, double extensionLengthMeters) { return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters, null); } - public Command c_holdArmPose(double degreesFromHorizontal, double extensionLengthMeters, Command onArrivalCommand) { + public Command c_holdArmPose(double degreesFromHorizontal, double extensionLengthMeters, Supplier onArrivalCommandDealer) { var cmd = this.runOnce(() -> { Command firstCommand; Command secondCommand; @@ -162,7 +163,7 @@ public Command c_holdArmPose(double degreesFromHorizontal, double extensionLengt firstCommand.schedule(); (new SequentialCommandGroup(new WaitCommand(wait), secondCommand)).schedule(); - if (onArrivalCommand != null) (new SequentialCommandGroup(new WaitCommand(secondWait), onArrivalCommand)).schedule(); + if (onArrivalCommandDealer != null) (new SequentialCommandGroup(new WaitCommand(secondWait), onArrivalCommandDealer.get())).schedule(); }); // long story. basically, parallel command group requires it's subcommands' requirements. however, we want one subcommand to be able to die wihle the other one lives, so we just do this instead and leak commands. it's fine because they'll get cleaned up when their atomic base subsystems gets taken over by new commands cmd.setName("arm - c_holdArmPose"); return cmd; From 880687fcf34e19163802e1971e4b4b414e339575 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Sun, 2 Apr 2023 17:44:37 -0700 Subject: [PATCH 34/63] Minor tuning --- src/main/java/org/usfirst/frc4904/robot/Robot.java | 1 + src/main/java/org/usfirst/frc4904/robot/RobotMap.java | 2 +- .../frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java | 2 +- .../frc4904/robot/subsystems/arm/ArmPivotSubsystem.java | 4 ++-- .../usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java | 6 +++--- 5 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index b224d5d..cc4eadc 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -121,6 +121,7 @@ public void teleopExecute() { // SmartDashboard.putBoolean("isFlipped - IMPORTANT", NathanGain.isFlippy); SmartDashboard.putNumber("gyroooo", RobotMap.Component.navx.getAngle()); SmartDashboard.putNumber("arm extension length", RobotMap.Component.arm.armExtensionSubsystem.getCurrentExtensionLength()); + SmartDashboard.putNumber("zeroing", RobotMap.Component.arm.armExtensionSubsystem.motor.getSensorPositionRotations()); SmartDashboard.putNumber("arm pivot angle", RobotMap.Component.arm.armPivotSubsystem.getCurrentAngleDegrees()); // SmartDashboard.putNumber("Falcon temp", RobotContainer2.Component.leftATalonFX.getTemperature()); diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index bc63c9f..5766e57 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -211,7 +211,7 @@ public RobotMap() { TalonMotorSubsystem armRotationMotors = new TalonMotorSubsystem("Arm Pivot Subsystem", NeutralMode.Brake, 0, leftPivotMotor, rightPivotMotor); ArmExtensionSubsystem armExtensionSubsystem = new ArmExtensionSubsystem( new TalonMotorSubsystem("Arm Extension Subsystem", NeutralMode.Brake, 0, armExtensionMotor), - () -> ArmPivotSubsystem.motorRevsToAngle(armRotationMotors.getSensorPositionRotations()) + () -> ArmPivotSubsystem.motorRevsToAngle(armRotationMotors.getSensorPositionRotations() * 0.911 - 6.3) ); // Autonomous.autonCommand = Component.chassis.c_buildPathPlannerAuto( // PID.Drive.kS, PID.Drive.kV, PID.Drive.kA, diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java index 72aedd8..42526e0 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java @@ -56,7 +56,7 @@ public void initializeEncoderPositions(double meters) { } public double getCurrentExtensionLength() { - return revsToExtensionLength(motor.getSensorPositionRotations()) * 0.968 -0.0853; + return revsToExtensionLength(motor.getSensorPositionRotations()) * 0.968 - 0.0853; } public void setVoltageSafely(double voltage) { diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java index c7c4680..c79a68f 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java @@ -59,8 +59,8 @@ public class ArmPivotSubsystem extends SubsystemBase { public static final double kG_extended = 0.6; // TODO: tune - public static final double kP = 0;//0.04; //extended: .36915 retracted: .01464 - public static final double kI = 0;//0.01; + public static final double kP = 0.06;//;//0.04; //extended: .36915 retracted: .01464 + public static final double kI = 0.02;//0.01; public static final double kD = 0; public final TalonMotorSubsystem armMotorGroup; diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index f27e790..241c51a 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -38,7 +38,7 @@ public class ArmSubsystem extends SubsystemBase { static { // cones.put(1, new Triple<>(-19., Units.inchesToMeters(0), 3.)); floorCones.put(2, new Triple<>(29., Units.inchesToMeters(16), 3.)); - floorCones.put(3, new Triple<>(31., ArmExtensionSubsystem.MAX_EXTENSION_M, 3.)); + floorCones.put(3, new Triple<>(29., ArmExtensionSubsystem.MAX_EXTENSION_M-0.02, 3.)); } public static HashMap> cones = floorCones; @@ -47,7 +47,7 @@ public class ArmSubsystem extends SubsystemBase { static { // cubes.put(1, new Triple<>(-33., Units.inchesToMeters(0), 3.)); cubes.put(2, new Triple<>(15., Units.inchesToMeters(0), 4.5)); - cubes.put(3, new Triple<>(22., Units.inchesToMeters(0), 4.5)); + cubes.put(3, new Triple<>(20., Units.inchesToMeters(0), 4.5)); } public static final HashMap> otherPositions = new HashMap<>(); @@ -55,7 +55,7 @@ public class ArmSubsystem extends SubsystemBase { // https://docs.google.com/spreadsheets/d/1B7Ie4efOpuZb4UQsk8lHycGvi6BspnF74DUMLmiKGUM/edit#gid=0 in degrees, meters otherPositions.put("homeUp", new Pair<>(70., Units.inchesToMeters(0.))); // TODO: get number @thomasrimer otherPositions.put("homeDown", new Pair<>(-37., Units.inchesToMeters(0.))); - otherPositions.put("intakeShelf", new Pair<>(31., Units.inchesToMeters(20.))); + otherPositions.put("intakeShelf", new Pair<>(25., Units.inchesToMeters(20.))); } From ae877018ef778f0256f2aa363a9f67523929cf63 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Sun, 2 Apr 2023 20:58:41 -0700 Subject: [PATCH 35/63] UT, partial: trigger command factory, changes... changes command factory semantics from construction time to initialize time --- .../operators/DefaultOperator.java | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java index f8bfa16..c96e274 100644 --- a/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java @@ -2,6 +2,7 @@ import org.usfirst.frc4904.robot.RobotMap; import org.usfirst.frc4904.robot.humaninterface.drivers.NathanGain; +import org.usfirst.frc4904.standard.commands.TriggerCommandFactory; import org.usfirst.frc4904.standard.humaninput.Operator; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -42,20 +43,20 @@ public void bindCommands() { // position + place cube - joystick.button7.onTrue(RobotMap.Component.arm.c_shootCubes(3)); - joystick.button9.onTrue(RobotMap.Component.arm.c_shootCubes(2)); + joystick.button7 .onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_shootCubes(3))); + joystick.button9 .onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_shootCubes(2))); // position cone - joystick.button8.onTrue(RobotMap.Component.arm.c_shootCones(3)); - joystick.button10.onTrue(RobotMap.Component.arm.c_shootCones(2)); + joystick.button8 .onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_shootCones(3))); + joystick.button10.onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_shootCones(2))); // intake positions - joystick.button6.onTrue(RobotMap.Component.arm.c_posIntakeShelf()); - joystick.button4.onTrue(RobotMap.Component.arm.c_posIntakeFloor()); + joystick.button6 .onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posIntakeShelf())); + joystick.button4 .onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posIntakeFloor())); // stow positions - joystick.button11.onTrue(RobotMap.Component.arm.c_posReturnToHomeDown()); - joystick.button12.onTrue(RobotMap.Component.arm.c_posReturnToHomeUp()); + joystick.button11.onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posReturnToHomeDown())); + joystick.button12.onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posReturnToHomeUp())); } } From ed05aa42d29cca4888fa2937eb1a4ebb60c0620b Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Sun, 2 Apr 2023 20:59:39 -0700 Subject: [PATCH 36/63] Killed slacky encoder --- src/main/java/org/usfirst/frc4904/robot/Robot.java | 7 ++++--- .../robot/humaninterface/operators/DefaultOperator.java | 2 +- .../frc4904/robot/subsystems/arm/ArmPivotSubsystem.java | 8 -------- .../frc4904/robot/subsystems/arm/ArmSubsystem.java | 6 +++--- 4 files changed, 8 insertions(+), 15 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index cc4eadc..2e3a961 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -22,6 +22,7 @@ import edu.wpi.first.math.controller.DifferentialDriveWheelVoltages; import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; @@ -99,10 +100,9 @@ public void teleopInitialize() { ) ); - // Intake // FIXME: use nameCommand to make it cleaner with expresions (no varibales) - var cmdnull = RobotMap.Component.intake.c_holdVoltage(0); + // var cmdnull = RobotMap.Component.intake.c_holdVoltage(0); } @@ -119,11 +119,12 @@ public void teleopExecute() { // SmartDashboard.putBoolean("isFlipped - IMPORTANT", NathanGain.isFlippy); + // if (RobotMap.HumanInput.Operator.joystick.getPOV()) { SmartDashboard.putNumber("gyroooo", RobotMap.Component.navx.getAngle()); SmartDashboard.putNumber("arm extension length", RobotMap.Component.arm.armExtensionSubsystem.getCurrentExtensionLength()); SmartDashboard.putNumber("zeroing", RobotMap.Component.arm.armExtensionSubsystem.motor.getSensorPositionRotations()); SmartDashboard.putNumber("arm pivot angle", RobotMap.Component.arm.armPivotSubsystem.getCurrentAngleDegrees()); - + //} // SmartDashboard.putNumber("Falcon temp", RobotContainer2.Component.leftATalonFX.getTemperature()); diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java index f8bfa16..71bf404 100644 --- a/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java @@ -28,7 +28,7 @@ public void bindCommands() { // Intake // FIXME: use nameCommand to make it cleaner with expresions (no varibales) var zeroIntake = RobotMap.Component.intake.c_holdVoltage(0); - var holdPiece = RobotMap.Component.intake.c_holdVoltage(-2).withTimeout(0.5).andThen(RobotMap.Component.intake.c_holdVoltage(-1)); + var holdPiece = RobotMap.Component.intake.c_holdVoltage(-2).withTimeout(0.5).andThen(RobotMap.Component.intake.c_holdVoltage(-1.6)); var runIntake = RobotMap.Component.intake.c_holdVoltage(-8); var runOuttake = RobotMap.Component.intake.c_holdVoltage(3); diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java index 7980872..43dcb10 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java @@ -66,18 +66,11 @@ public class ArmPivotSubsystem extends SubsystemBase { public final TalonMotorSubsystem armMotorGroup; public final TelescopingArmPivotFeedForward feedforward; public final DoubleSupplier extensionDealerMeters; - private final EncoderWithSlack slackyEncoder; public ArmPivotSubsystem(TalonMotorSubsystem armMotorGroup, DoubleSupplier extensionDealerMeters) { this.armMotorGroup = armMotorGroup; this.extensionDealerMeters = () -> extensionDealerMeters.getAsDouble(); this.feedforward = new TelescopingArmPivotFeedForward(kG_retracted, kG_extended, kS, kV, kA); - this.slackyEncoder = new EncoderWithSlack( - GEARBOX_SLACK_DEGREES, - armMotorGroup::getSensorPositionRotations, - Units.rotationsToDegrees(1/GEARBOX_RATIO), - true - ); } public double getCurrentAngleDegrees() { @@ -91,7 +84,6 @@ public double getCurrentAngleDegrees() { */ public void initializeEncoderPositions() { armMotorGroup.zeroSensors(angleToMotorRevs(HARD_STOP_ARM_ANGLE)); - slackyEncoder.zeroSlackDirection(true); } public static double motorRevsToAngle(double revs) { diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index 1e8b354..dd6d103 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -30,15 +30,15 @@ public class ArmSubsystem extends SubsystemBase { public static final HashMap> shelfCones = new HashMap<>(); //in degrees, meters static { // cones.put(1, new Triple<>(-19., Units.inchesToMeters(0), 3.)); - shelfCones.put(2, new Triple<>(29., Units.inchesToMeters(19), 3.2)); - shelfCones.put(3, new Triple<>(41., ArmExtensionSubsystem.MAX_EXTENSION_M, 3.2)); + shelfCones.put(2, new Triple<>(29., Units.inchesToMeters(16), 3.)); + shelfCones.put(3, new Triple<>(41., ArmExtensionSubsystem.MAX_EXTENSION_M, 3.)); shelfCones.put(4, new Triple<>(180.0-41, ArmExtensionSubsystem.MAX_EXTENSION_M, 4.)); } public static final HashMap> floorCones = new HashMap<>(); //in degrees, meters static { // cones.put(1, new Triple<>(-19., Units.inchesToMeters(0), 3.)); - floorCones.put(2, new Triple<>(29., Units.inchesToMeters(16), 3.)); + floorCones.put(2, new Triple<>(29., Units.inchesToMeters(14), 3.)); floorCones.put(3, new Triple<>(29., ArmExtensionSubsystem.MAX_EXTENSION_M-0.02, 3.)); } From 5163c2733d82c0962f6e748bd117c147e6fe4c64 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Sun, 2 Apr 2023 21:23:37 -0700 Subject: [PATCH 37/63] theoretically only lying to the feed forward now --- .../subsystems/arm/ArmExtensionSubsystem.java | 18 ++++++++++++------ .../subsystems/arm/ArmPivotSubsystem.java | 6 +++++- src/main/java/org/usfirst/frc4904/standard | 2 +- 3 files changed, 18 insertions(+), 8 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java index 1264bbb..6e2e5d9 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java @@ -95,12 +95,18 @@ public Command c_controlVelocity(DoubleSupplier metersPerSecondSupplier) { } public Pair c_holdExtension(double extensionLengthMeters, double maxVelocity, double maxAcceleration) { - ezControl controller = new ezControl(kP, kI, kD, - (double position, double velocity) -> this.feedforward.calculate( - Units.degreesToRadians(angleDealer_DEG.getAsDouble()) - (Math.PI/2), - velocity, - 0 - )); + ezControl controller = new ezControl( + kP, kI, kD, + (double position, double velocity) -> this.feedforward.calculate( + Units.degreesToRadians(angleDealer_DEG.getAsDouble()) - (Math.PI/2), + velocity, + 0 + )) { + @Override + public void updateSetpoint(double setpoint, double setpoint_dt) { + super.updateSetpoint(setpoint * 0.968 - 0.0853, setpoint_dt); + } + }; TrapezoidProfile profile = new TrapezoidProfile(new TrapezoidProfile.Constraints(maxVelocity, maxAcceleration), new TrapezoidProfile.State((extensionLengthMeters - 0.0853)/0.968, 0), diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java index 43dcb10..a689fc2 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java @@ -129,7 +129,11 @@ public Pair c_holdRotation(double degreesFromHorizontal, double SmartDashboard.putNumber("Intended voltage", maxAccelDegPerSecSquare); return brr; } - ); + ){ + @Override + public void updateSetpoint(double setpoint, double setpoint_dt) { + super.updateSetpoint(setpoint * 0.911 - 6.3, setpoint_dt); + }; TrapezoidProfile profile = new TrapezoidProfile( new TrapezoidProfile.Constraints(maxVelDegPerSec, maxAccelDegPerSecSquare), diff --git a/src/main/java/org/usfirst/frc4904/standard b/src/main/java/org/usfirst/frc4904/standard index f8afa81..1af05e3 160000 --- a/src/main/java/org/usfirst/frc4904/standard +++ b/src/main/java/org/usfirst/frc4904/standard @@ -1 +1 @@ -Subproject commit f8afa8106e399f23d923d7278cc2b4ab7c740c5d +Subproject commit 1af05e38d023a6e056a9047ed1970b75a815d790 From bc6b409f06842a35f73596729e8a0db90eae90e5 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Sun, 2 Apr 2023 21:55:46 -0700 Subject: [PATCH 38/63] UT: redo arm subsystems with onArrivalCommands --- .../java/org/usfirst/frc4904/robot/Robot.java | 6 -- .../subsystems/arm/ArmExtensionSubsystem.java | 12 +++- .../subsystems/arm/ArmPivotSubsystem.java | 13 +++- .../robot/subsystems/arm/ArmSubsystem.java | 62 ++++++++++--------- src/main/java/org/usfirst/frc4904/standard | 2 +- 5 files changed, 53 insertions(+), 42 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index b224d5d..732c60d 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -98,12 +98,6 @@ public void teleopInitialize() { RobotMap.Component.arm.armPivotSubsystem.c_controlAngularVelocity(pivot_getter::getAsDouble) ) ); - - - // Intake - // FIXME: use nameCommand to make it cleaner with expresions (no varibales) - var cmdnull = RobotMap.Component.intake.c_holdVoltage(0); - } @Override diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java index 1c29919..e196a64 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java @@ -4,8 +4,10 @@ package org.usfirst.frc4904.robot.subsystems.arm; import java.util.function.DoubleSupplier; +import java.util.function.Supplier; import org.opencv.core.Mat.Tuple2; +import org.usfirst.frc4904.standard.commands.TriggerCommandFactory; import org.usfirst.frc4904.standard.custom.motioncontrollers.ezControl; import org.usfirst.frc4904.standard.custom.motioncontrollers.ezMotion; import org.usfirst.frc4904.standard.subsystems.motor.TalonMotorSubsystem; @@ -17,6 +19,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WrapperCommand; public class ArmExtensionSubsystem extends SubsystemBase { public static final double MAXIMUM_HORIZONTAL_SAFE_EXTENSION_M = Units.inchesToMeters(48); @@ -94,7 +98,7 @@ public Command c_controlVelocity(DoubleSupplier metersPerSecondSupplier) { return cmd; } - public Pair c_holdExtension(double extensionLengthMeters, double maxVelocity, double maxAcceleration) { + public Command c_holdExtension(double extensionLengthMeters, double maxVelocity, double maxAcceleration, Supplier onArrivalCommandDealer) { ezControl controller = new ezControl(kP, kI, kD, (double position, double velocity) -> this.feedforward.calculate( Units.degreesToRadians(angleDealer_DEG.getAsDouble()) - (Math.PI/2), @@ -105,9 +109,11 @@ public Pair c_holdExtension(double extensionLengthMeters, doubl TrapezoidProfile profile = new TrapezoidProfile(new TrapezoidProfile.Constraints(maxVelocity, maxAcceleration), new TrapezoidProfile.State((extensionLengthMeters - 0.0853)/0.968, 0), new TrapezoidProfile.State((getCurrentExtensionLength() - 0.0853)/0.968, 0)); - var cmd = new ezMotion(controller, this::getCurrentExtensionLength, motor::setVoltage, (double t) -> new Tuple2(profile.calculate(t).position, profile.calculate(t).velocity), this, motor); + var cmd = new ezMotion(controller, this::getCurrentExtensionLength, motor::setVoltage, () -> (double t) -> new Tuple2(profile.calculate(t).position, profile.calculate(t).velocity), this, motor); cmd.setName("arm - c_holdExtension"); - return new Pair(cmd, profile.totalTime()); + return onArrivalCommandDealer == null ? cmd : + cmd.alongWith((new WaitCommand(profile.totalTime()).andThen( + new TriggerCommandFactory(onArrivalCommandDealer)))); } } diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java index 132a1e0..cfe64a7 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java @@ -1,8 +1,12 @@ package org.usfirst.frc4904.robot.subsystems.arm; +import java.util.function.DoubleFunction; import java.util.function.DoubleSupplier; +import java.util.function.Supplier; import org.opencv.core.Mat.Tuple2; +import org.usfirst.frc4904.standard.commands.Noop; +import org.usfirst.frc4904.standard.commands.TriggerCommandFactory; import org.usfirst.frc4904.standard.custom.motioncontrollers.ezControl; import org.usfirst.frc4904.standard.custom.motioncontrollers.ezMotion; import org.usfirst.frc4904.standard.subsystems.motor.TalonMotorSubsystem; @@ -14,6 +18,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.WaitCommand; public class ArmPivotSubsystem extends SubsystemBase { public static final double HARD_STOP_ARM_ANGLE = -38; @@ -124,7 +129,7 @@ public Command c_controlAngularVelocity(DoubleSupplier degPerSecDealer) { return cmd; } - public Pair c_holdRotation(double degreesFromHorizontal, double maxVelDegPerSec, double maxAccelDegPerSecSquare) { + public Command c_holdRotation(double degreesFromHorizontal, double maxVelDegPerSec, double maxAccelDegPerSecSquare, Supplier onArrivalCommandDealer) { ezControl controller = new ezControl( kP, kI, kD, (position, velocityDegPerSec) -> { @@ -144,6 +149,7 @@ public Pair c_holdRotation(double degreesFromHorizontal, double new TrapezoidProfile.State((degreesFromHorizontal + 6.3)/0.911, 0), new TrapezoidProfile.State((getCurrentAngleDegrees() + 6.3)/0.911, 0) ); + var cmd = new ezMotion( controller, () -> this.getCurrentAngleDegrees(), @@ -162,7 +168,8 @@ public Pair c_holdRotation(double degreesFromHorizontal, double cmd.setName("arm - c_holdRotation"); // return new Pair(new ezMotion(controller, () -> this.getCurrentAngleDegrees() * Math.PI / 180, armMotorGroup::setVoltage, // (double t) -> new Tuple2(profile.calculate(t).position, profile.calculate(t).velocity), this), profile.totalTime()); - return new Pair( - cmd, profile.totalTime()); + return onArrivalCommandDealer == null ? cmd : + cmd.alongWith((new WaitCommand(profile.totalTime())) + .andThen(new TriggerCommandFactory(onArrivalCommandDealer))); } } diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index 847e6bb..21b96f5 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -1,10 +1,13 @@ package org.usfirst.frc4904.robot.subsystems.arm; import java.util.HashMap; +import java.util.concurrent.Callable; +import java.util.function.Function; import java.util.function.Supplier; import org.usfirst.frc4904.robot.RobotMap; import org.usfirst.frc4904.robot.subsystems.Intake; +import org.usfirst.frc4904.standard.commands.Noop; import org.usfirst.frc4904.standard.custom.Triple; import org.usfirst.frc4904.robot.humaninterface.drivers.NathanGain; @@ -138,34 +141,35 @@ public Command c_holdArmPose(double degreesFromHorizontal, double extensionLengt } public Command c_holdArmPose(double degreesFromHorizontal, double extensionLengthMeters, Supplier onArrivalCommandDealer) { - var cmd = this.runOnce(() -> { - Command firstCommand; - Command secondCommand; - double wait; - double secondWait; - - Pair pivotMovement = armPivotSubsystem.c_holdRotation(degreesFromHorizontal, - MAX_VELOCITY_PIVOT, MAX_ACCEL_PIVOT); - Pair extensionMovement = armExtensionSubsystem.c_holdExtension(extensionLengthMeters, - MAX_VELOCITY_EXTENSION, MAX_ACCEL_EXTENSION); - - if ((extensionLengthMeters - armExtensionSubsystem.getCurrentExtensionLength()) > 0) { - firstCommand = pivotMovement.getFirst(); - wait = pivotMovement.getSecond(); - secondCommand = extensionMovement.getFirst(); - secondWait = wait + extensionMovement.getSecond(); - } else { - firstCommand = extensionMovement.getFirst(); - wait = extensionMovement.getSecond(); - secondCommand = pivotMovement.getFirst(); - secondWait = wait + pivotMovement.getSecond(); - } - - firstCommand.schedule(); - (new SequentialCommandGroup(new WaitCommand(wait), secondCommand)).schedule(); - if (onArrivalCommandDealer != null) (new SequentialCommandGroup(new WaitCommand(secondWait), onArrivalCommandDealer.get())).schedule(); - }); // long story. basically, parallel command group requires it's subcommands' requirements. however, we want one subcommand to be able to die wihle the other one lives, so we just do this instead and leak commands. it's fine because they'll get cleaned up when their atomic base subsystems gets taken over by new commands - cmd.setName("arm - c_holdArmPose"); - return cmd; + final Supplier sanitizedArrivalCommandDealer = onArrivalCommandDealer != null ? onArrivalCommandDealer : () -> new Noop(); + + Function, Command> pivotAndThen = (then) -> armPivotSubsystem.c_holdRotation(degreesFromHorizontal, MAX_VELOCITY_PIVOT, MAX_ACCEL_PIVOT, then); + Function, Command> extendAndThen = (then) -> armExtensionSubsystem.c_holdExtension(extensionLengthMeters, MAX_VELOCITY_EXTENSION, MAX_ACCEL_EXTENSION, then); + + if (extensionLengthMeters > armExtensionSubsystem.getCurrentExtensionLength()) { + return pivotAndThen.apply(() -> extendAndThen.apply(sanitizedArrivalCommandDealer)); + } else { + return extendAndThen.apply(() -> pivotAndThen.apply(sanitizedArrivalCommandDealer)); + } + + // if ((extensionLengthMeters - armExtensionSubsystem.getCurrentExtensionLength()) > 0) { + // firstCommand = pivotMovement.getFirst(); + // wait = pivotMovement.getSecond(); + // secondCommand = extensionMovement.getFirst(); + // secondWait = wait + extensionMovement.getSecond(); + // } else { + // firstCommand = extensionMovement.getFirst(); + // wait = extensionMovement.getSecond(); + // secondCommand = pivotMovement.getFirst(); + // secondWait = wait + pivotMovement.getSecond(); + // } + + // firstCommand.schedule(); + // (new SequentialCommandGroup(new WaitCommand(wait), secondCommand)).schedule(); + // if (onArrivalCommandDealer != null) (new SequentialCommandGroup(new WaitCommand(secondWait), onArrivalCommandDealer.get())).schedule(); + + // }); // long story. basically, parallel command group requires its subcommands' requirements. however, we want one subcommand to be able to die wihle the other one lives, so we just do this instead and leak commands. it's fine because they'll get cleaned up when their atomic base subsystems gets taken over by new commands + // cmd.setName("arm - c_holdArmPose"); + // return cmd; } } diff --git a/src/main/java/org/usfirst/frc4904/standard b/src/main/java/org/usfirst/frc4904/standard index f8afa81..3ce0092 160000 --- a/src/main/java/org/usfirst/frc4904/standard +++ b/src/main/java/org/usfirst/frc4904/standard @@ -1 +1 @@ -Subproject commit f8afa8106e399f23d923d7278cc2b4ab7c740c5d +Subproject commit 3ce009290aeac7107fef9206a7e5c90d2f355fb3 From e8de5e96834176ae5366837806788c373de88817 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Sun, 2 Apr 2023 21:56:30 -0700 Subject: [PATCH 39/63] Funny --- src/main/java/org/usfirst/frc4904/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 2e3a961..9498117 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -119,7 +119,7 @@ public void teleopExecute() { // SmartDashboard.putBoolean("isFlipped - IMPORTANT", NathanGain.isFlippy); - // if (RobotMap.HumanInput.Operator.joystick.getPOV()) { + // if (RobotMap.HumanInput.Operator.joystick.getPOV() != 0) { SmartDashboard.putNumber("gyroooo", RobotMap.Component.navx.getAngle()); SmartDashboard.putNumber("arm extension length", RobotMap.Component.arm.armExtensionSubsystem.getCurrentExtensionLength()); SmartDashboard.putNumber("zeroing", RobotMap.Component.arm.armExtensionSubsystem.motor.getSensorPositionRotations()); From fbba71b37c5b6d9830a646c0dcd900d7a4e4bd4b Mon Sep 17 00:00:00 2001 From: Exr0n Date: Sun, 2 Apr 2023 22:29:31 -0700 Subject: [PATCH 40/63] not sure why secondary holdArmPose is being scheduled prematurely --- .../robot/subsystems/arm/ArmExtensionSubsystem.java | 12 +++++++++--- .../robot/subsystems/arm/ArmPivotSubsystem.java | 12 +++++++++--- .../frc4904/robot/subsystems/arm/ArmSubsystem.java | 5 +++-- 3 files changed, 21 insertions(+), 8 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java index 73d5ebd..e1599de 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java @@ -18,6 +18,8 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WrapperCommand; @@ -117,9 +119,13 @@ public void updateSetpoint(double setpoint, double setpoint_dt) { new TrapezoidProfile.State((getCurrentExtensionLength() - 0.0853)/0.968, 0)); var cmd = new ezMotion(controller, this::getCurrentExtensionLength, motor::setVoltage, () -> (double t) -> new Tuple2(profile.calculate(t).position, profile.calculate(t).velocity), this, motor); cmd.setName("arm - c_holdExtension"); - return onArrivalCommandDealer == null ? cmd : - cmd.alongWith((new WaitCommand(profile.totalTime()).andThen( - new TriggerCommandFactory(onArrivalCommandDealer)))); + return onArrivalCommandDealer == null ? cmd : new ParallelCommandGroup( + cmd, + new SequentialCommandGroup( + new WaitCommand(profile.totalTime()), + new TriggerCommandFactory(onArrivalCommandDealer) + ) + ); } } diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java index d5c9760..717958b 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java @@ -17,6 +17,8 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -165,8 +167,12 @@ public void updateSetpoint(double setpoint, double setpoint_dt) { cmd.setName("arm - c_holdRotation"); // return new Pair(new ezMotion(controller, () -> this.getCurrentAngleDegrees() * Math.PI / 180, armMotorGroup::setVoltage, // (double t) -> new Tuple2(profile.calculate(t).position, profile.calculate(t).velocity), this), profile.totalTime()); - return onArrivalCommandDealer == null ? cmd : - cmd.alongWith((new WaitCommand(profile.totalTime())) - .andThen(new TriggerCommandFactory(onArrivalCommandDealer))); + return onArrivalCommandDealer == null ? cmd : new ParallelCommandGroup( + cmd, + new SequentialCommandGroup( + new WaitCommand(profile.totalTime()), + new TriggerCommandFactory(onArrivalCommandDealer) + ) + ); } } diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index df55c37..1c54167 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -8,6 +8,7 @@ import org.usfirst.frc4904.robot.RobotMap; import org.usfirst.frc4904.robot.subsystems.Intake; import org.usfirst.frc4904.standard.commands.Noop; +import org.usfirst.frc4904.standard.commands.TriggerCommandFactory; import org.usfirst.frc4904.standard.custom.Triple; import org.usfirst.frc4904.robot.humaninterface.drivers.NathanGain; @@ -69,12 +70,12 @@ public ArmSubsystem(ArmPivotSubsystem armPivotSubsystem, ArmExtensionSubsystem a } public Command c_posReturnToHomeUp() { - var cmd = c_holdArmPose(otherPositions.get("homeUp")); + var cmd = new TriggerCommandFactory(() -> c_holdArmPose(otherPositions.get("homeUp"))); cmd.setName("arm position - home (up)"); return cmd; } public Command c_posReturnToHomeDown() { - var cmd = c_holdArmPose(otherPositions.get("homeDown")); + var cmd = new TriggerCommandFactory(() -> c_holdArmPose(otherPositions.get("homeDown"))); cmd.setName("arm position - home (down)"); return cmd; } From 1d509cd816dd61dfcc838994f22a131d5613dcaa Mon Sep 17 00:00:00 2001 From: zbuster05 Date: Mon, 3 Apr 2023 14:39:20 -0500 Subject: [PATCH 41/63] theoretically exponential now --- src/main/java/org/usfirst/frc4904/robot/Robot.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 9498117..7af398d 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -38,6 +38,10 @@ public class Robot extends CommandRobotBase { private final Driver driver = new NathanGain(); private final org.usfirst.frc4904.standard.humaninput.Operator operator = new DefaultOperator(); + protected double scaleGain(double input, double gain, double exp) { + return Math.pow(Math.abs(input), exp) * gain * Math.signum(input); + } + @Override public void initialize() { } @@ -93,7 +97,7 @@ public void teleopInitialize() { // RobotMap.Component.arm.c_posReturnToHomeUp(NathanGain.isFlippy) // )); - final DoubleSupplier pivot_getter = () -> RobotMap.HumanInput.Operator.joystick.getAxis(1) * 50; + final DoubleSupplier pivot_getter = () -> scaleGain(RobotMap.HumanInput.Operator.joystick.getAxis(1), 60, 3); (new Trigger(() -> pivot_getter.getAsDouble() != 0)).onTrue( nameCommand("arm - teleop - armPivot operator override", RobotMap.Component.arm.armPivotSubsystem.c_controlAngularVelocity(pivot_getter::getAsDouble) From 531c211deb0a2a2342111f3ca9cb189953a0fc8e Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Mon, 3 Apr 2023 14:10:47 -0700 Subject: [PATCH 42/63] use new TriggerCommandFactory; name commands --- src/main/java/org/usfirst/frc4904/robot/Robot.java | 3 +++ .../robot/humaninterface/operators/DefaultOperator.java | 1 - .../frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java | 2 +- .../frc4904/robot/subsystems/arm/ArmPivotSubsystem.java | 2 +- src/main/java/org/usfirst/frc4904/standard | 2 +- 5 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 9498117..4f6522d 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -127,6 +127,9 @@ public void teleopExecute() { //} // SmartDashboard.putNumber("Falcon temp", RobotContainer2.Component.leftATalonFX.getTemperature()); + SmartDashboard.putData(RobotMap.Component.arm.armPivotSubsystem); + SmartDashboard.putData(RobotMap.Component.arm.armExtensionSubsystem); + } diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java index bf5f450..2325ee5 100644 --- a/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java @@ -58,5 +58,4 @@ public void bindCommands() { joystick.button11.onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posReturnToHomeDown())); joystick.button12.onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posReturnToHomeUp())); } - } diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java index e1599de..2ccd342 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java @@ -123,7 +123,7 @@ public void updateSetpoint(double setpoint, double setpoint_dt) { cmd, new SequentialCommandGroup( new WaitCommand(profile.totalTime()), - new TriggerCommandFactory(onArrivalCommandDealer) + new TriggerCommandFactory("arm extension", onArrivalCommandDealer) ) ); } diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java index 717958b..34e4822 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java @@ -171,7 +171,7 @@ public void updateSetpoint(double setpoint, double setpoint_dt) { cmd, new SequentialCommandGroup( new WaitCommand(profile.totalTime()), - new TriggerCommandFactory(onArrivalCommandDealer) + new TriggerCommandFactory("arm pivot", onArrivalCommandDealer) ) ); } diff --git a/src/main/java/org/usfirst/frc4904/standard b/src/main/java/org/usfirst/frc4904/standard index 3ce0092..4c837b4 160000 --- a/src/main/java/org/usfirst/frc4904/standard +++ b/src/main/java/org/usfirst/frc4904/standard @@ -1 +1 @@ -Subproject commit 3ce009290aeac7107fef9206a7e5c90d2f355fb3 +Subproject commit 4c837b436382b830be0d8091b2749af8b843f946 From 7f08301c510f326917a24c2e065001894fe812eb Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Mon, 3 Apr 2023 14:12:08 -0700 Subject: [PATCH 43/63] tune arm pivot trapezoidal and stowdown hashmap angles --- .../robot/seenoevil/RobotContainer2.java | 306 +++++++++--------- .../robot/subsystems/arm/ArmSubsystem.java | 6 +- 2 files changed, 156 insertions(+), 156 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java index 18d74eb..f4de08f 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java @@ -313,176 +313,176 @@ public Trajectory getTrajectory(String trajectoryName) { return trajectories.get(trajectoryName); } - public Command balanceAuton(Supplier wheelSpeeds, BiConsumer outputVolts){ - var command = new SequentialCommandGroup( - //1. Position arm to place gamepiece - // TODO: options: either place the game picee, or try to flip over, shoot, and then come back so that we are in the same state - - // implement going over and shooting a cone? - - new ParallelCommandGroup( - //3. Retract arm - // RobotMap.Component.arm.c_posReturnToHomeDown(false), - new SequentialCommandGroup( - new WaitCommand(1), //TODO: set wait time to allow arm to get started before moving? - //4. Drive forward past ramp - getAutonomousCommand(getTrajectory("past_ramp")), - - //5. Drive back to get partially on ramp - getAutonomousCommand(getTrajectory("back_to_ramp")) - ) - ) - // new Balance(RobotMap.Component.navx, wheelSpeeds, outputVolts, 1, -0.1) - //6. balance code here - ); +// public Command balanceAuton(Supplier wheelSpeeds, BiConsumer outputVolts){ +// var command = new SequentialCommandGroup( +// //1. Position arm to place gamepiece +// // TODO: options: either place the game picee, or try to flip over, shoot, and then come back so that we are in the same state + +// // implement going over and shooting a cone? + +// new ParallelCommandGroup( +// //3. Retract arm +// // RobotMap.Component.arm.c_posReturnToHomeDown(false), +// new SequentialCommandGroup( +// new WaitCommand(1), //TODO: set wait time to allow arm to get started before moving? +// //4. Drive forward past ramp +// getAutonomousCommand(getTrajectory("past_ramp")), + +// //5. Drive back to get partially on ramp +// getAutonomousCommand(getTrajectory("back_to_ramp")) +// ) +// ) +// // new Balance(RobotMap.Component.navx, wheelSpeeds, outputVolts, 1, -0.1) +// //6. balance code here +// ); - return command; - } +// return command; +// } - public Command balanceAutonAndShootCube(Supplier wheelSpeeds, BiConsumer outputVolts){ +// public Command balanceAutonAndShootCube(Supplier wheelSpeeds, BiConsumer outputVolts){ - var command = new SequentialCommandGroup( - //1. Position arm to place gamepiece - // TODO: options: either place the game picee, or try to flip over, shoot, and then come back so that we are in the same state +// var command = new SequentialCommandGroup( +// //1. Position arm to place gamepiece +// // TODO: options: either place the game picee, or try to flip over, shoot, and then come back so that we are in the same state - // implement going over and shooting a cone? +// // implement going over and shooting a cone? - new ParallelCommandGroup( - //3. Retract arm - // RobotMap.Component.arm.c_posReturnToHomeDown(false), - RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getFirst() - .withTimeout(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()) - .andThen(new WaitCommand(0.8)) - .andThen(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getFirst().withTimeout(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getSecond()).andThen(new InstantCommand(() -> RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.setVoltage(0))) - ), - new SequentialCommandGroup( - new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 100).getSecond()), - RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.8).andThen(RobotMap.Component.intake.c_holdVoltage(0)) - ), - new SequentialCommandGroup( - new WaitCommand(2.5), //TODO: set wait time to allow arm to get started before moving? - //4. Drive forward past ramp - getAutonomousCommand(getTrajectory("past_ramp")), +// new ParallelCommandGroup( +// //3. Retract arm +// // RobotMap.Component.arm.c_posReturnToHomeDown(false), +// RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getFirst() +// .withTimeout(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()) +// .andThen(new WaitCommand(0.8)) +// .andThen(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getFirst().withTimeout(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getSecond()).andThen(new InstantCommand(() -> RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.setVoltage(0))) +// ), +// new SequentialCommandGroup( +// new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 100).getSecond()), +// RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.8).andThen(RobotMap.Component.intake.c_holdVoltage(0)) +// ), +// new SequentialCommandGroup( +// new WaitCommand(2.5), //TODO: set wait time to allow arm to get started before moving? +// //4. Drive forward past ramp +// getAutonomousCommand(getTrajectory("past_ramp")), - //5. Drive back to get partially on ramp - getAutonomousCommand(getTrajectory("back_to_ramp")) - ) - ) - // new Balance(RobotMap.Component.navx, wheelSpeeds, outputVolts, 1, -0.1) - //6. balance code here - ); +// //5. Drive back to get partially on ramp +// getAutonomousCommand(getTrajectory("back_to_ramp")) +// ) +// ) +// // new Balance(RobotMap.Component.navx, wheelSpeeds, outputVolts, 1, -0.1) +// //6. balance code here +// ); - return command; - } - - // public Command notBalanceAuton(){ - // var command = new SequentialCommandGroup( - // //1. Position arm to place gamepiece - // RobotMap.Component.arm.placeCube(2, true) //TODO: set actual timeout - // , - // new ParallelCommandGroup( - // //3. Retract arm - // RobotMap.Component.arm.c_posReturnToHomeUp(false), - // new SequentialCommandGroup( - // new WaitCommand(1), //TODO: set wait time to allow arm to get started before moving? - // //4. Drive forward past ramp - // getAutonomousCommand(getTrajectory("past_ramp")) - // ) - // ) - // ); +// return command; +// } + +// // public Command notBalanceAuton(){ +// // var command = new SequentialCommandGroup( +// // //1. Position arm to place gamepiece +// // RobotMap.Component.arm.placeCube(2, true) //TODO: set actual timeout +// // , +// // new ParallelCommandGroup( +// // //3. Retract arm +// // RobotMap.Component.arm.c_posReturnToHomeUp(false), +// // new SequentialCommandGroup( +// // new WaitCommand(1), //TODO: set wait time to allow arm to get started before moving? +// // //4. Drive forward past ramp +// // getAutonomousCommand(getTrajectory("past_ramp")) +// // ) +// // ) +// // ); - // return command; - // } +// // return command; +// // } - public Command newAuton() { - var command = new SequentialCommandGroup( - //1. Position arm to place gamepiece - // TODO: options: either place the game picee, or try to flip over, shoot, and then come back so that we are in the same state +// public Command newAuton() { +// var command = new SequentialCommandGroup( +// //1. Position arm to place gamepiece +// // TODO: options: either place the game picee, or try to flip over, shoot, and then come back so that we are in the same state - // implement going over and shooting a cone? +// // implement going over and shooting a cone? - new ParallelCommandGroup( - //3. Retract arm - // RobotMap.Component.arm.c_posReturnToHomeDown(false), - RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getFirst(), - new SequentialCommandGroup( - new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()), - RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.5), - RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getFirst().withTimeout(0.8) - ), +// new ParallelCommandGroup( +// //3. Retract arm +// // RobotMap.Component.arm.c_posReturnToHomeDown(false), +// RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getFirst(), +// new SequentialCommandGroup( +// new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()), +// RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.5), +// RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getFirst().withTimeout(0.8) +// ), - new SequentialCommandGroup( - new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()+0.5), - getAutonomousCommand(getTrajectory("to_ramp")), - getAutonomousCommand(getTrajectory("angle_ramp_forward")), - new WaitCommand(2), - getAutonomousCommand(getTrajectory("go_over_ramp")), - getAutonomousCommand(getTrajectory("angle_ramp_backward")), - new WaitCommand(2), - getAutonomousCommand(getTrajectory("go_middle_ramp")) - ) - ) - // new Balance(RobotMap.Component.navx, wheelSpeeds, outputVolts, 1, -0.1) - //6. balance code here - ); +// new SequentialCommandGroup( +// new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()+0.5), +// getAutonomousCommand(getTrajectory("to_ramp")), +// getAutonomousCommand(getTrajectory("angle_ramp_forward")), +// new WaitCommand(2), +// getAutonomousCommand(getTrajectory("go_over_ramp")), +// getAutonomousCommand(getTrajectory("angle_ramp_backward")), +// new WaitCommand(2), +// getAutonomousCommand(getTrajectory("go_middle_ramp")) +// ) +// ) +// // new Balance(RobotMap.Component.navx, wheelSpeeds, outputVolts, 1, -0.1) +// //6. balance code here +// ); - return command; +// return command; - } +// } - public Command shootCubeAndCross() { - var command = new SequentialCommandGroup( - //1. Position arm to place gamepiece - // TODO: options: either place the game picee, or try to flip over, shoot, and then come back so that we are in the same state +// public Command shootCubeAndCross() { +// var command = new SequentialCommandGroup( +// //1. Position arm to place gamepiece +// // TODO: options: either place the game picee, or try to flip over, shoot, and then come back so that we are in the same state - // implement going over and shooting a cone? +// // implement going over and shooting a cone? - new ParallelCommandGroup( - //3. Retract arm - // RobotMap.Component.arm.c_posReturnToHomeDown(false), - RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getFirst() - .withTimeout(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()) - .andThen(new WaitCommand(0.8)) - .andThen(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getFirst().withTimeout(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getSecond()).andThen(new InstantCommand(() -> RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.setVoltage(0))) - ), - new SequentialCommandGroup( - new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 100).getSecond()), - RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.8).andThen(RobotMap.Component.intake.c_holdVoltage(0)) - ), - new SequentialCommandGroup( - new WaitCommand(2.5), //TODO: set wait time to allow arm to get started before moving? - //4. Drive out of the community and stop right in front of the next game piece. - getAutonomousCommand(getTrajectory("go_to_pickup_next")) - - ) - ) - // new Balance(RobotMap.Component.navx, wheelSpeeds, outputVolts, 1, -0.1) - //6. balance code here - ); +// new ParallelCommandGroup( +// //3. Retract arm +// // RobotMap.Component.arm.c_posReturnToHomeDown(false), +// RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getFirst() +// .withTimeout(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()) +// .andThen(new WaitCommand(0.8)) +// .andThen(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getFirst().withTimeout(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getSecond()).andThen(new InstantCommand(() -> RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.setVoltage(0))) +// ), +// new SequentialCommandGroup( +// new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 100).getSecond()), +// RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.8).andThen(RobotMap.Component.intake.c_holdVoltage(0)) +// ), +// new SequentialCommandGroup( +// new WaitCommand(2.5), //TODO: set wait time to allow arm to get started before moving? +// //4. Drive out of the community and stop right in front of the next game piece. +// getAutonomousCommand(getTrajectory("go_to_pickup_next")) + +// ) +// ) +// // new Balance(RobotMap.Component.navx, wheelSpeeds, outputVolts, 1, -0.1) +// //6. balance code here +// ); - return command; - } - public Command shoot2auto(){ //shoot cone, grab cube, shoot cube, doesn't balance - var command = new SequentialCommandGroup( - RobotMap.Component.arm.c_shootCones(4), - new ParallelCommandGroup( - RobotMap.Component.arm.c_posReturnToHomeDown(), - getAutonomousCommand(getTrajectory("go_to_pickup_next")) - ), - RobotMap.Component.intake.c_holdVoltage(-8).withTimeout(0.5), //TODO: for picking up cone tune to make sure it works - new ParallelRaceGroup( - RobotMap.Component.intake.c_holdVoltage(-1), - getAutonomousCommand(getTrajectory("from_pickup_to_place")) - ), - new ParallelCommandGroup( // shootign cube - RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getFirst(), - new SequentialCommandGroup( - new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()), - RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.5), - RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getFirst().withTimeout(0.8) - )) - ); - return command; - } +// return command; +// } +// public Command shoot2auto(){ //shoot cone, grab cube, shoot cube, doesn't balance +// var command = new SequentialCommandGroup( +// RobotMap.Component.arm.c_shootCones(4), +// new ParallelCommandGroup( +// RobotMap.Component.arm.c_posReturnToHomeDown(), +// getAutonomousCommand(getTrajectory("go_to_pickup_next")) +// ), +// RobotMap.Component.intake.c_holdVoltage(-8).withTimeout(0.5), //TODO: for picking up cone tune to make sure it works +// new ParallelRaceGroup( +// RobotMap.Component.intake.c_holdVoltage(-1), +// getAutonomousCommand(getTrajectory("from_pickup_to_place")) +// ), +// new ParallelCommandGroup( // shootign cube +// RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getFirst(), +// new SequentialCommandGroup( +// new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()), +// RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.5), +// RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getFirst().withTimeout(0.8) +// )) +// ); +// return command; +// } } diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index 1c54167..1d27333 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -28,8 +28,8 @@ public class ArmSubsystem extends SubsystemBase { public static final double MAX_VELOCITY_EXTENSION = 1; public static final double MAX_ACCEL_EXTENSION = 2; - public static final double MAX_VELOCITY_PIVOT = 150; - public static final double MAX_ACCEL_PIVOT = 200; + public static final double MAX_VELOCITY_PIVOT = 160; + public static final double MAX_ACCEL_PIVOT = 210; public static final HashMap> shelfCones = new HashMap<>(); //in degrees, meters static { @@ -59,7 +59,7 @@ public class ArmSubsystem extends SubsystemBase { static { // https://docs.google.com/spreadsheets/d/1B7Ie4efOpuZb4UQsk8lHycGvi6BspnF74DUMLmiKGUM/edit#gid=0 in degrees, meters otherPositions.put("homeUp", new Pair<>(70., Units.inchesToMeters(0.))); // TODO: get number @thomasrimer - otherPositions.put("homeDown", new Pair<>(-37., Units.inchesToMeters(0.))); + otherPositions.put("homeDown", new Pair<>(-41., -0.1)); otherPositions.put("intakeShelf", new Pair<>(25., Units.inchesToMeters(20.))); } From 8823a03e35b4ec8de5796163bf9835fd9a816d31 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Mon, 3 Apr 2023 14:37:06 -0700 Subject: [PATCH 44/63] stow up after shoot; differentiate b/w shelf and floor intake --- .../robot/subsystems/arm/ArmSubsystem.java | 24 ++++++++++++------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index 1d27333..1e5d8ed 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -15,7 +15,7 @@ import edu.wpi.first.math.Pair; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; - +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -31,16 +31,18 @@ public class ArmSubsystem extends SubsystemBase { public static final double MAX_VELOCITY_PIVOT = 160; public static final double MAX_ACCEL_PIVOT = 210; + // SHELF CONES public static final HashMap> shelfCones = new HashMap<>(); //in degrees, meters - static { + static { // SHELF CONES // cones.put(1, new Triple<>(-19., Units.inchesToMeters(0), 3.)); shelfCones.put(2, new Triple<>(29., Units.inchesToMeters(16), 3.)); shelfCones.put(3, new Triple<>(41., ArmExtensionSubsystem.MAX_EXTENSION_M, 3.)); shelfCones.put(4, new Triple<>(180.0-41, ArmExtensionSubsystem.MAX_EXTENSION_M, 4.)); } + // FLOOR CONES public static final HashMap> floorCones = new HashMap<>(); //in degrees, meters - static { + static { // FLOOR CONES // cones.put(1, new Triple<>(-19., Units.inchesToMeters(0), 3.)); floorCones.put(2, new Triple<>(29., Units.inchesToMeters(14), 3.)); floorCones.put(3, new Triple<>(29., ArmExtensionSubsystem.MAX_EXTENSION_M-0.02, 3.)); @@ -58,7 +60,7 @@ public class ArmSubsystem extends SubsystemBase { public static final HashMap> otherPositions = new HashMap<>(); static { // https://docs.google.com/spreadsheets/d/1B7Ie4efOpuZb4UQsk8lHycGvi6BspnF74DUMLmiKGUM/edit#gid=0 in degrees, meters - otherPositions.put("homeUp", new Pair<>(70., Units.inchesToMeters(0.))); // TODO: get number @thomasrimer + otherPositions.put("homeUp", new Pair<>(65., Units.inchesToMeters(0.))); // TODO: get number @thomasrimer otherPositions.put("homeDown", new Pair<>(-41., -0.1)); otherPositions.put("intakeShelf", new Pair<>(25., Units.inchesToMeters(20.))); } @@ -81,7 +83,7 @@ public Command c_posReturnToHomeDown() { } public Command c_posIntakeShelf() { // TODO: back up 14 inches -- remember to always use meters - cones = floorCones; + cones = shelfCones; var cmd = c_holdArmPose(otherPositions.get("intakeShelf")); cmd.setName("arm position - pre shelf intake"); return cmd; @@ -107,8 +109,10 @@ public Command c_shootCones(int shelf) { return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters, () -> RobotMap.Component.intake.c_holdVoltage(voltage).withTimeout(0.5) - .andThen(RobotMap.Component.intake.c_holdVoltage(0)) - .andThen(c_posReturnToHomeUp()) + .andThen(new ParallelCommandGroup( + RobotMap.Component.intake.c_holdVoltage(0), + c_posReturnToHomeUp() + )) ); } @@ -126,8 +130,10 @@ public Command c_shootCubes(int shelf) { return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters, () -> RobotMap.Component.intake.c_holdVoltage(voltage).withTimeout(0.5) - .andThen(RobotMap.Component.intake.c_holdVoltage(0)) - .andThen(c_posReturnToHomeUp()) + .andThen(new ParallelCommandGroup( + RobotMap.Component.intake.c_holdVoltage(0), + c_posReturnToHomeUp() + )) ); } From 7ca5deb9595735abbad1c75056b3fbd0850b5e02 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Mon, 3 Apr 2023 14:38:38 -0700 Subject: [PATCH 45/63] submodule update --- src/main/java/org/usfirst/frc4904/standard | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/usfirst/frc4904/standard b/src/main/java/org/usfirst/frc4904/standard index 4c837b4..3ce0092 160000 --- a/src/main/java/org/usfirst/frc4904/standard +++ b/src/main/java/org/usfirst/frc4904/standard @@ -1 +1 @@ -Subproject commit 4c837b436382b830be0d8091b2749af8b843f946 +Subproject commit 3ce009290aeac7107fef9206a7e5c90d2f355fb3 From cf9d8017f1befa8b9a05bba12b116f0ca52f2404 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Mon, 3 Apr 2023 14:56:20 -0700 Subject: [PATCH 46/63] UT: rewrite balance auton with onArrivalCommand holdArmPose --- .../robot/seenoevil/RobotContainer2.java | 299 +++++++++--------- .../frc4904/robot/subsystems/Intake.java | 6 + .../robot/subsystems/arm/ArmSubsystem.java | 11 +- src/main/java/org/usfirst/frc4904/standard | 2 +- 4 files changed, 156 insertions(+), 162 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java index f4de08f..674aa88 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java @@ -313,176 +313,161 @@ public Trajectory getTrajectory(String trajectoryName) { return trajectories.get(trajectoryName); } -// public Command balanceAuton(Supplier wheelSpeeds, BiConsumer outputVolts){ -// var command = new SequentialCommandGroup( -// //1. Position arm to place gamepiece -// // TODO: options: either place the game picee, or try to flip over, shoot, and then come back so that we are in the same state - -// // implement going over and shooting a cone? - -// new ParallelCommandGroup( -// //3. Retract arm -// // RobotMap.Component.arm.c_posReturnToHomeDown(false), -// new SequentialCommandGroup( -// new WaitCommand(1), //TODO: set wait time to allow arm to get started before moving? -// //4. Drive forward past ramp -// getAutonomousCommand(getTrajectory("past_ramp")), - -// //5. Drive back to get partially on ramp -// getAutonomousCommand(getTrajectory("back_to_ramp")) -// ) -// ) -// // new Balance(RobotMap.Component.navx, wheelSpeeds, outputVolts, 1, -0.1) -// //6. balance code here -// ); + public Command balanceAuton(Supplier wheelSpeeds, BiConsumer outputVolts){ + var command = new SequentialCommandGroup( + //1. Position arm to place gamepiece + // TODO: options: either place the game picee, or try to flip over, shoot, and then come back so that we are in the same state + + // implement going over and shooting a cone? + + new ParallelCommandGroup( + //3. Retract arm + // RobotMap.Component.arm.c_posReturnToHomeDown(false), + new SequentialCommandGroup( + new WaitCommand(1), //TODO: set wait time to allow arm to get started before moving? + //4. Drive forward past ramp + getAutonomousCommand(getTrajectory("past_ramp")), + + //5. Drive back to get partially on ramp + getAutonomousCommand(getTrajectory("back_to_ramp")) + ) + ) + // new Balance(RobotMap.Component.navx, wheelSpeeds, outputVolts, 1, -0.1) + //6. balance code here + ); -// return command; -// } + return command; + } -// public Command balanceAutonAndShootCube(Supplier wheelSpeeds, BiConsumer outputVolts){ - -// var command = new SequentialCommandGroup( -// //1. Position arm to place gamepiece -// // TODO: options: either place the game picee, or try to flip over, shoot, and then come back so that we are in the same state - -// // implement going over and shooting a cone? - -// new ParallelCommandGroup( -// //3. Retract arm -// // RobotMap.Component.arm.c_posReturnToHomeDown(false), -// RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getFirst() -// .withTimeout(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()) -// .andThen(new WaitCommand(0.8)) -// .andThen(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getFirst().withTimeout(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getSecond()).andThen(new InstantCommand(() -> RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.setVoltage(0))) -// ), -// new SequentialCommandGroup( -// new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 100).getSecond()), -// RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.8).andThen(RobotMap.Component.intake.c_holdVoltage(0)) -// ), -// new SequentialCommandGroup( -// new WaitCommand(2.5), //TODO: set wait time to allow arm to get started before moving? -// //4. Drive forward past ramp -// getAutonomousCommand(getTrajectory("past_ramp")), - -// //5. Drive back to get partially on ramp -// getAutonomousCommand(getTrajectory("back_to_ramp")) -// ) -// ) -// // new Balance(RobotMap.Component.navx, wheelSpeeds, outputVolts, 1, -0.1) -// //6. balance code here -// ); + public Command balanceAutonAndShootCube(Supplier wheelSpeeds, BiConsumer outputVolts){ + final double CUBE_SHOOT_ANGLE = 180-15; + final double CUBE_SHOOT_VOLTAGE = 4.5; + final var arm = RobotMap.Component.arm; + var cmd = arm.armPivotSubsystem.c_holdRotation(CUBE_SHOOT_ANGLE, arm.MAX_VELOCITY_PIVOT, arm.MAX_ACCEL_PIVOT, () -> + new SequentialCommandGroup( + RobotMap.Component.intake.c_holdVoltage(CUBE_SHOOT_VOLTAGE).withTimeout(0.8), + RobotMap.Component.intake.c_neutralOutput(), + arm.c_posReturnToHomeUp(() -> new SequentialCommandGroup( + getAutonomousCommand(getTrajectory("past_ramp")), + getAutonomousCommand(getTrajectory("back_to_ramp")) + // new Balance(RobotMap.Component.navx, wheelSpeeds, outputVolts, 1, -0.1) + //6. balance code here + )) + ) + ); + -// return command; -// } - -// // public Command notBalanceAuton(){ -// // var command = new SequentialCommandGroup( -// // //1. Position arm to place gamepiece -// // RobotMap.Component.arm.placeCube(2, true) //TODO: set actual timeout -// // , -// // new ParallelCommandGroup( -// // //3. Retract arm -// // RobotMap.Component.arm.c_posReturnToHomeUp(false), -// // new SequentialCommandGroup( -// // new WaitCommand(1), //TODO: set wait time to allow arm to get started before moving? -// // //4. Drive forward past ramp -// // getAutonomousCommand(getTrajectory("past_ramp")) -// // ) -// // ) -// // ); + return cmd; + } + + // // public Command notBalanceAuton(){ + // // var command = new SequentialCommandGroup( + // // //1. Position arm to place gamepiece + // // RobotMap.Component.arm.placeCube(2, true) //TODO: set actual timeout + // // , + // // new ParallelCommandGroup( + // // //3. Retract arm + // // RobotMap.Component.arm.c_posReturnToHomeUp(false), + // // new SequentialCommandGroup( + // // new WaitCommand(1), //TODO: set wait time to allow arm to get started before moving? + // // //4. Drive forward past ramp + // // getAutonomousCommand(getTrajectory("past_ramp")) + // // ) + // // ) + // // ); -// // return command; -// // } + // // return command; + // // } -// public Command newAuton() { -// var command = new SequentialCommandGroup( -// //1. Position arm to place gamepiece -// // TODO: options: either place the game picee, or try to flip over, shoot, and then come back so that we are in the same state + // public Command newAuton() { + // var command = new SequentialCommandGroup( + // //1. Position arm to place gamepiece + // // TODO: options: either place the game picee, or try to flip over, shoot, and then come back so that we are in the same state -// // implement going over and shooting a cone? + // // implement going over and shooting a cone? -// new ParallelCommandGroup( -// //3. Retract arm -// // RobotMap.Component.arm.c_posReturnToHomeDown(false), -// RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getFirst(), -// new SequentialCommandGroup( -// new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()), -// RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.5), -// RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getFirst().withTimeout(0.8) -// ), + // new ParallelCommandGroup( + // //3. Retract arm + // // RobotMap.Component.arm.c_posReturnToHomeDown(false), + // RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getFirst(), + // new SequentialCommandGroup( + // new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()), + // RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.5), + // RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getFirst().withTimeout(0.8) + // ), -// new SequentialCommandGroup( -// new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()+0.5), -// getAutonomousCommand(getTrajectory("to_ramp")), -// getAutonomousCommand(getTrajectory("angle_ramp_forward")), -// new WaitCommand(2), -// getAutonomousCommand(getTrajectory("go_over_ramp")), -// getAutonomousCommand(getTrajectory("angle_ramp_backward")), -// new WaitCommand(2), -// getAutonomousCommand(getTrajectory("go_middle_ramp")) -// ) -// ) -// // new Balance(RobotMap.Component.navx, wheelSpeeds, outputVolts, 1, -0.1) -// //6. balance code here -// ); + // new SequentialCommandGroup( + // new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()+0.5), + // getAutonomousCommand(getTrajectory("to_ramp")), + // getAutonomousCommand(getTrajectory("angle_ramp_forward")), + // new WaitCommand(2), + // getAutonomousCommand(getTrajectory("go_over_ramp")), + // getAutonomousCommand(getTrajectory("angle_ramp_backward")), + // new WaitCommand(2), + // getAutonomousCommand(getTrajectory("go_middle_ramp")) + // ) + // ) + // // new Balance(RobotMap.Component.navx, wheelSpeeds, outputVolts, 1, -0.1) + // //6. balance code here + // ); -// return command; + // return command; -// } + // } -// public Command shootCubeAndCross() { -// var command = new SequentialCommandGroup( -// //1. Position arm to place gamepiece -// // TODO: options: either place the game picee, or try to flip over, shoot, and then come back so that we are in the same state + // public Command shootCubeAndCross() { + // var command = new SequentialCommandGroup( + // //1. Position arm to place gamepiece + // // TODO: options: either place the game picee, or try to flip over, shoot, and then come back so that we are in the same state -// // implement going over and shooting a cone? + // // implement going over and shooting a cone? -// new ParallelCommandGroup( -// //3. Retract arm -// // RobotMap.Component.arm.c_posReturnToHomeDown(false), -// RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getFirst() -// .withTimeout(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()) -// .andThen(new WaitCommand(0.8)) -// .andThen(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getFirst().withTimeout(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getSecond()).andThen(new InstantCommand(() -> RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.setVoltage(0))) -// ), -// new SequentialCommandGroup( -// new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 100).getSecond()), -// RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.8).andThen(RobotMap.Component.intake.c_holdVoltage(0)) -// ), -// new SequentialCommandGroup( -// new WaitCommand(2.5), //TODO: set wait time to allow arm to get started before moving? -// //4. Drive out of the community and stop right in front of the next game piece. -// getAutonomousCommand(getTrajectory("go_to_pickup_next")) - -// ) -// ) -// // new Balance(RobotMap.Component.navx, wheelSpeeds, outputVolts, 1, -0.1) -// //6. balance code here -// ); + // new ParallelCommandGroup( + // //3. Retract arm + // // RobotMap.Component.arm.c_posReturnToHomeDown(false), + // RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getFirst() + // .withTimeout(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()) + // .andThen(new WaitCommand(0.8)) + // .andThen(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getFirst().withTimeout(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getSecond()).andThen(new InstantCommand(() -> RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.setVoltage(0))) + // ), + // new SequentialCommandGroup( + // new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 100).getSecond()), + // RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.8).andThen(RobotMap.Component.intake.c_holdVoltage(0)) + // ), + // new SequentialCommandGroup( + // new WaitCommand(2.5), //TODO: set wait time to allow arm to get started before moving? + // //4. Drive out of the community and stop right in front of the next game piece. + // getAutonomousCommand(getTrajectory("go_to_pickup_next")) + + // ) + // ) + // // new Balance(RobotMap.Component.navx, wheelSpeeds, outputVolts, 1, -0.1) + // //6. balance code here + // ); -// return command; -// } -// public Command shoot2auto(){ //shoot cone, grab cube, shoot cube, doesn't balance -// var command = new SequentialCommandGroup( -// RobotMap.Component.arm.c_shootCones(4), -// new ParallelCommandGroup( -// RobotMap.Component.arm.c_posReturnToHomeDown(), -// getAutonomousCommand(getTrajectory("go_to_pickup_next")) -// ), -// RobotMap.Component.intake.c_holdVoltage(-8).withTimeout(0.5), //TODO: for picking up cone tune to make sure it works -// new ParallelRaceGroup( -// RobotMap.Component.intake.c_holdVoltage(-1), -// getAutonomousCommand(getTrajectory("from_pickup_to_place")) -// ), -// new ParallelCommandGroup( // shootign cube -// RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getFirst(), -// new SequentialCommandGroup( -// new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()), -// RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.5), -// RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getFirst().withTimeout(0.8) -// )) -// ); -// return command; -// } + // return command; + // } + // public Command shoot2auto(){ //shoot cone, grab cube, shoot cube, doesn't balance + // var command = new SequentialCommandGroup( + // RobotMap.Component.arm.c_shootCones(4), + // new ParallelCommandGroup( + // RobotMap.Component.arm.c_posReturnToHomeDown(), + // getAutonomousCommand(getTrajectory("go_to_pickup_next")) + // ), + // RobotMap.Component.intake.c_holdVoltage(-8).withTimeout(0.5), //TODO: for picking up cone tune to make sure it works + // new ParallelRaceGroup( + // RobotMap.Component.intake.c_holdVoltage(-1), + // getAutonomousCommand(getTrajectory("from_pickup_to_place")) + // ), + // new ParallelCommandGroup( // shootign cube + // RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getFirst(), + // new SequentialCommandGroup( + // new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()), + // RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.5), + // RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getFirst().withTimeout(0.8) + // )) + // ); + // return command; + // } } diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/Intake.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/Intake.java index e63843f..c4f9e3c 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/Intake.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/Intake.java @@ -26,6 +26,12 @@ public void setPower(double power) { leftMotors.setPower(power); rightMotors.setPower(-power); } + public Command c_neutralOutput() { + return Commands.runOnce(() -> { + leftMotors.neutralOutput(); + rightMotors.neutralOutput(); + }, leftMotors, rightMotors); + } public Command c_holdVoltage(double voltage) { return Commands.run(() -> { setVoltage(voltage); diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index 1e5d8ed..ccc2ef8 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -71,13 +71,16 @@ public ArmSubsystem(ArmPivotSubsystem armPivotSubsystem, ArmExtensionSubsystem a this.armExtensionSubsystem = armExtensionSubsystem; } - public Command c_posReturnToHomeUp() { - var cmd = new TriggerCommandFactory(() -> c_holdArmPose(otherPositions.get("homeUp"))); + public Command c_posReturnToHomeUp() { return c_posReturnToHomeUp(null); } + public Command c_posReturnToHomeUp(Supplier onArrivalCommandDealer) { + var cmd = new TriggerCommandFactory(() -> c_holdArmPose(otherPositions.get("homeUp"), onArrivalCommandDealer)); cmd.setName("arm position - home (up)"); return cmd; } - public Command c_posReturnToHomeDown() { - var cmd = new TriggerCommandFactory(() -> c_holdArmPose(otherPositions.get("homeDown"))); + + public Command c_posReturnToHomeDown() { return c_posReturnToHomeDown(null); } + public Command c_posReturnToHomeDown(Supplier onArrivalCommandDealer) { + var cmd = new TriggerCommandFactory(() -> c_holdArmPose(otherPositions.get("homeDown"), onArrivalCommandDealer)); cmd.setName("arm position - home (down)"); return cmd; } diff --git a/src/main/java/org/usfirst/frc4904/standard b/src/main/java/org/usfirst/frc4904/standard index 3ce0092..d2415f0 160000 --- a/src/main/java/org/usfirst/frc4904/standard +++ b/src/main/java/org/usfirst/frc4904/standard @@ -1 +1 @@ -Subproject commit 3ce009290aeac7107fef9206a7e5c90d2f355fb3 +Subproject commit d2415f03fcdc8e09e11719fc5cfef17850fc6de0 From 5ae62378123922f9b52223f4377372ae69706050 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Mon, 3 Apr 2023 15:19:31 -0700 Subject: [PATCH 47/63] make intake stop after cone shot --- .../java/org/usfirst/frc4904/robot/Robot.java | 3 ++- .../robot/subsystems/arm/ArmSubsystem.java | 19 +++++++++---------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 1d09af9..e691abf 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -162,8 +162,9 @@ public void autonomousInitialize() { // SATURDAY MORNING TEST: is the cube shooter auton gonna work + var commnand = donttouchme.balanceAutonAndShootCube(donttouchme.m_robotDrive::getWheelSpeeds, donttouchme.m_robotDrive::tankDriveVolts); // var commnand = donttouchme.balanceAutonAndShootCube(donttouchme.m_robotDrive::getWheelSpeeds, donttouchme.m_robotDrive::tankDriveVolts); - var commnand = donttouchme.getAutonomousCommand(donttouchme.getTrajectory("straight_forward")); + // var commnand = donttouchme.getAutonomousCommand(donttouchme.getTrajectory("straight_forward")); commnand.schedule(); } diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index ccc2ef8..f1ceec2 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -5,6 +5,7 @@ import java.util.function.Function; import java.util.function.Supplier; +import org.usfirst.frc4904.robot.Robot; import org.usfirst.frc4904.robot.RobotMap; import org.usfirst.frc4904.robot.subsystems.Intake; import org.usfirst.frc4904.standard.commands.Noop; @@ -111,11 +112,10 @@ public Command c_shootCones(int shelf) { var voltage = cones.get(shelf).getThird(); return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters, - () -> RobotMap.Component.intake.c_holdVoltage(voltage).withTimeout(0.5) - .andThen(new ParallelCommandGroup( - RobotMap.Component.intake.c_holdVoltage(0), - c_posReturnToHomeUp() - )) + () -> new SequentialCommandGroup( + RobotMap.Component.intake.c_holdVoltage(voltage).withTimeout(0.5), + RobotMap.Component.intake.c_neutralOutput(), c_posReturnToHomeUp() + ) ); } @@ -132,11 +132,10 @@ public Command c_shootCubes(int shelf) { var voltage = cubes.get(shelf).getThird(); return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters, - () -> RobotMap.Component.intake.c_holdVoltage(voltage).withTimeout(0.5) - .andThen(new ParallelCommandGroup( - RobotMap.Component.intake.c_holdVoltage(0), - c_posReturnToHomeUp() - )) + () -> new SequentialCommandGroup( + RobotMap.Component.intake.c_holdVoltage(voltage).withTimeout(0.5), + RobotMap.Component.intake.c_neutralOutput(), c_posReturnToHomeUp() + ) ); } From 05106ae81d05bcfa5d24862c84adc2ccad70d236 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Mon, 3 Apr 2023 19:41:00 -0700 Subject: [PATCH 48/63] OK OK IT IS WORKING OK --- src/main/java/org/usfirst/frc4904/robot/Robot.java | 2 +- .../frc4904/robot/humaninterface/drivers/NathanGain.java | 2 +- .../usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java | 4 ++-- src/main/java/org/usfirst/frc4904/standard | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index e691abf..825e1a8 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -97,7 +97,7 @@ public void teleopInitialize() { // RobotMap.Component.arm.c_posReturnToHomeUp(NathanGain.isFlippy) // )); - final DoubleSupplier pivot_getter = () -> scaleGain(RobotMap.HumanInput.Operator.joystick.getAxis(1), 60, 3); + final DoubleSupplier pivot_getter = () -> scaleGain(RobotMap.HumanInput.Operator.joystick.getAxis(1),60, 1); (new Trigger(() -> pivot_getter.getAsDouble() != 0)).onTrue( nameCommand("arm - teleop - armPivot operator override", RobotMap.Component.arm.armPivotSubsystem.c_controlAngularVelocity(pivot_getter::getAsDouble) diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java index d8b7e3a..a66fcae 100644 --- a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java @@ -16,7 +16,7 @@ public class NathanGain extends Driver { public static final double NORMAL_SPEED_GAIN = 0.5; // TODO TUNE - public static final double NORMAL_TURN_GAIN = 0.3; + public static final double NORMAL_TURN_GAIN = 0.25; public static final double PRECISE_SPEED_SCALE = 0.2; public static final double PRECISE_TURN_SCALE = 0.1; diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index f1ceec2..c668649 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -36,9 +36,9 @@ public class ArmSubsystem extends SubsystemBase { public static final HashMap> shelfCones = new HashMap<>(); //in degrees, meters static { // SHELF CONES // cones.put(1, new Triple<>(-19., Units.inchesToMeters(0), 3.)); - shelfCones.put(2, new Triple<>(29., Units.inchesToMeters(16), 3.)); + shelfCones.put(2, new Triple<>(29., Units.inchesToMeters(15), 3.)); shelfCones.put(3, new Triple<>(41., ArmExtensionSubsystem.MAX_EXTENSION_M, 3.)); - shelfCones.put(4, new Triple<>(180.0-41, ArmExtensionSubsystem.MAX_EXTENSION_M, 4.)); + shelfCones.put(4, new Triple<>(180.0-41, ArmExtensionSubsystem.MAX_EXTENSION_M, 3.)); } // FLOOR CONES diff --git a/src/main/java/org/usfirst/frc4904/standard b/src/main/java/org/usfirst/frc4904/standard index d2415f0..5217d90 160000 --- a/src/main/java/org/usfirst/frc4904/standard +++ b/src/main/java/org/usfirst/frc4904/standard @@ -1 +1 @@ -Subproject commit d2415f03fcdc8e09e11719fc5cfef17850fc6de0 +Subproject commit 5217d908faa8f888b1b3fad9e5fc56372ab8abb2 From d03a605127373ef20170012e34ecbc2feedf5593 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Mon, 3 Apr 2023 19:59:59 -0700 Subject: [PATCH 49/63] rewrite autons to use new onArrivalCommand semantics --- .../robot/seenoevil/RobotContainer2.java | 172 +++++------------- .../robot/subsystems/arm/ArmSubsystem.java | 12 +- src/main/java/org/usfirst/frc4904/standard | 2 +- 3 files changed, 59 insertions(+), 127 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java index 674aa88..5d359e4 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java @@ -27,6 +27,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import com.ctre.phoenix.motorcontrol.NeutralMode; @@ -42,6 +43,7 @@ import org.usfirst.frc4904.robot.seenoevil.Constants.AutoConstants; import org.usfirst.frc4904.robot.seenoevil.Constants.DriveConstants; +import org.usfirst.frc4904.standard.commands.TriggerCommandFactory; import org.usfirst.frc4904.standard.custom.sensors.NavX; import org.usfirst.frc4904.robot.Robot; import org.usfirst.frc4904.robot.RobotMap; @@ -340,134 +342,60 @@ public Command balanceAuton(Supplier wheelSpeeds, } public Command balanceAutonAndShootCube(Supplier wheelSpeeds, BiConsumer outputVolts){ - final double CUBE_SHOOT_ANGLE = 180-15; - final double CUBE_SHOOT_VOLTAGE = 4.5; - final var arm = RobotMap.Component.arm; - var cmd = arm.armPivotSubsystem.c_holdRotation(CUBE_SHOOT_ANGLE, arm.MAX_VELOCITY_PIVOT, arm.MAX_ACCEL_PIVOT, () -> - new SequentialCommandGroup( - RobotMap.Component.intake.c_holdVoltage(CUBE_SHOOT_VOLTAGE).withTimeout(0.8), - RobotMap.Component.intake.c_neutralOutput(), - arm.c_posReturnToHomeUp(() -> new SequentialCommandGroup( - getAutonomousCommand(getTrajectory("past_ramp")), - getAutonomousCommand(getTrajectory("back_to_ramp")) - // new Balance(RobotMap.Component.navx, wheelSpeeds, outputVolts, 1, -0.1) - //6. balance code here - )) - ) - ); - + var cmd = RobotMap.Component.arm.c_shootCubes(4, () -> new SequentialCommandGroup( + getAutonomousCommand(getTrajectory("past_ramp")), + getAutonomousCommand(getTrajectory("back_to_ramp")) + )); // high shelf flippy return cmd; } - // // public Command notBalanceAuton(){ - // // var command = new SequentialCommandGroup( - // // //1. Position arm to place gamepiece - // // RobotMap.Component.arm.placeCube(2, true) //TODO: set actual timeout - // // , - // // new ParallelCommandGroup( - // // //3. Retract arm - // // RobotMap.Component.arm.c_posReturnToHomeUp(false), - // // new SequentialCommandGroup( - // // new WaitCommand(1), //TODO: set wait time to allow arm to get started before moving? - // // //4. Drive forward past ramp - // // getAutonomousCommand(getTrajectory("past_ramp")) - // // ) - // // ) - // // ); - - // // return command; - // // } - - // public Command newAuton() { - // var command = new SequentialCommandGroup( - // //1. Position arm to place gamepiece - // // TODO: options: either place the game picee, or try to flip over, shoot, and then come back so that we are in the same state - - // // implement going over and shooting a cone? - - // new ParallelCommandGroup( - // //3. Retract arm - // // RobotMap.Component.arm.c_posReturnToHomeDown(false), - // RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getFirst(), - // new SequentialCommandGroup( - // new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()), - // RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.5), - // RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getFirst().withTimeout(0.8) - // ), - - // new SequentialCommandGroup( - // new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()+0.5), - // getAutonomousCommand(getTrajectory("to_ramp")), - // getAutonomousCommand(getTrajectory("angle_ramp_forward")), - // new WaitCommand(2), - // getAutonomousCommand(getTrajectory("go_over_ramp")), - // getAutonomousCommand(getTrajectory("angle_ramp_backward")), - // new WaitCommand(2), - // getAutonomousCommand(getTrajectory("go_middle_ramp")) - // ) + // public Command notBalanceAuton(){ + // var command = new SequentialCommandGroup( + // //1. Position arm to place gamepiece + // RobotMap.Component.arm.placeCube(2, true) //TODO: set actual timeout + // , + // new ParallelCommandGroup( + // //3. Retract arm + // RobotMap.Component.arm.c_posReturnToHomeUp(false), + // new SequentialCommandGroup( + // new WaitCommand(1), //TODO: set wait time to allow arm to get started before moving? + // //4. Drive forward past ramp + // getAutonomousCommand(getTrajectory("past_ramp")) // ) - // // new Balance(RobotMap.Component.navx, wheelSpeeds, outputVolts, 1, -0.1) - // //6. balance code here - // ); - - // return command; - + // ) + // ); + + // return command; // } - // public Command shootCubeAndCross() { - // var command = new SequentialCommandGroup( - // //1. Position arm to place gamepiece - // // TODO: options: either place the game picee, or try to flip over, shoot, and then come back so that we are in the same state - - // // implement going over and shooting a cone? - - // new ParallelCommandGroup( - // //3. Retract arm - // // RobotMap.Component.arm.c_posReturnToHomeDown(false), - // RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getFirst() - // .withTimeout(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()) - // .andThen(new WaitCommand(0.8)) - // .andThen(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getFirst().withTimeout(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getSecond()).andThen(new InstantCommand(() -> RobotMap.Component.arm.armPivotSubsystem.armMotorGroup.setVoltage(0))) - // ), - // new SequentialCommandGroup( - // new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 100).getSecond()), - // RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.8).andThen(RobotMap.Component.intake.c_holdVoltage(0)) - // ), - // new SequentialCommandGroup( - // new WaitCommand(2.5), //TODO: set wait time to allow arm to get started before moving? - // //4. Drive out of the community and stop right in front of the next game piece. - // getAutonomousCommand(getTrajectory("go_to_pickup_next")) - - // ) - // ) - // // new Balance(RobotMap.Component.navx, wheelSpeeds, outputVolts, 1, -0.1) - // //6. balance code here - // ); + public Command newAuton() { + var cmd = RobotMap.Component.arm.c_shootCubes(4, () -> new SequentialCommandGroup( + getAutonomousCommand(getTrajectory("to_ramp")), + getAutonomousCommand(getTrajectory("angle_ramp_forward")), + new WaitCommand(1), + getAutonomousCommand(getTrajectory("go_over_ramp")), + getAutonomousCommand(getTrajectory("angle_ramp_backward")), + new WaitCommand(1), + getAutonomousCommand(getTrajectory("go_middle_ramp")) + )); - // return command; - // } - // public Command shoot2auto(){ //shoot cone, grab cube, shoot cube, doesn't balance - // var command = new SequentialCommandGroup( - // RobotMap.Component.arm.c_shootCones(4), - // new ParallelCommandGroup( - // RobotMap.Component.arm.c_posReturnToHomeDown(), - // getAutonomousCommand(getTrajectory("go_to_pickup_next")) - // ), - // RobotMap.Component.intake.c_holdVoltage(-8).withTimeout(0.5), //TODO: for picking up cone tune to make sure it works - // new ParallelRaceGroup( - // RobotMap.Component.intake.c_holdVoltage(-1), - // getAutonomousCommand(getTrajectory("from_pickup_to_place")) - // ), - // new ParallelCommandGroup( // shootign cube - // RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getFirst(), - // new SequentialCommandGroup( - // new WaitCommand(RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(180-15, 150, 200).getSecond()), - // RobotMap.Component.intake.c_holdVoltage(4.5).withTimeout(0.5), - // RobotMap.Component.arm.armPivotSubsystem.c_holdRotation(0, 150, 150).getFirst().withTimeout(0.8) - // )) - // ); - // return command; - // } - + return cmd; + } + + public Command twoPieceAuton(){ //shoot cone, grab cube, shoot cube, doesn't balance + var cmd = RobotMap.Component.arm.c_shootCones(4, // shoot cone + () -> new SequentialCommandGroup( + new ParallelCommandGroup( // then, in parallel + getAutonomousCommand(getTrajectory("go_to_pickup_next")), // go to pickup location + (new WaitCommand(3)).andThen(RobotMap.Component.intake.c_holdVoltage(-8)) // with a delay, start intaking so that we are intaking while we approach the piece + ), + new TriggerCommandFactory( // then, as a separated parallel schedule, + () -> RobotMap.Component.intake.c_holdVoltage(-1), // hold the game piece in + () -> getAutonomousCommand(getTrajectory("from_pickup_to_place")) // return to the next placement location + ), + RobotMap.Component.arm.c_shootCubes(4) // finally, shoot the cube we just picked up and stow + )); + return cmd; + } } diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index f1ceec2..74bfa1e 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -56,6 +56,8 @@ public class ArmSubsystem extends SubsystemBase { // cubes.put(1, new Triple<>(-33., Units.inchesToMeters(0), 3.)); cubes.put(2, new Triple<>(15., Units.inchesToMeters(0), 4.5)); cubes.put(3, new Triple<>(20., Units.inchesToMeters(0), 4.5)); + cubes.put(4, new Triple<>(180.-20, Units.inchesToMeters(0), 4.5)); + cubes.put(5, new Triple<>(180.-15, Units.inchesToMeters(0), 4.5)); } public static final HashMap> otherPositions = new HashMap<>(); @@ -106,7 +108,8 @@ public Command c_angleCones(int shelf) { return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters); } - public Command c_shootCones(int shelf) { + public Command c_shootCones(int shelf) { return c_shootCones(shelf, null); } + public Command c_shootCones(int shelf, Supplier onArrivalCommandDealer) { var degreesFromHorizontal = cones.get(shelf).getFirst(); var extensionLengthMeters = cones.get(shelf).getSecond(); var voltage = cones.get(shelf).getThird(); @@ -114,7 +117,7 @@ public Command c_shootCones(int shelf) { return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters, () -> new SequentialCommandGroup( RobotMap.Component.intake.c_holdVoltage(voltage).withTimeout(0.5), - RobotMap.Component.intake.c_neutralOutput(), c_posReturnToHomeUp() + RobotMap.Component.intake.c_neutralOutput(), c_posReturnToHomeUp(onArrivalCommandDealer) ) ); } @@ -126,7 +129,8 @@ public Command c_angleCubes(int shelf) { return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters, null); } - public Command c_shootCubes(int shelf) { + public Command c_shootCubes(int shelf) { return c_shootCubes(shelf, null); } + public Command c_shootCubes(int shelf, Supplier onArrivalCommandDealer) { var degreesFromHorizontal = cubes.get(shelf).getFirst(); var extensionLengthMeters = cubes.get(shelf).getSecond(); var voltage = cubes.get(shelf).getThird(); @@ -134,7 +138,7 @@ public Command c_shootCubes(int shelf) { return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters, () -> new SequentialCommandGroup( RobotMap.Component.intake.c_holdVoltage(voltage).withTimeout(0.5), - RobotMap.Component.intake.c_neutralOutput(), c_posReturnToHomeUp() + RobotMap.Component.intake.c_neutralOutput(), c_posReturnToHomeUp(onArrivalCommandDealer) ) ); } diff --git a/src/main/java/org/usfirst/frc4904/standard b/src/main/java/org/usfirst/frc4904/standard index d2415f0..4646627 160000 --- a/src/main/java/org/usfirst/frc4904/standard +++ b/src/main/java/org/usfirst/frc4904/standard @@ -1 +1 @@ -Subproject commit d2415f03fcdc8e09e11719fc5cfef17850fc6de0 +Subproject commit 464662714221519419c8822fcaa837c4de035cc6 From d0e337fdaee1c62d50b52059795e25fb55b57d9c Mon Sep 17 00:00:00 2001 From: Exr0n Date: Mon, 3 Apr 2023 20:11:25 -0700 Subject: [PATCH 50/63] reformat robotcontainer2 autons --- .../robot/seenoevil/RobotContainer2.java | 587 +++++++++--------- 1 file changed, 293 insertions(+), 294 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java index 5d359e4..d89725e 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java @@ -64,226 +64,220 @@ */ public class RobotContainer2 { - private static final double placeholderconstant = 0; //TODO: add value - private static TrajectoryConfig trajectoryConfig = new TrajectoryConfig( - AutoConstants.kMaxSpeedMetersPerSecond, - AutoConstants.kMaxAccelerationMetersPerSecondSquared) - // Add kinematics to ensure max speed is actually obeyed - .setKinematics(DriveConstants.kDriveKinematics) - // Apply the voltage constraint - .addConstraint(new DifferentialDriveVoltageConstraint( - new SimpleMotorFeedforward( - DriveConstants.ksVolts, - DriveConstants.kvVoltSecondsPerMeter, - DriveConstants.kaVoltSecondsSquaredPerMeter), - DriveConstants.kDriveKinematics, - 10)); - private static TrajectoryConfig trajectoryConfigReversed = new TrajectoryConfig( - AutoConstants.kMaxSpeedMetersPerSecond, - AutoConstants.kMaxAccelerationMetersPerSecondSquared) - .setKinematics(DriveConstants.kDriveKinematics) - .addConstraint(new DifferentialDriveVoltageConstraint( - new SimpleMotorFeedforward( - DriveConstants.ksVolts, - DriveConstants.kvVoltSecondsPerMeter, - DriveConstants.kaVoltSecondsSquaredPerMeter), - DriveConstants.kDriveKinematics, - 10)) - .setReversed(true); - private static TrajectoryConfig trajectoryConfigSlow = new TrajectoryConfig( - AutoConstants.kMaxSpeedMetersPerSecond/3, - AutoConstants.kMaxAccelerationMetersPerSecondSquared/2) - .setKinematics(DriveConstants.kDriveKinematics) - .addConstraint(new DifferentialDriveVoltageConstraint( - new SimpleMotorFeedforward( - DriveConstants.ksVolts, - DriveConstants.kvVoltSecondsPerMeter, - DriveConstants.kaVoltSecondsSquaredPerMeter), - DriveConstants.kDriveKinematics, - 10)) - .setReversed(false); - private static TrajectoryConfig trajectoryConfigSlowReversed = new TrajectoryConfig( - AutoConstants.kMaxSpeedMetersPerSecond/3, - AutoConstants.kMaxAccelerationMetersPerSecondSquared/2) - .setKinematics(DriveConstants.kDriveKinematics) - .addConstraint(new DifferentialDriveVoltageConstraint( - new SimpleMotorFeedforward( - DriveConstants.ksVolts, - DriveConstants.kvVoltSecondsPerMeter, - DriveConstants.kaVoltSecondsSquaredPerMeter), - DriveConstants.kDriveKinematics, - 10)) - .setReversed(true); - - private static Map trajectories = Map.ofEntries( - entry("sickle", TrajectoryGenerator.generateTrajectory( - // Start at the origin facing the +X direction - new Pose2d(0, 0, new Rotation2d(0)), - // Pass through these two interior waypoints, making an 's' curve path - // List.of(new Translation2d(0.33*dist, .15*dist), new Translation2d(0.66*dist, -.15*dist)), - List.of(new Translation2d(1, -1), new Translation2d(2, -1)), - - // End 3 meters straight ahead of where we started, facing forward - new Pose2d(3, 0, new Rotation2d(0)), - trajectoryConfig) - ), - entry("straight_forward", TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(0)), - List.of(new Translation2d(1, 0)), - new Pose2d(6, 0, new Rotation2d(0)), - trajectoryConfig - )), - entry("straight_backward", TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(Math.PI)), - List.of(new Translation2d(1, 0)), - new Pose2d(6, 0, new Rotation2d(Math.PI)), - trajectoryConfigReversed - )), - entry("turn_right", TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(0)), - List.of(), - new Pose2d(1, -1, new Rotation2d(-Math.PI/2)), - trajectoryConfig - )), - entry("past_ramp", TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(0)), - List.of(), - new Pose2d(4, 0, new Rotation2d(0)), - trajectoryConfig - )), - entry("back_to_ramp", TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(Math.PI)), - List.of(), - new Pose2d(1, 0, new Rotation2d(Math.PI)), - trajectoryConfigReversed - )), - - //new auton - entry("to_ramp", TrajectoryGenerator.generateTrajectory( - new Pose2d(0,0, new Rotation2d(0)), - List.of(), - new Pose2d(Units.inchesToMeters(24.19-.5),0,new Rotation2d(0)), - trajectoryConfig - )), - - entry("go_over_ramp", TrajectoryGenerator.generateTrajectory( - new Pose2d(0,0,new Rotation2d(0)), - List.of(), - new Pose2d(Units.inchesToMeters(118.02-.5)+placeholderconstant,0,new Rotation2d(0)), - trajectoryConfig - )), - entry("ramp_start", TrajectoryGenerator.generateTrajectory( - new Pose2d(0,0,new Rotation2d(0)), - List.of(), - new Pose2d(Units.inchesToMeters(5.001)+placeholderconstant,0,new Rotation2d(0)), - trajectoryConfigSlow - )), - entry("angle_ramp_backward", TrajectoryGenerator.generateTrajectory( - new Pose2d(0,0,new Rotation2d(Math.PI)), - List.of(), - new Pose2d(Units.inchesToMeters(5.001)+placeholderconstant,0,new Rotation2d(Math.PI)), - trajectoryConfigSlowReversed - )), - entry("go_middle_ramp", TrajectoryGenerator.generateTrajectory( - new Pose2d(0,0,new Rotation2d(Math.PI)), - List.of(), - new Pose2d(Units.inchesToMeters(53.5),0,new Rotation2d(Math.PI)), - trajectoryConfigReversed - )), - entry("go_to_pickup_next", TrajectoryGenerator.generateTrajectory( - new Pose2d(0,0,new Rotation2d(0)), - List.of(), - new Pose2d(4.44,0,new Rotation2d(0)), - trajectoryConfig)), - entry("from_pickup_to_place", TrajectoryGenerator.generateTrajectory( - new Pose2d(0,0,new Rotation2d(Math.PI)), - List.of(), - new Pose2d(4.44,0,new Rotation2d(Math.PI)), - trajectoryConfigReversed)) + private static final double placeholderconstant = 0; //TODO: add value + private static TrajectoryConfig trajectoryConfig = new TrajectoryConfig( + AutoConstants.kMaxSpeedMetersPerSecond, + AutoConstants.kMaxAccelerationMetersPerSecondSquared) + // Add kinematics to ensure max speed is actually obeyed + .setKinematics(DriveConstants.kDriveKinematics) + // Apply the voltage constraint + .addConstraint(new DifferentialDriveVoltageConstraint( + new SimpleMotorFeedforward( + DriveConstants.ksVolts, + DriveConstants.kvVoltSecondsPerMeter, + DriveConstants.kaVoltSecondsSquaredPerMeter + ), + DriveConstants.kDriveKinematics, + 10 + )); + private static TrajectoryConfig trajectoryConfigReversed = new TrajectoryConfig( + AutoConstants.kMaxSpeedMetersPerSecond, + AutoConstants.kMaxAccelerationMetersPerSecondSquared) + .setKinematics(DriveConstants.kDriveKinematics) + .addConstraint(new DifferentialDriveVoltageConstraint( + new SimpleMotorFeedforward( + DriveConstants.ksVolts, + DriveConstants.kvVoltSecondsPerMeter, + DriveConstants.kaVoltSecondsSquaredPerMeter), + DriveConstants.kDriveKinematics, + 10)) + .setReversed(true); + private static TrajectoryConfig trajectoryConfigSlow = new TrajectoryConfig( + AutoConstants.kMaxSpeedMetersPerSecond / 3, + AutoConstants.kMaxAccelerationMetersPerSecondSquared / 2) + .setKinematics(DriveConstants.kDriveKinematics) + .addConstraint(new DifferentialDriveVoltageConstraint( + new SimpleMotorFeedforward( + DriveConstants.ksVolts, + DriveConstants.kvVoltSecondsPerMeter, + DriveConstants.kaVoltSecondsSquaredPerMeter), + DriveConstants.kDriveKinematics, + 10)) + .setReversed(false); + private static TrajectoryConfig trajectoryConfigSlowReversed = new TrajectoryConfig( + AutoConstants.kMaxSpeedMetersPerSecond / 3, + AutoConstants.kMaxAccelerationMetersPerSecondSquared / 2) + .setKinematics(DriveConstants.kDriveKinematics) + .addConstraint(new DifferentialDriveVoltageConstraint( + new SimpleMotorFeedforward( + DriveConstants.ksVolts, + DriveConstants.kvVoltSecondsPerMeter, + DriveConstants.kaVoltSecondsSquaredPerMeter), + DriveConstants.kDriveKinematics, + 10)) + .setReversed(true); + + + private static Map trajectories = Map.ofEntries( + entry("sickle", TrajectoryGenerator.generateTrajectory( + // Start at the origin facing the +X direction + new Pose2d(0, 0, new Rotation2d(0)), + // Pass through these two interior waypoints, making an 's' curve path + // List.of(new Translation2d(0.33*dist, .15*dist), new Translation2d(0.66*dist, + // -.15*dist)), + List.of(new Translation2d(1, -1), new Translation2d(2, -1)), + + // End 3 meters straight ahead of where we started, facing forward + new Pose2d(3, 0, new Rotation2d(0)), + trajectoryConfig)), + entry("straight_forward", TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(0)), + List.of(new Translation2d(1, 0)), + new Pose2d(6, 0, new Rotation2d(0)), + trajectoryConfig)), + entry("straight_backward", TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(Math.PI)), + List.of(new Translation2d(1, 0)), + new Pose2d(6, 0, new Rotation2d(Math.PI)), + trajectoryConfigReversed)), + entry("turn_right", TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(0)), + List.of(), + new Pose2d(1, -1, new Rotation2d(-Math.PI / 2)), + trajectoryConfig)), + entry("past_ramp", TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(0)), + List.of(), + new Pose2d(4, 0, new Rotation2d(0)), + trajectoryConfig)), + entry("back_to_ramp", TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(Math.PI)), + List.of(), + new Pose2d(1, 0, new Rotation2d(Math.PI)), + trajectoryConfigReversed)), + + // new auton + entry("to_ramp", TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(0)), + List.of(), + new Pose2d(Units.inchesToMeters(24.19 - .5), 0, new Rotation2d(0)), + trajectoryConfig)), + + entry("go_over_ramp", TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(0)), + List.of(), + new Pose2d(Units.inchesToMeters(118.02 - .5) + placeholderconstant, 0, new Rotation2d(0)), + trajectoryConfig)), + entry("ramp_start", TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(0)), + List.of(), + new Pose2d(Units.inchesToMeters(5.001) + placeholderconstant, 0, new Rotation2d(0)), + trajectoryConfigSlow)), + entry("angle_ramp_backward", TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(Math.PI)), + List.of(), + new Pose2d(Units.inchesToMeters(5.001) + placeholderconstant, 0, new Rotation2d(Math.PI)), + trajectoryConfigSlowReversed)), + entry("go_middle_ramp", TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(Math.PI)), + List.of(), + new Pose2d(Units.inchesToMeters(53.5), 0, new Rotation2d(Math.PI)), + trajectoryConfigReversed)), + entry("go_to_pickup_next", TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(0)), + List.of(), + new Pose2d(4.44, 0, new Rotation2d(0)), + trajectoryConfig)), + entry("from_pickup_to_place", TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(Math.PI)), + List.of(), + new Pose2d(4.44, 0, new Rotation2d(Math.PI)), + trajectoryConfigReversed))); + + public static class Component { + public static WPI_TalonFX leftATalonFX; + public static WPI_TalonFX leftBTalonFX; + public static WPI_TalonFX rightATalonFX; + public static WPI_TalonFX rightBTalonFX; + public static WPI_TalonFX testTalon; + } + + // The robot's subsystems + // motors + + public final DriveSubsystem m_robotDrive; + // public final XboxController m_driverController; + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer2(WPI_TalonFX leftATalonFX, WPI_TalonFX leftBTalonFX, WPI_TalonFX rightATalonFX, + WPI_TalonFX rightBTalonFX, AHRS navx) { + + // The driver's controller + // m_driverController = new XboxController(OIConstants.kDriverControllerPort); + + // Configure the button bindings + Component.leftATalonFX = leftATalonFX; + Component.leftBTalonFX = leftBTalonFX; + Component.rightATalonFX = rightATalonFX; + Component.rightBTalonFX = rightBTalonFX; + + RobotContainer2.Component.leftATalonFX.setNeutralMode(NeutralMode.Coast); + RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Coast); + RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Coast); + RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Coast); + + Component.testTalon = new WPI_TalonFX(1); + // Component.leftATalonFX.setInverted(true); + // Component.leftBTalonFX.setInverted(true); + + this.m_robotDrive = new DriveSubsystem(navx); + + // Configure default commands + // Set the default drive command to split-stick arcade drive + // // A split-stick arcade command, with forward/backward controlled by the left + // // hand, and turning controlled by the right. + // new RunCommand( + // () -> m_robotDrive.arcadeDrive( + // -m_driverController.getLeftY(), -m_driverController.getRightX()), + // m_robotDrive)); + } + + public Command getAutonomousCommand(Trajectory trajectory) { + // RamseteCommandDebug ramseteCommand = new RamseteCommandDebug( + RamseteController EEEE = new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta); + EEEE.setEnabled(false); + RamseteCommand ramseteCommand = new RamseteCommand( + trajectory, + m_robotDrive::getPose, + EEEE, + new SimpleMotorFeedforward( + DriveConstants.ksVolts, + DriveConstants.kvVoltSecondsPerMeter, + DriveConstants.kaVoltSecondsSquaredPerMeter), + DriveConstants.kDriveKinematics, + m_robotDrive::getWheelSpeeds, + // new PIDController(DriveConstants.kPDriveVel, 0, 0), new + // PIDController(DriveConstants.kPDriveVel, 0, 0), + new PIDController(0, 0, 0), new PIDController(0, 0, 0), + // RamseteCommand passes volts to the callback + m_robotDrive::tankDriveVolts, + m_robotDrive ); - public static class Component { - public static WPI_TalonFX leftATalonFX; - public static WPI_TalonFX leftBTalonFX; - public static WPI_TalonFX rightATalonFX; - public static WPI_TalonFX rightBTalonFX; - public static WPI_TalonFX testTalon; - } - - // The robot's subsystems - // motors - - public final DriveSubsystem m_robotDrive; - // public final XboxController m_driverController; - - /** - * The container for the robot. Contains subsystems, OI devices, and commands. - */ - public RobotContainer2(WPI_TalonFX leftATalonFX, WPI_TalonFX leftBTalonFX, WPI_TalonFX rightATalonFX, WPI_TalonFX rightBTalonFX, AHRS navx) { - - // The driver's controller - // m_driverController = new XboxController(OIConstants.kDriverControllerPort); - - // Configure the button bindings - Component.leftATalonFX = leftATalonFX; - Component.leftBTalonFX = leftBTalonFX; - Component.rightATalonFX = rightATalonFX; - Component.rightBTalonFX = rightBTalonFX; - - - RobotContainer2.Component.leftATalonFX.setNeutralMode(NeutralMode.Coast); - RobotContainer2.Component.leftBTalonFX.setNeutralMode(NeutralMode.Coast); - RobotContainer2.Component.rightATalonFX.setNeutralMode(NeutralMode.Coast); - RobotContainer2.Component.rightBTalonFX.setNeutralMode(NeutralMode.Coast); - - Component.testTalon = new WPI_TalonFX(1); - // Component.leftATalonFX.setInverted(true); - // Component.leftBTalonFX.setInverted(true); - - this.m_robotDrive = new DriveSubsystem(navx); - - // Configure default commands - // Set the default drive command to split-stick arcade drive - // // A split-stick arcade command, with forward/backward controlled by the left - // // hand, and turning controlled by the right. - // new RunCommand( - // () -> m_robotDrive.arcadeDrive( - // -m_driverController.getLeftY(), -m_driverController.getRightX()), - // m_robotDrive)); - } - - public Command getAutonomousCommand(Trajectory trajectory) { - // RamseteCommandDebug ramseteCommand = new RamseteCommandDebug( - RamseteController EEEE = new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta); - EEEE.setEnabled(false); - RamseteCommand ramseteCommand = new RamseteCommand( - trajectory, - m_robotDrive::getPose, - EEEE, - new SimpleMotorFeedforward( - DriveConstants.ksVolts, - DriveConstants.kvVoltSecondsPerMeter, - DriveConstants.kaVoltSecondsSquaredPerMeter), - DriveConstants.kDriveKinematics, - m_robotDrive::getWheelSpeeds, - // new PIDController(DriveConstants.kPDriveVel, 0, 0), new PIDController(DriveConstants.kPDriveVel, 0, 0), - new PIDController(0, 0, 0), new PIDController(0, 0, 0), - // RamseteCommand passes volts to the callback - m_robotDrive::tankDriveVolts, - m_robotDrive); - - // Reset odometry to the starting pose of the trajectory. - // Pose2d initialPose = trajectory.getInitialPose(); - // SmartDashboard.putString("initial pose", initialPose.toString()); - // return new Gaming(m_robotDrive); - // Run path following command, then stop at the end. - // return Commands.run(() -> m_robotDrive.tankDriveVolts(1, 1), m_robotDrive); - //return Commands.runOnce(() -> m_robotDrive.arcadeDrive(0.5, 0), m_robotDrive); - //return Commands.runOnce(() -> Component.testTalon.setVoltage(6)); - return Commands.runOnce(() -> m_robotDrive.resetOdometry(trajectory.getInitialPose()) - ) .andThen( ramseteCommand) - .andThen(() -> m_robotDrive.tankDriveVolts(0, 0)); - } + // Reset odometry to the starting pose of the trajectory. + // Pose2d initialPose = trajectory.getInitialPose(); + // SmartDashboard.putString("initial pose", initialPose.toString()); + // return new Gaming(m_robotDrive); + // Run path following command, then stop at the end. + // return Commands.run(() -> m_robotDrive.tankDriveVolts(1, 1), m_robotDrive); + // return Commands.runOnce(() -> m_robotDrive.arcadeDrive(0.5, 0), + // m_robotDrive); + // return Commands.runOnce(() -> Component.testTalon.setVoltage(6)); + return Commands.runOnce(() -> m_robotDrive.resetOdometry(trajectory.getInitialPose())).andThen(ramseteCommand) + .andThen(() -> m_robotDrive.tankDriveVolts(0, 0)); + } /** * Use this to pass the autonomous command to the main {@link Robot} class. @@ -302,100 +296,105 @@ public Trajectory getTrajectory(String trajectoryName) { // Trajectory trajectory = new Trajectory(); // try { - // Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON); - // trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath); - // System.out.println("v\nv\nv\nv\ntrajectory total time" + String.valueOf(trajectory.getTotalTimeSeconds())); + // Path trajectoryPath = + // Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON); + // trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath); + // System.out.println("v\nv\nv\nv\ntrajectory total time" + + // String.valueOf(trajectory.getTotalTimeSeconds())); // } catch (IOException ex) { - // System.out.println("SHEEEEEESH"); - // DriverStation.reportError("Unable to open trajectory: " + trajectoryJSON, ex.getStackTrace()); + // System.out.println("SHEEEEEESH"); + // DriverStation.reportError("Unable to open trajectory: " + trajectoryJSON, + // ex.getStackTrace()); // } - // return exampleTrajectory; return trajectories.get(trajectoryName); } - public Command balanceAuton(Supplier wheelSpeeds, BiConsumer outputVolts){ - var command = new SequentialCommandGroup( - //1. Position arm to place gamepiece - // TODO: options: either place the game picee, or try to flip over, shoot, and then come back so that we are in the same state + public Command balanceAuton(Supplier wheelSpeeds, BiConsumer outputVolts) { + var command = new SequentialCommandGroup( + // 1. Position arm to place gamepiece + // TODO: options: either place the game picee, or try to flip over, shoot, and + // then come back so that we are in the same state - // implement going over and shooting a cone? + // implement going over and shooting a cone? new ParallelCommandGroup( - //3. Retract arm + // 3. Retract arm // RobotMap.Component.arm.c_posReturnToHomeDown(false), new SequentialCommandGroup( - new WaitCommand(1), //TODO: set wait time to allow arm to get started before moving? - //4. Drive forward past ramp + new WaitCommand(1), // TODO: set wait time to allow arm to get started before moving? + // 4. Drive forward past ramp getAutonomousCommand(getTrajectory("past_ramp")), - //5. Drive back to get partially on ramp + // 5. Drive back to get partially on ramp getAutonomousCommand(getTrajectory("back_to_ramp")) ) ) - // new Balance(RobotMap.Component.navx, wheelSpeeds, outputVolts, 1, -0.1) - //6. balance code here + // new Balance(RobotMap.Component.navx, wheelSpeeds, outputVolts, 1, -0.1) + // 6. balance code here ); - + return command; - } - - public Command balanceAutonAndShootCube(Supplier wheelSpeeds, BiConsumer outputVolts){ - var cmd = RobotMap.Component.arm.c_shootCubes(4, () -> new SequentialCommandGroup( - getAutonomousCommand(getTrajectory("past_ramp")), - getAutonomousCommand(getTrajectory("back_to_ramp")) - )); // high shelf flippy - - return cmd; - } - - // public Command notBalanceAuton(){ - // var command = new SequentialCommandGroup( - // //1. Position arm to place gamepiece - // RobotMap.Component.arm.placeCube(2, true) //TODO: set actual timeout - // , - // new ParallelCommandGroup( - // //3. Retract arm - // RobotMap.Component.arm.c_posReturnToHomeUp(false), - // new SequentialCommandGroup( - // new WaitCommand(1), //TODO: set wait time to allow arm to get started before moving? - // //4. Drive forward past ramp - // getAutonomousCommand(getTrajectory("past_ramp")) - // ) - // ) - // ); - - // return command; - // } + } - public Command newAuton() { - var cmd = RobotMap.Component.arm.c_shootCubes(4, () -> new SequentialCommandGroup( - getAutonomousCommand(getTrajectory("to_ramp")), - getAutonomousCommand(getTrajectory("angle_ramp_forward")), - new WaitCommand(1), - getAutonomousCommand(getTrajectory("go_over_ramp")), - getAutonomousCommand(getTrajectory("angle_ramp_backward")), - new WaitCommand(1), - getAutonomousCommand(getTrajectory("go_middle_ramp")) - )); - - return cmd; - } - - public Command twoPieceAuton(){ //shoot cone, grab cube, shoot cube, doesn't balance - var cmd = RobotMap.Component.arm.c_shootCones(4, // shoot cone - () -> new SequentialCommandGroup( - new ParallelCommandGroup( // then, in parallel - getAutonomousCommand(getTrajectory("go_to_pickup_next")), // go to pickup location - (new WaitCommand(3)).andThen(RobotMap.Component.intake.c_holdVoltage(-8)) // with a delay, start intaking so that we are intaking while we approach the piece - ), - new TriggerCommandFactory( // then, as a separated parallel schedule, - () -> RobotMap.Component.intake.c_holdVoltage(-1), // hold the game piece in - () -> getAutonomousCommand(getTrajectory("from_pickup_to_place")) // return to the next placement location - ), - RobotMap.Component.arm.c_shootCubes(4) // finally, shoot the cube we just picked up and stow - )); - return cmd; - } + public Command balanceAutonAndShootCube(Supplier wheelSpeeds, + BiConsumer outputVolts) { + var cmd = RobotMap.Component.arm.c_shootCubes(4, () -> new SequentialCommandGroup( + getAutonomousCommand(getTrajectory("past_ramp")), + getAutonomousCommand(getTrajectory("back_to_ramp")) + )); // high shelf flippy + + return cmd; + } + + // public Command notBalanceAuton(){ + // var command = new SequentialCommandGroup( + // //1. Position arm to place gamepiece + // RobotMap.Component.arm.placeCube(2, true) //TODO: set actual timeout + // , + // new ParallelCommandGroup( + // //3. Retract arm + // RobotMap.Component.arm.c_posReturnToHomeUp(false), + // new SequentialCommandGroup( + // new WaitCommand(1), //TODO: set wait time to allow arm to get started before + // moving? + // //4. Drive forward past ramp + // getAutonomousCommand(getTrajectory("past_ramp")) + // ) + // ) + // ); + + // return command; + // } + + public Command newAuton() { + var cmd = RobotMap.Component.arm.c_shootCubes(4, () -> new SequentialCommandGroup( + getAutonomousCommand(getTrajectory("to_ramp")), + getAutonomousCommand(getTrajectory("angle_ramp_forward")), + new WaitCommand(1), + getAutonomousCommand(getTrajectory("go_over_ramp")), + getAutonomousCommand(getTrajectory("angle_ramp_backward")), + new WaitCommand(1), + getAutonomousCommand(getTrajectory("go_middle_ramp")) + )); + + return cmd; + } + + public Command twoPieceAuton() { // shoot cone, grab cube, shoot cube, doesn't balance + var cmd = RobotMap.Component.arm.c_shootCones(4, // shoot cone + () -> new SequentialCommandGroup( + new ParallelCommandGroup( // then, in parallel + getAutonomousCommand(getTrajectory("go_to_pickup_next")), // go to pickup location + (new WaitCommand(3)).andThen(RobotMap.Component.intake.c_holdVoltage(-8)) // start intaking when we get close + ), + new TriggerCommandFactory( // then, as a separated parallel schedule, + () -> RobotMap.Component.intake.c_holdVoltage(-1), // hold the game piece in + () -> getAutonomousCommand(getTrajectory("from_pickup_to_place")) // return to the next placement location + ), + RobotMap.Component.arm.c_shootCubes(4) // finally, shoot the cube we just picked up and stow + )); + return cmd; + } } From 34086d67823e7c7c6f1446e6a57bf4df3cdd10d5 Mon Sep 17 00:00:00 2001 From: jac0rr Date: Tue, 4 Apr 2023 17:02:12 -0700 Subject: [PATCH 51/63] add 2piece+balance auton and redo path for it and 2piece -- needs testing --- .../deploy/pathplanner/center_mobility.path | 57 +++++++++++++------ .../robot/seenoevil/RobotContainer2.java | 45 ++++++++++++--- 2 files changed, 79 insertions(+), 23 deletions(-) diff --git a/src/main/deploy/pathplanner/center_mobility.path b/src/main/deploy/pathplanner/center_mobility.path index 77cc004..34e0d4a 100644 --- a/src/main/deploy/pathplanner/center_mobility.path +++ b/src/main/deploy/pathplanner/center_mobility.path @@ -2,13 +2,13 @@ "waypoints": [ { "anchorPoint": { - "x": 1.0, - "y": 3.0 + "x": 1.8058163911466711, + "y": 0.4585108250077607 }, "prevControl": null, "nextControl": { - "x": 2.0, - "y": 3.0 + "x": 2.8058163911466787, + "y": 0.4585108250077607 }, "holonomicAngle": 0, "isReversal": false, @@ -24,19 +24,19 @@ }, { "anchorPoint": { - "x": 3.2257993235545075, - "y": 2.9976040783512325 + "x": 7.0900275255411636, + "y": 0.9205009278074111 }, "prevControl": { - "x": 2.43846492164065, - "y": 3.0023737159346475 + "x": 6.302693123627306, + "y": 0.9252705653908261 }, "nextControl": { - "x": 4.225780974640867, - "y": 2.9915462329755536 + "x": 6.302693123627306, + "y": 0.9252705653908261 }, "holonomicAngle": 0, - "isReversal": false, + "isReversal": true, "velOverride": 1.0, "isLocked": false, "isStopPoint": false, @@ -49,12 +49,37 @@ }, { "anchorPoint": { - "x": 6.162371757772435, - "y": 3.025877703201374 + "x": 1.8230968041797195, + "y": 1.068776149705077 + }, + "prevControl": { + "x": 2.268778005912175, + "y": 1.068776149705077 + }, + "nextControl": { + "x": 2.268778005912175, + "y": 1.068776149705077 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.8415394018288036, + "y": 2.0653572852284596 }, "prevControl": { - "x": 6.015742280723526, - "y": 3.175457471538993 + "x": 2.569189420969124, + "y": 2.0653572852284596 }, "nextControl": null, "holonomicAngle": 0, @@ -72,7 +97,7 @@ ], "maxVelocity": 6.0, "maxAcceleration": 3.0, - "isReversed": null, + "isReversed": false, "markers": [ { "position": 0.5117897727272727, diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java index d89725e..0fd41f6 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java @@ -183,16 +183,29 @@ public class RobotContainer2 { List.of(), new Pose2d(Units.inchesToMeters(53.5), 0, new Rotation2d(Math.PI)), trajectoryConfigReversed)), - entry("go_to_pickup_next", TrajectoryGenerator.generateTrajectory( + entry("go_to_pickup_next", TrajectoryGenerator.generateTrajectory( //from cone place to cube pickup new Pose2d(0, 0, new Rotation2d(0)), List.of(), - new Pose2d(4.44, 0, new Rotation2d(0)), + new Pose2d(4.7625, 0.45085, new Rotation2d(0)), //x is 15 foot 7.5, y is 17.75 inches trajectoryConfig)), - entry("from_pickup_to_place", TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(Math.PI)), - List.of(), - new Pose2d(4.44, 0, new Rotation2d(Math.PI)), - trajectoryConfigReversed))); + entry("from_pickup_to_place", TrajectoryGenerator.generateTrajectory( //from cube pickup to cube node + new Pose2d(0, 0, new Rotation2d(Math.PI)), + List.of(),//same x as last time, little extra is to straighten out, could be tuned + new Pose2d(4.7625+0.1, 0.15, new Rotation2d(Math.PI)),//y is 15 cm to get to the cube node + trajectoryConfigReversed)), + entry("from_cube_place_to_ramp_edge", TrajectoryGenerator.generateTrajectory( //very curvy, might not work if we cant physically turn fast enough + new Pose2d(0, 0, new Rotation2d(0)), //y is 15 cm to get to the cube node + List.of(), + new Pose2d(0.503+0.4, 0.95, new Rotation2d(0)),//TODO: 0.4 is extra to get onto ramp, both needs tuning and we need to now how far we get onto ramp before stalling out + trajectoryConfig)),//I chose 0.4 bc its the length of the first section of the ramp, but other values might be better + entry("onto_ramp", TrajectoryGenerator.generateTrajectory( //balancing on ramp, NEEDS TUNING + new Pose2d(0, 0, new Rotation2d(0)), + List.of(), //chargestation has 2 parts, ramp and platform. 0.61 to balance form robot center at platform start + new Pose2d(0.61+0.2, 0, new Rotation2d(0)),//TODO: 0.2 is extra depending on how far onto the ramp we get stuck -- NEEDS TUNING + trajectoryConfig))); + + + public static class Component { public static WPI_TalonFX leftATalonFX; @@ -397,4 +410,22 @@ public Command twoPieceAuton() { // shoot cone, grab cube, shoot cube, doesn't b )); return cmd; } + public Command twoPieceBalanceAuton() { // shoot cone, grab cube, shoot cube, doesn't balance + var cmd = RobotMap.Component.arm.c_shootCones(4, // shoot cone + () -> new SequentialCommandGroup( + new ParallelCommandGroup( // then, in parallel + getAutonomousCommand(getTrajectory("go_to_pickup_next")), // go to pickup location + (new WaitCommand(2.5)).andThen(RobotMap.Component.intake.c_holdVoltage(-8)) // start intaking when we get close TODO: tune 2.5 sec wait (should be ~.5sec less than time to run spline above, but aim low to be safe) + ), + new TriggerCommandFactory( // then, as a separated parallel schedule, + () -> RobotMap.Component.intake.c_holdVoltage(-1), // hold the game piece in + () -> getAutonomousCommand(getTrajectory("from_pickup_to_place")) // return to the next placement location + ), + RobotMap.Component.arm.c_shootCubes(4), // finally, shoot the cube we just picked up and stow + getAutonomousCommand(getTrajectory("from_cube_place_to_ramp_edge")), + new WaitCommand(1.5), //wait for ramp to lower, TODO: needs tuning -- lower it as much as you can + getAutonomousCommand(getTrajectory("onto_ramp")) + )); + return cmd; + } } From 0f554d325de8ef4f179a2457df51b09d9cf5f85f Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Tue, 4 Apr 2023 20:40:12 -0700 Subject: [PATCH 52/63] praying --- .../java/org/usfirst/frc4904/robot/Robot.java | 4 +- .../org/usfirst/frc4904/robot/RobotMap.java | 4 +- .../operators/DefaultOperator.java | 30 +++-- .../frc4904/robot/seenoevil/Constants.java | 2 +- .../robot/seenoevil/RobotContainer2.java | 116 ++++++++++-------- .../frc4904/robot/subsystems/Intake.java | 6 + .../robot/subsystems/arm/ArmSubsystem.java | 19 +-- 7 files changed, 107 insertions(+), 74 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 825e1a8..1b606dd 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -162,9 +162,9 @@ public void autonomousInitialize() { // SATURDAY MORNING TEST: is the cube shooter auton gonna work - var commnand = donttouchme.balanceAutonAndShootCube(donttouchme.m_robotDrive::getWheelSpeeds, donttouchme.m_robotDrive::tankDriveVolts); // var commnand = donttouchme.balanceAutonAndShootCube(donttouchme.m_robotDrive::getWheelSpeeds, donttouchme.m_robotDrive::tankDriveVolts); - // var commnand = donttouchme.getAutonomousCommand(donttouchme.getTrajectory("straight_forward")); + // var commnand = donttouchme.balanceAutonAndShootCube(donttouchme.m_robotDrive::getWheelSpeeds, donttouchme.m_robotDrive::tankDriveVolts); + var commnand = donttouchme.getAutonomousCommand(donttouchme.getTrajectory("straight_forward")); commnand.schedule(); } diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index 5766e57..8c0e38b 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -88,8 +88,8 @@ public static class Metrics { // // 2023-robot constants public static class Chassis { public static final double GEAR_RATIO = 496/45; // https://www.desmos.com/calculator/llz7giggcf - public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(5 + 1/8); - public static final double TRACK_WIDTH_METERS = .533; // +/- 0.5 inches + public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(5.12); + public static final double TRACK_WIDTH_METERS = .8713; // +/- 0.5 inches public static final double CHASSIS_LENGTH = Units.inchesToMeters(37); // +/- 0.5 inches } diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java index 2325ee5..587edb1 100644 --- a/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java @@ -20,22 +20,20 @@ public DefaultOperator(String name) { public void bindCommands() { var joystick = RobotMap.HumanInput.Operator.joystick; - joystick.button3.onTrue(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> -0.3)); + // manual extension and retraction + joystick.button3.onTrue(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> -0.45)); joystick.button3.onFalse(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> 0)); - - joystick.button5.onTrue(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> 0.3)); + joystick.button5.onTrue(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> 0.45)); joystick.button5.onFalse(RobotMap.Component.arm.armExtensionSubsystem.c_controlVelocity(() -> 0)); // Intake // FIXME: use nameCommand to make it cleaner with expresions (no varibales) var zeroIntake = RobotMap.Component.intake.c_holdVoltage(0); - var holdPiece = RobotMap.Component.intake.c_holdVoltage(-2).withTimeout(0.5).andThen(RobotMap.Component.intake.c_holdVoltage(-1.6)); - var runIntake = RobotMap.Component.intake.c_holdVoltage(-8); var runOuttake = RobotMap.Component.intake.c_holdVoltage(3); // intake - joystick.button2.onTrue(runIntake); - joystick.button2.onFalse(holdPiece); + joystick.button2.onTrue(RobotMap.Component.intake.c_startIntake()); + joystick.button2.onFalse(RobotMap.Component.intake.c_holdItem()); // outtake joystick.button1.onTrue(runOuttake); @@ -50,12 +48,22 @@ public void bindCommands() { joystick.button8 .onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_shootCones(3))); joystick.button10.onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_shootCones(2))); + // intake positions - joystick.button6 .onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posIntakeShelf())); - joystick.button4 .onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posIntakeFloor())); + joystick.button6 .onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posIntakeShelf(null))); + joystick.button4 .onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posIntakeFloor(null))); // stow positions - joystick.button11.onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posReturnToHomeDown())); - joystick.button12.onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posReturnToHomeUp())); + joystick.button11.onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posReturnToHomeDown(null))); + joystick.button12.onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posReturnToHomeUp(null))); + + + // // intake positions + // joystick.button6 .onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posIntakeShelf(() -> RobotMap.Component.intake.c_startIntake()))); + // joystick.button4 .onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posIntakeFloor(() -> RobotMap.Component.intake.c_startIntake()))); + + // // stow positions + // joystick.button11.onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posReturnToHomeDown(() -> RobotMap.Component.intake.c_holdItem()))); + // joystick.button12.onTrue(new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posReturnToHomeUp(() -> RobotMap.Component.intake.c_holdItem()))); } } diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/Constants.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/Constants.java index 0efaf3e..e8f098e 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/Constants.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/Constants.java @@ -81,6 +81,6 @@ public static final class AutoConstants { // Reasonable baseline values for a RAMSETE follower in units of meters and seconds public static final double kRamseteB = 2; - public static final double kRamseteZeta = 0.2; + public static final double kRamseteZeta = 0.13; } } diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java index 0fd41f6..acc55da 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java @@ -117,44 +117,48 @@ public class RobotContainer2 { 10)) .setReversed(true); - - private static Map trajectories = Map.ofEntries( - entry("sickle", TrajectoryGenerator.generateTrajectory( - // Start at the origin facing the +X direction - new Pose2d(0, 0, new Rotation2d(0)), - // Pass through these two interior waypoints, making an 's' curve path - // List.of(new Translation2d(0.33*dist, .15*dist), new Translation2d(0.66*dist, - // -.15*dist)), - List.of(new Translation2d(1, -1), new Translation2d(2, -1)), - - // End 3 meters straight ahead of where we started, facing forward - new Pose2d(3, 0, new Rotation2d(0)), - trajectoryConfig)), - entry("straight_forward", TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(0)), - List.of(new Translation2d(1, 0)), - new Pose2d(6, 0, new Rotation2d(0)), - trajectoryConfig)), - entry("straight_backward", TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(Math.PI)), - List.of(new Translation2d(1, 0)), - new Pose2d(6, 0, new Rotation2d(Math.PI)), - trajectoryConfigReversed)), - entry("turn_right", TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(0)), - List.of(), - new Pose2d(1, -1, new Rotation2d(-Math.PI / 2)), - trajectoryConfig)), - entry("past_ramp", TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(0)), - List.of(), - new Pose2d(4, 0, new Rotation2d(0)), - trajectoryConfig)), - entry("back_to_ramp", TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(Math.PI)), - List.of(), - new Pose2d(1, 0, new Rotation2d(Math.PI)), - trajectoryConfigReversed)), + private static Map trajectories = Map.ofEntries( + entry("sickle", TrajectoryGenerator.generateTrajectory( + // Start at the origin facing the +X direction + new Pose2d(0, 0, new Rotation2d(0)), + // Pass through these two interior waypoints, making an 's' curve path + // List.of(new Translation2d(0.33*dist, .15*dist), new Translation2d(0.66*dist, -.15*dist)), + List.of(new Translation2d(1, -1), new Translation2d(2, -1)), + + // End 3 meters straight ahead of where we started, facing forward + new Pose2d(3, 0, new Rotation2d(0)), + trajectoryConfig) + ), + entry("straight_forward", TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(0)), + List.of(),//List.of(new Translation2d(2, 0)), + new Pose2d(2, 0, new Rotation2d(Math.PI/4)), + trajectoryConfig + )), + entry("straight_backward", TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(Math.PI)), + List.of(new Translation2d(1, 0)), + new Pose2d(6, 0, new Rotation2d(Math.PI)), + trajectoryConfigReversed + )), + entry("turn_right", TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(0)), + List.of(), + new Pose2d(1, -1, new Rotation2d(-Math.PI/2)), + trajectoryConfig + )), + entry("past_ramp", TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(0)), + List.of(), + new Pose2d(4, 0, new Rotation2d(0)), + trajectoryConfig + )), + entry("back_to_ramp", TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(Math.PI)), + List.of(), + new Pose2d(1, 0, new Rotation2d(Math.PI)), + trajectoryConfigReversed + )), // new auton entry("to_ramp", TrajectoryGenerator.generateTrajectory( @@ -278,19 +282,29 @@ public Command getAutonomousCommand(Trajectory trajectory) { m_robotDrive::tankDriveVolts, m_robotDrive ); - - // Reset odometry to the starting pose of the trajectory. - // Pose2d initialPose = trajectory.getInitialPose(); - // SmartDashboard.putString("initial pose", initialPose.toString()); - // return new Gaming(m_robotDrive); - // Run path following command, then stop at the end. - // return Commands.run(() -> m_robotDrive.tankDriveVolts(1, 1), m_robotDrive); - // return Commands.runOnce(() -> m_robotDrive.arcadeDrive(0.5, 0), - // m_robotDrive); - // return Commands.runOnce(() -> Component.testTalon.setVoltage(6)); - return Commands.runOnce(() -> m_robotDrive.resetOdometry(trajectory.getInitialPose())).andThen(ramseteCommand) - .andThen(() -> m_robotDrive.tankDriveVolts(0, 0)); - } + + // Reset odometry to the starting pose of the trajectory. + // Pose2d initialPose = trajectory.getInitialPose(); + // SmartDashboard.putString("initial pose", initialPose.toString()); + // return new Gaming(m_robotDrive); + // Run path following command, then stop at the end. + // return Commands.run(() -> m_robotDrive.tankDriveVolts(1, 1), m_robotDrive); + //return Commands.runOnce(() -> m_robotDrive.arcadeDrive(0.5, 0), m_robotDrive); + //return Commands.runOnce(() -> Component.testTalon.setVoltage(6)); + // m_robotDrive.resetOdometry(trajectory.getInitialPose()); + // return Commands.runOnce(() -> m_robotDrive.resetOdometry(trajectory.getInitialPose()) + // ) .andThen( ramseteCommand) + // .andThen(() -> m_robotDrive.tankDriveVolts(0, 0)); + + var cmd = new SequentialCommandGroup( + Commands.runOnce(() -> m_robotDrive.resetOdometry(trajectory.getInitialPose())), + ramseteCommand, + Commands.runOnce(() -> m_robotDrive.tankDriveVolts(0, 0)), + Commands.runOnce(() -> m_robotDrive.resetOdometry(trajectory.getInitialPose())) + ); + cmd.addRequirements(m_robotDrive); + return cmd; + } /** * Use this to pass the autonomous command to the main {@link Robot} class. diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/Intake.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/Intake.java index c4f9e3c..79d87e9 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/Intake.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/Intake.java @@ -37,6 +37,12 @@ public Command c_holdVoltage(double voltage) { setVoltage(voltage); }, leftMotors, rightMotors); } + public Command c_startIntake() { + return c_holdVoltage(-8); + } + public Command c_holdItem() { + return c_holdVoltage(-2).withTimeout(0.5).andThen(c_holdVoltage(-1.6)); + } public Command c_holdVoltageDefault() { var cmd = Commands.run(() -> setVoltage(DEFAULT_INTAKE_VOLTS), leftMotors, rightMotors); diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index 95f33db..e153d67 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -26,8 +26,8 @@ public class ArmSubsystem extends SubsystemBase { public final ArmPivotSubsystem armPivotSubsystem; public final ArmExtensionSubsystem armExtensionSubsystem; - public static final double MAX_VELOCITY_EXTENSION = 1; - public static final double MAX_ACCEL_EXTENSION = 2; + public static final double MAX_VELOCITY_EXTENSION = 1.5; + public static final double MAX_ACCEL_EXTENSION = 3; public static final double MAX_VELOCITY_PIVOT = 160; public static final double MAX_ACCEL_PIVOT = 210; @@ -36,7 +36,7 @@ public class ArmSubsystem extends SubsystemBase { public static final HashMap> shelfCones = new HashMap<>(); //in degrees, meters static { // SHELF CONES // cones.put(1, new Triple<>(-19., Units.inchesToMeters(0), 3.)); - shelfCones.put(2, new Triple<>(29., Units.inchesToMeters(15), 3.)); + shelfCones.put(2, new Triple<>(29.5, Units.inchesToMeters(18), 3.)); shelfCones.put(3, new Triple<>(41., ArmExtensionSubsystem.MAX_EXTENSION_M, 3.)); shelfCones.put(4, new Triple<>(180.0-41, ArmExtensionSubsystem.MAX_EXTENSION_M, 3.)); } @@ -87,16 +87,16 @@ public Command c_posReturnToHomeDown(Supplier onArrivalCommandDealer) { cmd.setName("arm position - home (down)"); return cmd; } - public Command c_posIntakeShelf() { + public Command c_posIntakeShelf(Supplier onArrivalCommandDealer) { // TODO: back up 14 inches -- remember to always use meters cones = shelfCones; - var cmd = c_holdArmPose(otherPositions.get("intakeShelf")); + var cmd = c_holdArmPose(otherPositions.get("intakeShelf"), onArrivalCommandDealer); cmd.setName("arm position - pre shelf intake"); return cmd; } - public Command c_posIntakeFloor() { + public Command c_posIntakeFloor(Supplier onArrivalCommandDealer) { cones = floorCones; - var cmd = c_holdArmPose(otherPositions.get("homeDown")); + var cmd = c_holdArmPose(otherPositions.get("homeDown"), onArrivalCommandDealer); cmd.setName("arm position - pre floor intake"); return cmd; } @@ -114,6 +114,11 @@ public Command c_shootCones(int shelf, Supplier onArrivalCommandDealer) var extensionLengthMeters = cones.get(shelf).getSecond(); var voltage = cones.get(shelf).getThird(); + + if (cones == shelfCones) { + return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters); + } + return c_holdArmPose(degreesFromHorizontal, extensionLengthMeters, () -> new SequentialCommandGroup( RobotMap.Component.intake.c_holdVoltage(voltage).withTimeout(0.5), From 523d59959d8e1a61c98629936b59a39ec135c7c9 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Tue, 4 Apr 2023 20:48:41 -0700 Subject: [PATCH 53/63] simple hallway practice auton (straight fwd) --- .../robot/seenoevil/RobotContainer2.java | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java index acc55da..481e662 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java @@ -442,4 +442,21 @@ public Command twoPieceBalanceAuton() { // shoot cone, grab cube, shoot cube, do )); return cmd; } + + public Command hallwayPracticeAuton() { // shoot cone, grab cube, shoot cube, doesn't balance + var cmd = RobotMap.Component.arm.c_shootCubes(4, // shoot cone + () -> new SequentialCommandGroup( + new ParallelCommandGroup( // then, in parallel + getAutonomousCommand(getTrajectory("straight_forward")), // go to pickup location + (new WaitCommand(1)).andThen(RobotMap.Component.intake.c_holdVoltage(-8)) // start intaking when we get close TODO: tune 2.5 sec wait (should be ~.5sec less than time to run spline above, but aim low to be safe) + ), + new TriggerCommandFactory( // then, as a separated parallel schedule, + (Supplier)() -> RobotMap.Component.intake.c_holdItem(), // hold the game piece in + () -> getAutonomousCommand(getTrajectory("straight_backward")) // return to the next placement location + ), + RobotMap.Component.arm.c_shootCubes(5), // finally, shoot the cube we just picked up and stow + getAutonomousCommand(getTrajectory("straight_forward")) + )); + return cmd; + } } From 14bcc3da6312e36007f2af41bef79bf30bc60e28 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Tue, 4 Apr 2023 20:49:45 -0700 Subject: [PATCH 54/63] make straight forward actually go straight forward --- .../org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java index 481e662..76b2345 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java @@ -132,13 +132,13 @@ public class RobotContainer2 { entry("straight_forward", TrajectoryGenerator.generateTrajectory( new Pose2d(0, 0, new Rotation2d(0)), List.of(),//List.of(new Translation2d(2, 0)), - new Pose2d(2, 0, new Rotation2d(Math.PI/4)), + new Pose2d(2, 0, new Rotation2d(0)), trajectoryConfig )), entry("straight_backward", TrajectoryGenerator.generateTrajectory( new Pose2d(0, 0, new Rotation2d(Math.PI)), List.of(new Translation2d(1, 0)), - new Pose2d(6, 0, new Rotation2d(Math.PI)), + new Pose2d(2, 0, new Rotation2d(Math.PI)), trajectoryConfigReversed )), entry("turn_right", TrajectoryGenerator.generateTrajectory( From cb591e5d665cbf396799c3e374f000a993a4a7ae Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Tue, 4 Apr 2023 20:50:31 -0700 Subject: [PATCH 55/63] For debugging --- src/main/java/org/usfirst/frc4904/robot/seenoevil/Constants.java | 1 + .../org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java | 1 + 2 files changed, 2 insertions(+) diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/Constants.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/Constants.java index e8f098e..6c2de0a 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/Constants.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/Constants.java @@ -78,6 +78,7 @@ public static final class DriveConstants { public static final class AutoConstants { public static final double kMaxSpeedMetersPerSecond = 3.0; // max like 6mps public static final double kMaxAccelerationMetersPerSecondSquared = 2.0; + public static final double kMaxCentripitalAcceleration = -1; // TODO set to something useful // Reasonable baseline values for a RAMSETE follower in units of meters and seconds public static final double kRamseteB = 2; diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java index acc55da..4d26065 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java @@ -19,6 +19,7 @@ import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.math.trajectory.TrajectoryUtil; +import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint; import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; From b1ac8a96346e613d43356ad5359dc25f37cae2ba Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Tue, 4 Apr 2023 22:38:28 -0700 Subject: [PATCH 56/63] m --- src/main/java/org/usfirst/frc4904/robot/Robot.java | 6 +++++- .../frc4904/robot/seenoevil/RobotContainer2.java | 10 +++++----- .../frc4904/robot/subsystems/arm/ArmSubsystem.java | 8 +++++--- 3 files changed, 15 insertions(+), 9 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 1b606dd..7b0a757 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -164,7 +164,7 @@ public void autonomousInitialize() { // SATURDAY MORNING TEST: is the cube shooter auton gonna work // var commnand = donttouchme.balanceAutonAndShootCube(donttouchme.m_robotDrive::getWheelSpeeds, donttouchme.m_robotDrive::tankDriveVolts); // var commnand = donttouchme.balanceAutonAndShootCube(donttouchme.m_robotDrive::getWheelSpeeds, donttouchme.m_robotDrive::tankDriveVolts); - var commnand = donttouchme.getAutonomousCommand(donttouchme.getTrajectory("straight_forward")); + var commnand = donttouchme.hallwayPracticeAuton(); commnand.schedule(); } @@ -185,6 +185,10 @@ public void autonomousExecute() { SmartDashboard.putNumber("Falcon temp", RobotContainer2.Component.leftATalonFX.getTemperature()); + SmartDashboard.putData(RobotMap.Component.arm.armPivotSubsystem); + SmartDashboard.putData(RobotMap.Component.arm.armExtensionSubsystem); + SmartDashboard.putData(RobotMap.Component.arm); + SmartDashboard.putData(donttouchme.m_robotDrive); } @Override diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java index 1c09480..34d0701 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java @@ -265,7 +265,7 @@ public RobotContainer2(WPI_TalonFX leftATalonFX, WPI_TalonFX leftBTalonFX, WPI_T public Command getAutonomousCommand(Trajectory trajectory) { // RamseteCommandDebug ramseteCommand = new RamseteCommandDebug( RamseteController EEEE = new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta); - EEEE.setEnabled(false); + EEEE.setEnabled(true); RamseteCommand ramseteCommand = new RamseteCommand( trajectory, m_robotDrive::getPose, @@ -449,11 +449,11 @@ public Command hallwayPracticeAuton() { // shoot cone, grab cube, shoot cube, do () -> new SequentialCommandGroup( new ParallelCommandGroup( // then, in parallel getAutonomousCommand(getTrajectory("straight_forward")), // go to pickup location - (new WaitCommand(1)).andThen(RobotMap.Component.intake.c_holdVoltage(-8)) // start intaking when we get close TODO: tune 2.5 sec wait (should be ~.5sec less than time to run spline above, but aim low to be safe) + RobotMap.Component.arm.c_posIntakeFloor(() -> RobotMap.Component.intake.c_holdVoltage(-8).withTimeout(5).andThen(RobotMap.Component.intake.c_holdItem()))// start intaking when we get close TODO: tune 2.5 sec wait (should be ~.5sec less than time to run spline above, but aim low to be safe) ), - new TriggerCommandFactory( // then, as a separated parallel schedule, - (Supplier)() -> RobotMap.Component.intake.c_holdItem(), // hold the game piece in - () -> getAutonomousCommand(getTrajectory("straight_backward")) // return to the next placement location + new ParallelCommandGroup( // then, as a separated parallel schedule, + getAutonomousCommand(getTrajectory("straight_backward")) // return to the next placement location + // RobotMap.Component.intake.c_holdItem() // hold the game piece in ), RobotMap.Component.arm.c_shootCubes(5), // finally, shoot the cube we just picked up and stow getAutonomousCommand(getTrajectory("straight_forward")) diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index e153d67..58576a3 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -47,6 +47,7 @@ public class ArmSubsystem extends SubsystemBase { // cones.put(1, new Triple<>(-19., Units.inchesToMeters(0), 3.)); floorCones.put(2, new Triple<>(29., Units.inchesToMeters(14), 3.)); floorCones.put(3, new Triple<>(29., ArmExtensionSubsystem.MAX_EXTENSION_M-0.02, 3.)); + floorCones.put(4, new Triple<>(180.0-41, ArmExtensionSubsystem.MAX_EXTENSION_M, 3.)); } public static HashMap> cones = floorCones; @@ -56,8 +57,8 @@ public class ArmSubsystem extends SubsystemBase { // cubes.put(1, new Triple<>(-33., Units.inchesToMeters(0), 3.)); cubes.put(2, new Triple<>(15., Units.inchesToMeters(0), 4.5)); cubes.put(3, new Triple<>(20., Units.inchesToMeters(0), 4.5)); - cubes.put(4, new Triple<>(180.-20, Units.inchesToMeters(0), 4.5)); - cubes.put(5, new Triple<>(180.-15, Units.inchesToMeters(0), 4.5)); + cubes.put(4, new Triple<>(180.-35, Units.inchesToMeters(0), 4.5)); + cubes.put(5, new Triple<>(180.-25, Units.inchesToMeters(0), 4.5)); } public static final HashMap> otherPositions = new HashMap<>(); @@ -135,7 +136,8 @@ public Command c_angleCubes(int shelf) { } public Command c_shootCubes(int shelf) { return c_shootCubes(shelf, null); } - public Command c_shootCubes(int shelf, Supplier onArrivalCommandDealer) { + public Command + c_shootCubes(int shelf, Supplier onArrivalCommandDealer) { var degreesFromHorizontal = cubes.get(shelf).getFirst(); var extensionLengthMeters = cubes.get(shelf).getSecond(); var voltage = cubes.get(shelf).getThird(); From 56e26f5e7df3a58e4ebe9b49f5717c5be249e61e Mon Sep 17 00:00:00 2001 From: Exr0n Date: Wed, 5 Apr 2023 14:36:26 -0700 Subject: [PATCH 57/63] debugging hallway auton AM --- .../java/org/usfirst/frc4904/robot/Robot.java | 18 ++++---- .../frc4904/robot/seenoevil/Constants.java | 2 +- .../robot/seenoevil/RobotContainer2.java | 44 +++++++++++++------ .../subsystems/arm/ArmExtensionSubsystem.java | 5 ++- .../subsystems/arm/ArmPivotSubsystem.java | 5 ++- 5 files changed, 46 insertions(+), 28 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 7b0a757..6194e57 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -173,17 +173,17 @@ public void autonomousExecute() { // TODO remove logging - SmartDashboard.putBoolean("isFlipped - IMPORTANT", NathanGain.isFlippy); - SmartDashboard.putString("pose string", donttouchme.m_robotDrive.getPose().toString()); - SmartDashboard.putNumber("pose x", donttouchme.m_robotDrive.getPose().getX()); - SmartDashboard.putNumber("pose y", donttouchme.m_robotDrive.getPose().getY()); - SmartDashboard.putNumber("pose heading", donttouchme.m_robotDrive.getPose().getRotation().getDegrees()); + // SmartDashboard.putBoolean("isFlipped - IMPORTANT", NathanGain.isFlippy); + // SmartDashboard.putString("pose string", donttouchme.m_robotDrive.getPose().toString()); + // SmartDashboard.putNumber("pose x", donttouchme.m_robotDrive.getPose().getX()); + // SmartDashboard.putNumber("pose y", donttouchme.m_robotDrive.getPose().getY()); + // SmartDashboard.putNumber("pose heading", donttouchme.m_robotDrive.getPose().getRotation().getDegrees()); - SmartDashboard.putNumber("gyroooo", RobotMap.Component.navx.getAngle()); - SmartDashboard.putNumber("armV extension length", RobotMap.Component.arm.armExtensionSubsystem.getCurrentExtensionLength()); - SmartDashboard.putNumber("arm pivot angle", RobotMap.Component.arm.armPivotSubsystem.getCurrentAngleDegrees()); + // SmartDashboard.putNumber("gyroooo", RobotMap.Component.navx.getAngle()); + // SmartDashboard.putNumber("armV extension length", RobotMap.Component.arm.armExtensionSubsystem.getCurrentExtensionLength()); + // SmartDashboard.putNumber("arm pivot angle", RobotMap.Component.arm.armPivotSubsystem.getCurrentAngleDegrees()); - SmartDashboard.putNumber("Falcon temp", RobotContainer2.Component.leftATalonFX.getTemperature()); + // SmartDashboard.putNumber("Falcon temp", RobotContainer2.Component.leftATalonFX.getTemperature()); SmartDashboard.putData(RobotMap.Component.arm.armPivotSubsystem); SmartDashboard.putData(RobotMap.Component.arm.armExtensionSubsystem); diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/Constants.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/Constants.java index 6c2de0a..81ba365 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/Constants.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/Constants.java @@ -82,6 +82,6 @@ public static final class AutoConstants { // Reasonable baseline values for a RAMSETE follower in units of meters and seconds public static final double kRamseteB = 2; - public static final double kRamseteZeta = 0.13; + public static final double kRamseteZeta = 0.127; } } diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java index 34d0701..12ba504 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java @@ -138,7 +138,7 @@ public class RobotContainer2 { )), entry("straight_backward", TrajectoryGenerator.generateTrajectory( new Pose2d(0, 0, new Rotation2d(Math.PI)), - List.of(new Translation2d(1, 0)), + List.of(), new Pose2d(2, 0, new Rotation2d(Math.PI)), trajectoryConfigReversed )), @@ -445,19 +445,35 @@ public Command twoPieceBalanceAuton() { // shoot cone, grab cube, shoot cube, do } public Command hallwayPracticeAuton() { // shoot cone, grab cube, shoot cube, doesn't balance - var cmd = RobotMap.Component.arm.c_shootCubes(4, // shoot cone - () -> new SequentialCommandGroup( - new ParallelCommandGroup( // then, in parallel - getAutonomousCommand(getTrajectory("straight_forward")), // go to pickup location - RobotMap.Component.arm.c_posIntakeFloor(() -> RobotMap.Component.intake.c_holdVoltage(-8).withTimeout(5).andThen(RobotMap.Component.intake.c_holdItem()))// start intaking when we get close TODO: tune 2.5 sec wait (should be ~.5sec less than time to run spline above, but aim low to be safe) - ), - new ParallelCommandGroup( // then, as a separated parallel schedule, - getAutonomousCommand(getTrajectory("straight_backward")) // return to the next placement location - // RobotMap.Component.intake.c_holdItem() // hold the game piece in - ), - RobotMap.Component.arm.c_shootCubes(5), // finally, shoot the cube we just picked up and stow + // var onArrivalCommand = getAutonomousCommand(getTrajectory("straight_forward")); // go to pickup location + var onArrivalCommand = new SequentialCommandGroup( + getAutonomousCommand(getTrajectory("straight_forward")), // go to pickup location + getAutonomousCommand(getTrajectory("straight_backward")), // return to the next placement location getAutonomousCommand(getTrajectory("straight_forward")) - )); - return cmd; + ); + // var onArrivalCommand = new SequentialCommandGroup( + // new ParallelDeadlineGroup( // then, in parallel + // (new WaitCommand(1)).andThen(getAutonomousCommand(getTrajectory("straight_forward"))) // go to pickup location + // //, new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posIntakeFloor(() -> RobotMap.Component.intake.c_holdVoltage(-8).withTimeout(5).andThen(RobotMap.Component.intake.c_holdItem())))// start intaking when we get close TODO: tune 2.5 sec wait (should be ~.5sec less than time to run spline above, but aim low to be safe) + // //, new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posIntakeFloor(() -> RobotMap.Component.intake.c_holdVoltage(-8.)))// start intaking when we get close TODO: tune 2.5 sec wait (should be ~.5sec less than time to run spline above, but aim low to be safe) + // ), + // new ParallelDeadlineGroup( // then, as a separated parallel schedule, + // getAutonomousCommand(getTrajectory("straight_backward")) // return to the next placement location + // // , new TriggerCommandFactory(() -> RobotMap.Component.intake.c_holdItem()) // hold the game piece in + // ), + // // new TriggerCommandFactory(() -> RobotMap.Component.arm.c_shootCubes(5, () -> getAutonomousCommand(getTrajectory("straight_forward")))) // finally, shoot the cube we just picked up and stow + // getAutonomousCommand(getTrajectory("straight_forward")) + // ); + + // var cmd = RobotMap.Component.arm.c_shootCubes(4, () -> onArrivalCommand); + var cmd = RobotMap.Component.arm.c_shootCubes(4); + SmartDashboard.putString("auton reqs", onArrivalCommand.getRequirements().toString()); + + var total_parallel = new ParallelCommandGroup( + new TriggerCommandFactory(() -> cmd), + new TriggerCommandFactory(() -> (new WaitCommand(2.5)).andThen(onArrivalCommand)) + ); + // return cmd; + return onArrivalCommand; } } diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java index 2ccd342..35055ad 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java @@ -23,6 +23,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WrapperCommand; +import static org.usfirst.frc4904.robot.Utils.nameCommand; public class ArmExtensionSubsystem extends SubsystemBase { public static final double MAXIMUM_HORIZONTAL_SAFE_EXTENSION_M = Units.inchesToMeters(48); @@ -119,12 +120,12 @@ public void updateSetpoint(double setpoint, double setpoint_dt) { new TrapezoidProfile.State((getCurrentExtensionLength() - 0.0853)/0.968, 0)); var cmd = new ezMotion(controller, this::getCurrentExtensionLength, motor::setVoltage, () -> (double t) -> new Tuple2(profile.calculate(t).position, profile.calculate(t).velocity), this, motor); cmd.setName("arm - c_holdExtension"); - return onArrivalCommandDealer == null ? cmd : new ParallelCommandGroup( + return onArrivalCommandDealer == null ? cmd : nameCommand("extension w/ onArrival", new ParallelCommandGroup( cmd, new SequentialCommandGroup( new WaitCommand(profile.totalTime()), new TriggerCommandFactory("arm extension", onArrivalCommandDealer) - ) + )) ); } } diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java index 34e4822..3e23a5a 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java @@ -21,6 +21,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.WaitCommand; +import static org.usfirst.frc4904.robot.Utils.nameCommand; public class ArmPivotSubsystem extends SubsystemBase { public static final double HARD_STOP_ARM_ANGLE = -38; @@ -167,12 +168,12 @@ public void updateSetpoint(double setpoint, double setpoint_dt) { cmd.setName("arm - c_holdRotation"); // return new Pair(new ezMotion(controller, () -> this.getCurrentAngleDegrees() * Math.PI / 180, armMotorGroup::setVoltage, // (double t) -> new Tuple2(profile.calculate(t).position, profile.calculate(t).velocity), this), profile.totalTime()); - return onArrivalCommandDealer == null ? cmd : new ParallelCommandGroup( + return onArrivalCommandDealer == null ? cmd : nameCommand("pivot w/ onArrival: " + cmd.getName(), new ParallelCommandGroup( cmd, new SequentialCommandGroup( new WaitCommand(profile.totalTime()), new TriggerCommandFactory("arm pivot", onArrivalCommandDealer) - ) + )) ); } } From 81f8196f868a56992d9e3dde50329ae52a6d98b4 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Wed, 5 Apr 2023 18:37:48 -0700 Subject: [PATCH 58/63] snap: last robot testing before comp (test autons) --- .../java/org/usfirst/frc4904/robot/Robot.java | 26 +++---- .../robot/seenoevil/RobotContainer2.java | 75 +++++++++++-------- 2 files changed, 55 insertions(+), 46 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 6194e57..9bdad2c 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -164,7 +164,7 @@ public void autonomousInitialize() { // SATURDAY MORNING TEST: is the cube shooter auton gonna work // var commnand = donttouchme.balanceAutonAndShootCube(donttouchme.m_robotDrive::getWheelSpeeds, donttouchme.m_robotDrive::tankDriveVolts); // var commnand = donttouchme.balanceAutonAndShootCube(donttouchme.m_robotDrive::getWheelSpeeds, donttouchme.m_robotDrive::tankDriveVolts); - var commnand = donttouchme.hallwayPracticeAuton(); + var commnand = donttouchme.twoPieceBalanceAuton(); commnand.schedule(); } @@ -174,21 +174,21 @@ public void autonomousExecute() { // SmartDashboard.putBoolean("isFlipped - IMPORTANT", NathanGain.isFlippy); - // SmartDashboard.putString("pose string", donttouchme.m_robotDrive.getPose().toString()); - // SmartDashboard.putNumber("pose x", donttouchme.m_robotDrive.getPose().getX()); - // SmartDashboard.putNumber("pose y", donttouchme.m_robotDrive.getPose().getY()); - // SmartDashboard.putNumber("pose heading", donttouchme.m_robotDrive.getPose().getRotation().getDegrees()); + SmartDashboard.putString("pose string", donttouchme.m_robotDrive.getPose().toString()); + SmartDashboard.putNumber("pose x", donttouchme.m_robotDrive.getPose().getX()); + SmartDashboard.putNumber("pose y", donttouchme.m_robotDrive.getPose().getY()); + SmartDashboard.putNumber("pose heading", donttouchme.m_robotDrive.getPose().getRotation().getDegrees()); - // SmartDashboard.putNumber("gyroooo", RobotMap.Component.navx.getAngle()); - // SmartDashboard.putNumber("armV extension length", RobotMap.Component.arm.armExtensionSubsystem.getCurrentExtensionLength()); - // SmartDashboard.putNumber("arm pivot angle", RobotMap.Component.arm.armPivotSubsystem.getCurrentAngleDegrees()); + SmartDashboard.putNumber("gyroooo", RobotMap.Component.navx.getAngle()); + SmartDashboard.putNumber("armV extension length", RobotMap.Component.arm.armExtensionSubsystem.getCurrentExtensionLength()); + SmartDashboard.putNumber("arm pivot angle", RobotMap.Component.arm.armPivotSubsystem.getCurrentAngleDegrees()); // SmartDashboard.putNumber("Falcon temp", RobotContainer2.Component.leftATalonFX.getTemperature()); - SmartDashboard.putData(RobotMap.Component.arm.armPivotSubsystem); - SmartDashboard.putData(RobotMap.Component.arm.armExtensionSubsystem); - SmartDashboard.putData(RobotMap.Component.arm); - SmartDashboard.putData(donttouchme.m_robotDrive); + // SmartDashboard.putData(RobotMap.Component.arm.armPivotSubsystem); + // SmartDashboard.putData(RobotMap.Component.arm.armExtensionSubsystem); + // SmartDashboard.putData(RobotMap.Component.arm); + // SmartDashboard.putData(donttouchme.m_robotDrive); } @Override @@ -228,8 +228,6 @@ public void testInitialize() { @Override public void testExecute() { RobotMap.Component.arm.armExtensionSubsystem.initializeEncoderPositions(0); - - } diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java index 12ba504..0e6bfb7 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java @@ -265,7 +265,7 @@ public RobotContainer2(WPI_TalonFX leftATalonFX, WPI_TalonFX leftBTalonFX, WPI_T public Command getAutonomousCommand(Trajectory trajectory) { // RamseteCommandDebug ramseteCommand = new RamseteCommandDebug( RamseteController EEEE = new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta); - EEEE.setEnabled(true); + EEEE.setEnabled(false); RamseteCommand ramseteCommand = new RamseteCommand( trajectory, m_robotDrive::getPose, @@ -297,12 +297,22 @@ public Command getAutonomousCommand(Trajectory trajectory) { // ) .andThen( ramseteCommand) // .andThen(() -> m_robotDrive.tankDriveVolts(0, 0)); + var cmd = new SequentialCommandGroup( - Commands.runOnce(() -> m_robotDrive.resetOdometry(trajectory.getInitialPose())), + Commands.runOnce(() -> { + Pose2d init = trajectory.getInitialPose(); + m_robotDrive.resetOdometry(new Pose2d(init.getX(), init.getY(), new Rotation2d(0))); + SmartDashboard.putString("Trajg", init.toString()); + }), ramseteCommand, Commands.runOnce(() -> m_robotDrive.tankDriveVolts(0, 0)), - Commands.runOnce(() -> m_robotDrive.resetOdometry(trajectory.getInitialPose())) - ); + Commands.runOnce(() -> { + Pose2d init = trajectory.getInitialPose(); + // m_robotDrive.resetOdometry(init); + m_robotDrive.resetOdometry(new Pose2d(init.getX(), init.getY(), new Rotation2d(0))); + + SmartDashboard.putString("Trajg2", init.toString()); + }) ); cmd.addRequirements(m_robotDrive); return cmd; } @@ -428,15 +438,15 @@ public Command twoPieceAuton() { // shoot cone, grab cube, shoot cube, doesn't b public Command twoPieceBalanceAuton() { // shoot cone, grab cube, shoot cube, doesn't balance var cmd = RobotMap.Component.arm.c_shootCones(4, // shoot cone () -> new SequentialCommandGroup( - new ParallelCommandGroup( // then, in parallel - getAutonomousCommand(getTrajectory("go_to_pickup_next")), // go to pickup location - (new WaitCommand(2.5)).andThen(RobotMap.Component.intake.c_holdVoltage(-8)) // start intaking when we get close TODO: tune 2.5 sec wait (should be ~.5sec less than time to run spline above, but aim low to be safe) - ), - new TriggerCommandFactory( // then, as a separated parallel schedule, - () -> RobotMap.Component.intake.c_holdVoltage(-1), // hold the game piece in - () -> getAutonomousCommand(getTrajectory("from_pickup_to_place")) // return to the next placement location - ), - RobotMap.Component.arm.c_shootCubes(4), // finally, shoot the cube we just picked up and stow + // new ParallelCommandGroup( // then, in parallel + // getAutonomousCommand(getTrajectory("go_to_pickup_next")), // go to pickup location + // (new WaitCommand(2.5)).andThen(RobotMap.Component.intake.c_holdVoltage(-8)) // start intaking when we get close TODO: tune 2.5 sec wait (should be ~.5sec less than time to run spline above, but aim low to be safe) + // ), + // new TriggerCommandFactory( // then, as a separated parallel schedule, + // () -> RobotMap.Component.intake.c_holdVoltage(-1), // hold the game piece in + // () -> getAutonomousCommand(getTrajectory("from_pickup_to_place")) // return to the next placement location + // ), + // RobotMap.Component.arm.c_shootCubes(4), // finally, shoot the cube we just picked up and stow getAutonomousCommand(getTrajectory("from_cube_place_to_ramp_edge")), new WaitCommand(1.5), //wait for ramp to lower, TODO: needs tuning -- lower it as much as you can getAutonomousCommand(getTrajectory("onto_ramp")) @@ -446,34 +456,35 @@ public Command twoPieceBalanceAuton() { // shoot cone, grab cube, shoot cube, do public Command hallwayPracticeAuton() { // shoot cone, grab cube, shoot cube, doesn't balance // var onArrivalCommand = getAutonomousCommand(getTrajectory("straight_forward")); // go to pickup location - var onArrivalCommand = new SequentialCommandGroup( - getAutonomousCommand(getTrajectory("straight_forward")), // go to pickup location - getAutonomousCommand(getTrajectory("straight_backward")), // return to the next placement location - getAutonomousCommand(getTrajectory("straight_forward")) - ); // var onArrivalCommand = new SequentialCommandGroup( - // new ParallelDeadlineGroup( // then, in parallel - // (new WaitCommand(1)).andThen(getAutonomousCommand(getTrajectory("straight_forward"))) // go to pickup location - // //, new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posIntakeFloor(() -> RobotMap.Component.intake.c_holdVoltage(-8).withTimeout(5).andThen(RobotMap.Component.intake.c_holdItem())))// start intaking when we get close TODO: tune 2.5 sec wait (should be ~.5sec less than time to run spline above, but aim low to be safe) - // //, new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posIntakeFloor(() -> RobotMap.Component.intake.c_holdVoltage(-8.)))// start intaking when we get close TODO: tune 2.5 sec wait (should be ~.5sec less than time to run spline above, but aim low to be safe) - // ), - // new ParallelDeadlineGroup( // then, as a separated parallel schedule, - // getAutonomousCommand(getTrajectory("straight_backward")) // return to the next placement location - // // , new TriggerCommandFactory(() -> RobotMap.Component.intake.c_holdItem()) // hold the game piece in - // ), - // // new TriggerCommandFactory(() -> RobotMap.Component.arm.c_shootCubes(5, () -> getAutonomousCommand(getTrajectory("straight_forward")))) // finally, shoot the cube we just picked up and stow - // getAutonomousCommand(getTrajectory("straight_forward")) - // ); + // getAutonomousCommand(getTrajectory("straight_forward")), // go to pickup location + // getAutonomousCommand(getTrajectory("straight_backward"))//, // return to the next placement location + // // getAutonomousCommand(getTrajectory("straight_forward")) + // ); + // SmartDashboard.putString("funnyboi", getTrajectory("straight_backward").getInitialPose().toString()); + var onArrivalCommand = new SequentialCommandGroup( + new ParallelDeadlineGroup( // then, in parallel + (new WaitCommand(1)).andThen(getAutonomousCommand(getTrajectory("straight_forward"))) // go to pickup location + //, new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posIntakeFloor(() -> RobotMap.Component.intake.c_holdVoltage(-8).withTimeout(5).andThen(RobotMap.Component.intake.c_holdItem())))// start intaking when we get close TODO: tune 2.5 sec wait (should be ~.5sec less than time to run spline above, but aim low to be safe) + , new TriggerCommandFactory(() -> RobotMap.Component.arm.c_posIntakeFloor(() -> RobotMap.Component.intake.c_holdVoltage(-8.)))// start intaking when we get close TODO: tune 2.5 sec wait (should be ~.5sec less than time to run spline above, but aim low to be safe) + ), + new ParallelDeadlineGroup( // then, as a separated parallel schedule, + getAutonomousCommand(getTrajectory("straight_backward")) // return to the next placement location + , new TriggerCommandFactory(() -> RobotMap.Component.intake.c_holdItem()) // hold the game piece in + ), + new TriggerCommandFactory(() -> RobotMap.Component.arm.c_shootCubes(5, () -> getAutonomousCommand(getTrajectory("straight_forward")))) // finally, shoot the cube we just picked up and stow + // getAutonomousCommand(getTrajectory("straight_forward")) + ); // var cmd = RobotMap.Component.arm.c_shootCubes(4, () -> onArrivalCommand); var cmd = RobotMap.Component.arm.c_shootCubes(4); - SmartDashboard.putString("auton reqs", onArrivalCommand.getRequirements().toString()); + // SmartDashboard.putString("auton reqs", onArrivalCommand.getRequirements().toString()); var total_parallel = new ParallelCommandGroup( new TriggerCommandFactory(() -> cmd), new TriggerCommandFactory(() -> (new WaitCommand(2.5)).andThen(onArrivalCommand)) ); // return cmd; - return onArrivalCommand; + return total_parallel; } } From deddc88cbcc663017eb12b779c4bef2a10297695 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Wed, 5 Apr 2023 18:55:30 -0700 Subject: [PATCH 59/63] ################### pre packing for SVR, partial working autons and good tele From 40cec1f82ef3ca873f11474607509141762c55e9 Mon Sep 17 00:00:00 2001 From: jac0rr Date: Fri, 5 May 2023 16:12:47 -0700 Subject: [PATCH 60/63] implemented new joystick (untested) --- src/main/java/org/usfirst/frc4904/robot/Robot.java | 4 ++-- src/main/java/org/usfirst/frc4904/robot/RobotMap.java | 9 ++++++--- .../frc4904/robot/humaninterface/drivers/NathanGain.java | 9 ++++----- 3 files changed, 12 insertions(+), 10 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 55a9d6e..12f6210 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -95,8 +95,8 @@ public void teleopInitialize() { public void teleopExecute() { SmartDashboard.putBoolean("isFlipped - IMPORTANT", NathanGain.isFlippy); SmartDashboard.putNumber("gyroooo", RobotMap.Component.navx.getAngle()); - SmartDashboard.putNumber("driverjoystick x", RobotMap.HumanInput.Driver.drivingJoystick.getX()); - SmartDashboard.putNumber("driverjoystick y", RobotMap.HumanInput.Driver.drivingJoystick.getY()); + SmartDashboard.putNumber("driverjoystick speed", RobotMap.HumanInput.Driver.throttleJoystick.getY()); + SmartDashboard.putNumber("driverjoystick turn", RobotMap.HumanInput.Driver.turnJoystick.getX()); } @Override diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index a3b2a6a..845e6c9 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -41,7 +41,8 @@ public static class Port { public static class HumanInput { public static final int joystick = 0; public static final int xboxController = 1; - public static final int drivingJoystick = 2; + public static final int throttleJoystick = 2; + public static final int turnJoystick = 3; } // // blinky constants @@ -151,7 +152,8 @@ public static class Input { public static class HumanInput { public static class Driver { public static CustomCommandXbox xbox; - public static CustomCommandJoystick drivingJoystick; + public static CustomCommandJoystick throttleJoystick; + public static CustomCommandJoystick turnJoystick; } public static class Operator { @@ -164,7 +166,8 @@ public RobotMap() { HumanInput.Driver.xbox = new CustomCommandXbox(Port.HumanInput.xboxController, 0.1); HumanInput.Operator.joystick = new CustomCommandJoystick(Port.HumanInput.joystick, 0.1); - HumanInput.Driver.drivingJoystick = new CustomCommandJoystick(Port.HumanInput.drivingJoystick, 0.1); + HumanInput.Driver.throttleJoystick = new CustomCommandJoystick(Port.HumanInput.throttleJoystick, 0.1); + HumanInput.Driver.turnJoystick = new CustomCommandJoystick(Port.HumanInput.turnJoystick, 0.1); // // UDP things // try { // Component.robotUDP = new RobotUDP(Port.Network.LOCAL_SOCKET_ADDRESS, Port.Network.LOCALIZATION_ADDRESS); diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java index 7a6ab8a..8a30f2c 100644 --- a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java @@ -36,13 +36,13 @@ protected double scaleGain(double input, double gain, double exp) { @Override public void bindCommands() { - RobotMap.HumanInput.Driver.xbox.leftBumper().onTrue(new InstantCommand( + RobotMap.HumanInput.Driver.throttleJoystick.button1.onTrue(new InstantCommand( () -> { NathanGain.precisionScaleY = PRECISE_SPEED_SCALE; NathanGain.precisionScaleTurn = PRECISE_TURN_SCALE; } )); - RobotMap.HumanInput.Driver.xbox.leftBumper().onFalse(new InstantCommand(( + RobotMap.HumanInput.Driver.throttleJoystick.button1.onFalse(new InstantCommand(( ) -> { NathanGain.precisionScaleY = NORMAL_SPEED_GAIN; NathanGain.precisionScaleTurn = NORMAL_TURN_GAIN; @@ -51,7 +51,6 @@ public void bindCommands() { RobotMap.HumanInput.Driver.xbox.y().onTrue(new InstantCommand(() -> NathanGain.precisionScaleY = 1)); RobotMap.HumanInput.Driver.xbox.y().onFalse(new InstantCommand(() -> NathanGain.precisionScaleY = NORMAL_SPEED_GAIN)); - RobotMap.HumanInput.Driver.drivingJoystick.button1.onTrue(new InstantCommand(() -> System.out.println("\n\n\n\n\n\nButton 1 pressed\n\n\n\n\n\n\n"))); } @Override @@ -61,7 +60,7 @@ public double getX() { @Override public double getY() { - double rawSpeed = RobotMap.HumanInput.Driver.xbox.getRightTriggerAxis() - RobotMap.HumanInput.Driver.xbox.getLeftTriggerAxis(); + double rawSpeed = RobotMap.HumanInput.Driver.throttleJoystick.getY(); double speed; speed = scaleGain(rawSpeed, NathanGain.precisionScaleY, NathanGain.SPEED_EXP) * NathanGain.Y_SPEED_SCALE; @@ -76,7 +75,7 @@ public double getY() { @Override public double getTurnSpeed() { - double rawTurnSpeed = RobotMap.HumanInput.Driver.xbox.getLeftX(); + double rawTurnSpeed = RobotMap.HumanInput.Driver.turnJoystick.getX(); double turnSpeed = scaleGain(rawTurnSpeed, NathanGain.precisionScaleTurn, NathanGain.TURN_EXP) * NathanGain.TURN_SPEED_SCALE; // double precisionTurnSpeed = scaleGain(RobotMap.HumanInput.Driver.xbox.getRightX(), 0.08, 1.2); // double operatorControlTurnSpeed = scaleGain(RobotMap.HumanInput.Operator.joystick.getAxis(0), 0.2, 1.5); From fa4f6966dae2c7f2e3cb6756ed48a0fd672dbaaa Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Fri, 5 May 2023 16:29:05 -0700 Subject: [PATCH 61/63] . --- .../robot/humaninterface/drivers/NathanGain.java | 15 +++++++++++++-- src/main/java/org/usfirst/frc4904/standard | 2 +- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java index 8a30f2c..6b5352e 100644 --- a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java @@ -49,8 +49,19 @@ public void bindCommands() { } )); - RobotMap.HumanInput.Driver.xbox.y().onTrue(new InstantCommand(() -> NathanGain.precisionScaleY = 1)); - RobotMap.HumanInput.Driver.xbox.y().onFalse(new InstantCommand(() -> NathanGain.precisionScaleY = NORMAL_SPEED_GAIN)); + RobotMap.HumanInput.Driver.throttleJoystick.button1.onTrue(new InstantCommand(() -> NathanGain.precisionScaleY = 1)); + RobotMap.HumanInput.Driver.throttleJoystick.button1.onFalse(new InstantCommand(() -> NathanGain.precisionScaleY = NORMAL_SPEED_GAIN)); + + RobotMap.HumanInput.Driver.throttleJoystick.button2.onTrue(new InstantCommand(() -> NathanGain.precisionScaleY = 1)); + RobotMap.HumanInput.Driver.throttleJoystick.button2.onFalse(new InstantCommand(() -> NathanGain.precisionScaleY = NORMAL_SPEED_GAIN)); + + RobotMap.HumanInput.Driver.turnJoystick.button1.onTrue(new InstantCommand(() -> NathanGain.precisionScaleY = 1)); + RobotMap.HumanInput.Driver.turnJoystick.button1.onFalse(new InstantCommand(() -> NathanGain.precisionScaleY = NORMAL_SPEED_GAIN)); + + RobotMap.HumanInput.Driver.turnJoystick.button2.onTrue(new InstantCommand(() -> NathanGain.precisionScaleY = 1)); + RobotMap.HumanInput.Driver.turnJoystick.button1.onFalse(new InstantCommand(() -> NathanGain.precisionScaleY = NORMAL_SPEED_GAIN)); + + } @Override diff --git a/src/main/java/org/usfirst/frc4904/standard b/src/main/java/org/usfirst/frc4904/standard index 6984d40..109068d 160000 --- a/src/main/java/org/usfirst/frc4904/standard +++ b/src/main/java/org/usfirst/frc4904/standard @@ -1 +1 @@ -Subproject commit 6984d402dc16e4d48c9e783d946d4a77e4576c37 +Subproject commit 109068d9539104d0a255b81e0f1f6efb0e6f5f2b From d0b74a501cb7045f6822c17dfda56c435375640c Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Mon, 2 Oct 2023 16:56:19 -0700 Subject: [PATCH 62/63] cube shooting + lerped arm pivot (again) --- src/main/java/org/usfirst/frc4904/robot/Robot.java | 2 ++ .../robot/humaninterface/operators/DefaultOperator.java | 6 ++++-- .../frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java | 2 +- .../frc4904/robot/subsystems/arm/ArmPivotSubsystem.java | 2 +- src/main/java/org/usfirst/frc4904/standard | 2 +- 5 files changed, 9 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index bcf630c..d6c88cd 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -229,6 +229,8 @@ public void testExecute() { public void alwaysExecute() { SmartDashboard.putNumber("Arm angle", RobotMap.Component.arm.armPivotSubsystem.getCurrentAngleDegrees()); SmartDashboard.putNumber("arm extension length", RobotMap.Component.arm.armExtensionSubsystem.getCurrentExtensionLength()); + SmartDashboard.putNumber("intake speed", RobotMap.HumanInput.Operator.joystick.getAxis(3)); + // SmartDashboard.putNumber("gyroooo", RobotMap.Component.navx.getAngle()); } diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java index 587edb1..f782ec4 100644 --- a/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/operators/DefaultOperator.java @@ -5,6 +5,8 @@ import org.usfirst.frc4904.standard.commands.TriggerCommandFactory; import org.usfirst.frc4904.standard.humaninput.Operator; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.InstantCommand; public class DefaultOperator extends Operator { @@ -28,8 +30,8 @@ public void bindCommands() { // Intake // FIXME: use nameCommand to make it cleaner with expresions (no varibales) - var zeroIntake = RobotMap.Component.intake.c_holdVoltage(0); - var runOuttake = RobotMap.Component.intake.c_holdVoltage(3); + var zeroIntake = RobotMap.Component.intake.c_holdVoltage(9); + var runOuttake = RobotMap.Component.intake.c_holdVoltage(joystick.getAxis(3)); // intake joystick.button2.onTrue(RobotMap.Component.intake.c_startIntake()); diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java index 35055ad..78e81ea 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java @@ -63,7 +63,7 @@ public void initializeEncoderPositions(double meters) { } public double getCurrentExtensionLength() { - return revsToExtensionLength(motor.getSensorPositionRotations()) * 0.968 - 0.0853; + return revsToExtensionLength(motor.getSensorPositionRotations()); } public void setVoltageSafely(double voltage) { diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java index 3e23a5a..dec9ef2 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java @@ -83,7 +83,7 @@ public ArmPivotSubsystem(TalonMotorSubsystem armMotorGroup, DoubleSupplier exten public double getCurrentAngleDegrees() { // return slackyEncoder.getRealPosition(); - return motorRevsToAngle(armMotorGroup.getSensorPositionRotations()) * 0.911 - 6.3; + return motorRevsToAngle(armMotorGroup.getSensorPositionRotations())*0.985 - 5.18; // * 0.911 - 6.3 } diff --git a/src/main/java/org/usfirst/frc4904/standard b/src/main/java/org/usfirst/frc4904/standard index 109068d..5492244 160000 --- a/src/main/java/org/usfirst/frc4904/standard +++ b/src/main/java/org/usfirst/frc4904/standard @@ -1 +1 @@ -Subproject commit 109068d9539104d0a255b81e0f1f6efb0e6f5f2b +Subproject commit 54922444352baeef934533ff5f45bc07bed78500 From 2fe422972557cee63d6194c9fdc8b92852b8d898 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Thu, 5 Oct 2023 16:47:08 -0700 Subject: [PATCH 63/63] general calgames prep -- auton and minor tuning --- src/main/java/org/usfirst/frc4904/robot/Robot.java | 3 ++- .../robot/humaninterface/drivers/NathanGain.java | 4 ++-- .../frc4904/robot/seenoevil/RobotContainer2.java | 12 ++++++------ .../frc4904/robot/subsystems/arm/ArmSubsystem.java | 5 +++++ 4 files changed, 15 insertions(+), 9 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index d6c88cd..1c6589d 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -158,7 +158,8 @@ public void autonomousInitialize() { // SATURDAY MORNING TEST: is the cube shooter auton gonna work // var commnand = donttouchme.balanceAutonAndShootCube(donttouchme.m_robotDrive::getWheelSpeeds, donttouchme.m_robotDrive::tankDriveVolts); // var commnand = donttouchme.balanceAutonAndShootCube(donttouchme.m_robotDrive::getWheelSpeeds, donttouchme.m_robotDrive::tankDriveVolts); - var commnand = donttouchme.twoPieceBalanceAuton(); + + var commnand = donttouchme.simpleAuton(); //for calgames; just place a cone and leave the zone commnand.schedule(); } diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java index 61588e8..bf577f2 100644 --- a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java @@ -91,7 +91,7 @@ public double getTurnSpeed() { // double precisionTurnSpeed = scaleGain(RobotMap.HumanInput.Driver.xbox.getRightX(), 0.08, 1.2); // double operatorControlTurnSpeed = scaleGain(RobotMap.HumanInput.Operator.joystick.getAxis(0), 0.2, 1.5); - if (NathanGain.isFlippy) return (turnSpeed) * -1; - return turnSpeed;// + precisionTurnSpeed; + if (NathanGain.isFlippy) return (turnSpeed); + return turnSpeed*-1;// + precisionTurnSpeed; //is mounted inverted so we need to revert by default } } diff --git a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java index 0e6bfb7..bcfe553 100644 --- a/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java +++ b/src/main/java/org/usfirst/frc4904/robot/seenoevil/RobotContainer2.java @@ -435,9 +435,10 @@ public Command twoPieceAuton() { // shoot cone, grab cube, shoot cube, doesn't b )); return cmd; } - public Command twoPieceBalanceAuton() { // shoot cone, grab cube, shoot cube, doesn't balance - var cmd = RobotMap.Component.arm.c_shootCones(4, // shoot cone + public Command simpleAuton() { // this is JUST placing a cube and exiting the zone, no complicated stuff we cant do + var cmd = RobotMap.Component.arm.c_holdRotation(135, () -> new SequentialCommandGroup( + RobotMap.Component.intake.c_holdVoltage(9).withTimeout(0.5), // new ParallelCommandGroup( // then, in parallel // getAutonomousCommand(getTrajectory("go_to_pickup_next")), // go to pickup location // (new WaitCommand(2.5)).andThen(RobotMap.Component.intake.c_holdVoltage(-8)) // start intaking when we get close TODO: tune 2.5 sec wait (should be ~.5sec less than time to run spline above, but aim low to be safe) @@ -447,10 +448,9 @@ public Command twoPieceBalanceAuton() { // shoot cone, grab cube, shoot cube, do // () -> getAutonomousCommand(getTrajectory("from_pickup_to_place")) // return to the next placement location // ), // RobotMap.Component.arm.c_shootCubes(4), // finally, shoot the cube we just picked up and stow - getAutonomousCommand(getTrajectory("from_cube_place_to_ramp_edge")), - new WaitCommand(1.5), //wait for ramp to lower, TODO: needs tuning -- lower it as much as you can - getAutonomousCommand(getTrajectory("onto_ramp")) - )); + getAutonomousCommand(getTrajectory("past_ramp")) + + )); return cmd; } diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index 58576a3..96df8ab 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -150,6 +150,11 @@ public Command c_angleCubes(int shelf) { ); } + public Command c_holdRotation(double degreesFromHorizontal, Supplier onArrivalCommandDealer) { + return armPivotSubsystem.c_holdRotation(degreesFromHorizontal, MAX_VELOCITY_PIVOT, MAX_ACCEL_PIVOT, onArrivalCommandDealer); + } + + public Command c_holdArmPose(Pair emacs) { return c_holdArmPose(emacs.getFirst(), emacs.getSecond()); }