Team4388/RiseOfRidgebotics2020
1
0
Fork 0
mirror of https://github.com/Team4388/RiseOfRidgebotics2020.git synced 2026-06-09 08:48:01 -06:00
Code Issues Packages Projects Releases Wiki Activity

Gettign trimming to work (this may not work)

Browse Source
This commit is contained in:
ryan123rudder
2020-02-29 23:45:07 -07:00
parent eb129c6b34
commit 272951ed67
7 changed files with 122 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+13 -8
View File
@@ -59,6 +59,7 @@ import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.LED;
import frc4388.robot.commands.TrackTarget;
import frc4388.robot.commands.TrimShooter;
import frc4388.robot.commands.StorageOutake;
import frc4388.robot.commands.StoragePrepAim;
import frc4388.robot.commands.StoragePrepIntake;
@@ -97,6 +98,7 @@ public class RobotContainer {
private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
@@ -174,17 +176,12 @@ public class RobotContainer {
// safety for climber and leveler
new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)
//.whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber))
//.whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber));
.whileHeld(new RunCommand(() -> SmartDashboard.putNumber("Angle Current", m_robotShooter.m_angleAdjustMotor.getOutputCurrent())))
.whileHeld(new RunCommand(() -> SmartDashboard.putNumber("Rotate Current", m_robotShooterAim.m_shooterRotateMotor.getOutputCurrent())));
.whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber))
.whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber));
// starts tracking target
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
.whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage))
//.whileHeld(new ParallelCommandGroup(
//new HoldTarget(m_robotShooter,m_robotShooterAim),
//new HoodPositionPID(m_robotShooter)))
.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage));
@@ -196,16 +193,24 @@ public class RobotContainer {
new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS)
.whileHeld(new StorageOutake(m_robotStorage));
//TEST FOR HOOD
new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
.whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0.3)))
.whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0)));
//TEST FOR HOOD
new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
.whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(-0.3)))
.whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0)));
//Trims shooter
new JoystickButton(getOperatorJoystick(), XboxController.TOP_BOTTOM_DPAD_AXIS)
.whenPressed(new TrimShooter(m_robotShooter));
//Calibrates turret and hood
new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON)
.whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim));
}
/**
@@ -323,4 +328,4 @@ public class RobotContainer {
{
return m_driverXbox.getJoyStick();
}
}
}
src/main/java/frc4388/robot/Trims.java
+18
View File
@@ -0,0 +1,18 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot;
public class Trims{
public double m_turretTrim;
public double m_hoodTrim;
public Trims(double turretTrim, double hoodTrim){
m_turretTrim = turretTrim;
m_hoodTrim = hoodTrim;
}
}
src/main/java/frc4388/robot/commands/HoldTarget.java
+7 -4
View File
@@ -32,8 +32,11 @@ public class HoldTarget extends CommandBase {
double yAngle = 0;
double target = 0;
public double distance;
public static double fireVel;
public static double fireAngle;
public double fireVel;
public double fireAngle;
public double m_hoodTrim;
public double m_turretTrim;
/**
* Uses the Limelight to track the target
@@ -69,7 +72,7 @@ public class HoldTarget extends CommandBase {
//Deadzones
else if(turnAmount > 0 && turnAmount < VisionConstants.MOTOR_DEAD_ZONE){turnAmount = VisionConstants.MOTOR_DEAD_ZONE;}
else if(turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE){turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;}
m_shooterAim.runShooterWithInput(-turnAmount);
m_shooterAim.runShooterWithInput(-turnAmount - m_shooter.shooterTrims.m_turretTrim);
//Finding Distance
distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180));
@@ -81,7 +84,7 @@ public class HoldTarget extends CommandBase {
fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2)));
fireAngle = Math.atan(yVel/xVel) * (180/Math.PI);
m_shooter.m_fireVel = fireVel;
m_shooter.m_fireAngle = fireAngle;
m_shooter.m_fireAngle = fireAngle + m_shooter.shooterTrims.m_hoodTrim;
}/*
else{
src/main/java/frc4388/robot/commands/TrackTarget.java
+9 -2
View File
@@ -14,12 +14,14 @@ import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.LimeLight;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.commands.TrimShooter;
import frc4388.utility.controller.IHandController;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class TrackTarget extends CommandBase {
@@ -37,6 +39,9 @@ public class TrackTarget extends CommandBase {
public static double fireVel;
public static double fireAngle;
public double m_hoodTrim;
public double m_turretTrim;
/**
* Uses the Limelight to track the target
*/
@@ -74,7 +79,7 @@ public class TrackTarget extends CommandBase {
} else if (turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE) {
turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;
}
m_shooterAim.runShooterWithInput(-turnAmount);
m_shooterAim.runShooterWithInput(-turnAmount - m_shooter.shooterTrims.m_turretTrim);
// Finding Distance
distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
@@ -85,8 +90,10 @@ public class TrackTarget extends CommandBase {
fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2)));
fireAngle = Math.atan(yVel/xVel) * (180/Math.PI);
m_shooter.m_fireVel = fireVel;
m_shooter.m_fireAngle = fireAngle;
m_shooter.m_fireAngle = fireAngle + m_shooter.shooterTrims.m_hoodTrim;
}
}
src/main/java/frc4388/robot/commands/TrimShooter.java
+68
View File
@@ -0,0 +1,68 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
import edu.wpi.first.wpilibj.Joystick;
import frc4388.utility.controller.XboxController;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.RobotContainer;
import frc4388.robot.Trims;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
public class TrimShooter extends CommandBase {
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
public double turretTrim = 0;
public double hoodTrim = 0;
public Shooter m_shooter;
/**
* Creates a new TrimShooter.
*/
public TrimShooter(Shooter shootSub) {
m_shooter = shootSub;
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(m_operatorXbox.getDPadTop()){
hoodTrim += 1;
}
else if(m_operatorXbox.getDPadBottom()){
hoodTrim -= 1;
}
else if(m_operatorXbox.getDPadRight()){
turretTrim += 1;
}
else if(m_operatorXbox.getDPadLeft()){
turretTrim -= 1;
}
m_shooter.shooterTrims.m_turretTrim = turretTrim;
m_shooter.shooterTrims.m_hoodTrim = hoodTrim;
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/subsystems/Shooter.java
+6
View File
@@ -26,6 +26,7 @@ import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Gains;
import frc4388.robot.Trims;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.utility.ShooterTables;
import frc4388.utility.controller.IHandController;
@@ -52,6 +53,8 @@ public class Shooter extends SubsystemBase {
public double m_fireVel;
public double m_fireAngle;
CANDigitalInput m_hoodUpLimit, m_hoodDownLimit;
public Trims shooterTrims;
/*
* Creates a new Shooter subsystem.
@@ -59,6 +62,8 @@ public class Shooter extends SubsystemBase {
public Shooter() {
//Testing purposes reseting gyros
//resetGyroAngleAdj();
shooterTrims = new Trims(0, 0);
m_shooterFalcon.configFactoryDefault();
m_shooterFalcon.setNeutralMode(NeutralMode.Coast);
m_shooterFalcon.setInverted(true);
@@ -97,6 +102,7 @@ public class Shooter extends SubsystemBase {
m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, 33);
m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, 3);
}
@Override
src/main/java/frc4388/robot/subsystems/ShooterAim.java
+1
View File
@@ -32,6 +32,7 @@ public class ShooterAim extends SubsystemBase {
CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController();
public CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder();
/**
* Creates a new ShooterAim.
*/