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

Merge branch 'master' into button

Browse Source
This commit is contained in:
Keenan D. Buckley
2020-02-22 00:03:14 +00:00
committed by GitHub
parent e7e91d9c4c 73e2d0fd59
commit 37a5035de5
10 changed files with 714 additions and 262 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java
+1 -1
View File
@@ -37,7 +37,7 @@ public class DriveStraightAtVelocityPID extends CommandBase {
@Override
public void execute() {
//System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
m_drive.runDriveStraightVelocityPID(-m_targetVel, m_targetGyro);
m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro);
}
// Called once the command ends or is interrupted.
src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
+1 -1
View File
@@ -47,7 +47,7 @@ public class DriveStraightToPositionPID extends CommandBase {
//System.err.println("| \n Sensor Pos \n" + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY));
//System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY));
//System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY));
m_drive.runDriveStraightPositionPID(m_targetPosOut, m_targetGyro);
m_drive.runDrivePositionPID(m_targetPosOut, m_targetGyro);
}
// Called once the command ends or is interrupted.
src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+1 -1
View File
@@ -38,7 +38,7 @@ public class DriveWithJoystick extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double moveInput = m_controller.getLeftYAxis();
double moveInput = -m_controller.getLeftYAxis();
double steerInput = m_controller.getRightXAxis();
double moveOutput = 0;
double steerOutput = 0;
src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java
+118
View File
@@ -0,0 +1,118 @@
/*----------------------------------------------------------------------------*/
/* 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.wpilibj2.command.CommandBase;
import edu.wpi.first.wpiutil.math.MathUtil;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
import frc4388.utility.controller.IHandController;
public class DriveWithJoystickDriveStraight extends CommandBase {
Drive m_drive;
double m_targetGyro, m_currentGyro;
double m_stopPos;
long m_currTime, m_deltaTime;
long m_deadTimeSteer, m_deadTimeMove;
long m_deadTimeout = 100;
IHandController m_controller;
/**
* Creates a new DriveWithJoystickDriveStraight to control the drivetrain with an Xbox controller.
* Applies a curved ramp to the input from the controllers to make the robot less "touchy".
* Also uses PIDs to keep the robot on course when given a "dead" or 0 input.
* @param subsystem pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
* @param controller pass the Driver {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the
* {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in
* {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
*/
public DriveWithJoystickDriveStraight(Drive subsystem, IHandController controller) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_controller = controller;
addRequirements(m_drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_currTime = System.currentTimeMillis();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(1);
double moveInput = -m_controller.getLeftYAxis();
double steerInput = m_controller.getRightXAxis();
double moveOutput = 0;
m_deltaTime = System.currentTimeMillis() - m_currTime;
m_currTime = System.currentTimeMillis();
/* If steer stick is being used */
if (steerInput != 0) {
m_deadTimeSteer = m_currTime;
}
/* Curves the moveInput to be slightly more gradual at first */
if (moveInput >= 0) {
moveOutput = -Math.cos(1.571*moveInput)+1;
} else {
moveOutput = Math.cos(1.571*moveInput)-1;
}
/* If steer stick has not been used for less than 1 sec */
if (m_currTime - m_deadTimeSteer < m_deadTimeout) {
runDriveWithInput(moveOutput, steerInput);
resetGyroTarget();
}
/* If steer stick has not been used for 1 sec */
else {
runDriveStraight(moveOutput);
}
}
private void runDriveWithInput(double move, double steer) {
double cosMultiplier = .45;
double steerOutput = 0;
double deadzone = .2;
/* Curves the steer output to be similarily gradual */
if (steer > 0){
steerOutput = -cosMultiplier*Math.cos(1.571*steer)+(cosMultiplier+deadzone);
} else {
steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone);
}
m_drive.driveWithInput(move, steerOutput);
System.out.println("Driving With Input");
}
private void runDriveStraight(double move) {
m_drive.driveWithInputAux(move * 3/4, m_targetGyro);
System.out.println("Driving Straight with Target: " + m_targetGyro);
}
/**
* set target angle to current angle (prevents buildup of gyro error).
*/
private void resetGyroTarget() {
//m_targetGyro = m_currentGyro;
m_targetGyro = m_currentGyro
+ m_drive.getTurnRate();
}
// 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/commands/DriveWithJoystickUsingDeadAssistPID.java
+74 -41
View File
@@ -15,10 +15,12 @@ import frc4388.utility.controller.IHandController;
public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
Drive m_drive;
double m_targetGyro;
long lastTime;
double m_targetGyro, m_currentGyro;
double m_stopPos;
long m_currTime, m_deltaTime;
long m_deadTimeSteer, m_deadTimeMove;
long m_deadTimeout = 100;
IHandController m_controller;
boolean isAuxPIDEnabled = false;
/**
* Creates a new DriveWithJoystickUsingDeadAssistPID to control the drivetrain with an Xbox controller.
@@ -30,7 +32,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
* {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
*/
public DriveWithJoystickUsingDeadAssistPID(Drive subsystem, IHandController controller) {
// Use addRequirements() here to declare subsystem dependencies.
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_controller = controller;
addRequirements(m_drive);
@@ -39,35 +41,32 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
// Called when the command is initially scheduled.
@Override
public void initialize() {
lastTime = System.currentTimeMillis();
m_currTime = System.currentTimeMillis();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
double moveInput = m_controller.getLeftYAxis();
m_currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
double moveInput = -m_controller.getLeftYAxis();
double steerInput = m_controller.getRightXAxis();
double moveOutput = 0;
double steerOutput = 0;
long deltaTime = System.currentTimeMillis() - lastTime;
lastTime = System.currentTimeMillis();
/* If AuxPID is enabled, then update using the steer input */
if (isAuxPIDEnabled) {
m_targetGyro += 2 * steerInput * deltaTime;
m_targetGyro = MathUtil.clamp( m_targetGyro,
currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/4),
currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/4));
}
/* Otherwise set target angle to current angle (prevents buildup of gyro error) */
else {
m_targetGyro = currentGyro;
}
m_deltaTime = System.currentTimeMillis() - m_currTime;
m_currTime = System.currentTimeMillis();
/* If move stick is being used */
if (moveInput != 0) {
m_deadTimeMove = m_currTime;
m_stopPos = m_drive.m_rightFrontMotor.getSelectedSensorPosition()
+ (m_drive.m_rightFrontMotor.getSelectedSensorVelocity());
}
/* If steer stick is being used */
if (steerInput != 0) {
m_deadTimeSteer = m_currTime;
}
/* If move stick has been pressed within 1 sec */
if (m_currTime - m_deadTimeMove < m_deadTimeout) {
/* Curves the moveInput to be slightly more gradual at first */
if (moveInput >= 0) {
moveOutput = -Math.cos(1.571*moveInput)+1;
@@ -75,32 +74,66 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
moveOutput = Math.cos(1.571*moveInput)-1;
}
/* If steer stick is being used. */
if (steerInput != 0) {
double cosMultiplier = .45;
double deadzone = .2;
/* Curves the steer output to be similarily gradual */
if (steerInput > 0){
steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone);
} else {
steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone);
}
m_drive.driveWithInput(moveOutput, steerOutput);
isAuxPIDEnabled = false;
/* If steer stick has not been used for less than 1 sec */
if (m_currTime - m_deadTimeSteer < m_deadTimeout) {
runDriveWithInput(moveOutput, steerInput);
resetGyroTarget();
}
/* If only the move stick is being used */
/* If steer stick has not been used for 1 sec */
else {
m_drive.driveWithInputAux(moveOutput, m_targetGyro);
isAuxPIDEnabled = true;
runDriveStraight(moveOutput);
}
}
/* If the move stick is not being used */
/* If the move stick has not been used for 1 sec */
else {
m_drive.runDriveStraightVelocityPID(0, m_targetGyro);
isAuxPIDEnabled = true;
runStoppedTurn(steerInput);
}
}
private void runDriveWithInput(double move, double steer) {
double cosMultiplier = .45;
double steerOutput = 0;
double deadzone = .2;
/* Curves the steer output to be similarily gradual */
if (steer > 0){
steerOutput = -cosMultiplier*Math.cos(1.571*steer)+(cosMultiplier+deadzone);
} else {
steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone);
}
m_drive.driveWithInput(move, steerOutput);
System.out.println("Driving With Input");
}
private void runDriveStraight(double move) {
m_drive.driveWithInputAux(move * 3/4, m_targetGyro);
System.out.println("Driving Straight with Target: " + m_targetGyro);
}
private void runStoppedTurn(double steer) {
updateGyroTarget(steer);
m_drive.runDrivePositionPID(m_stopPos, m_targetGyro);
System.out.println("Turning with Target: " + m_targetGyro);
}
/**
* If AuxPID is enabled, then update using the steer input
*/
private void updateGyroTarget(double steerInput) {
m_targetGyro -= 5 * steerInput * m_deltaTime;
m_targetGyro = MathUtil.clamp( m_targetGyro,
m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/8),
m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/8));
}
/**
* set target angle to current angle (prevents buildup of gyro error).
*/
private void resetGyroTarget() {
m_targetGyro = m_currentGyro;
m_targetGyro = m_currentGyro
+ m_drive.getTurnRate();
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {