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

rework Deadassist to enable after a timeout

Browse Source 
This keeps it out of the hair of the drivers while also possibly being useful
This commit is contained in:
Keenan D. Buckley
2020-02-17 18:25:48 -07:00
parent 3cdeec01eb
commit b729d04db4
3 changed files with 54 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+1 -1
View File
@@ -78,7 +78,7 @@ public class RobotContainer {
/* Default Commands */
// drives the robot with a two-axis input from the driver controller
m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController()));
m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, getDriverController()));
// drives intake with input from triggers on the opperator controller
m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController()));
// drives climber with input from triggers on the opperator controller
src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
+49 -26
View File
@@ -16,7 +16,9 @@ import frc4388.utility.controller.IHandController;
public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
Drive m_drive;
double m_targetGyro, m_currentGyro;
long m_lastTime, m_deltaTime;
long m_currTime, m_deltaTime;
long m_deadTimeSteer, m_deadTimeMove;
long m_deadTimeout = 500;
IHandController m_controller;
/**
@@ -38,7 +40,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_lastTime = System.currentTimeMillis();
m_currTime = System.currentTimeMillis();
}
// Called every time the scheduler runs while the command is scheduled.
@@ -48,12 +50,20 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
double moveInput = -m_controller.getLeftYAxis();
double steerInput = m_controller.getRightXAxis();
double moveOutput = 0;
double steerOutput = 0;
m_deltaTime = System.currentTimeMillis() - m_lastTime;
m_lastTime = System.currentTimeMillis();
m_deltaTime = System.currentTimeMillis() - m_currTime;
m_currTime = System.currentTimeMillis();
/* If move stick is being used */
if (moveInput != 0) {
m_deadTimeMove = m_currTime;
}
/* If steer stick is being used */
if (steerInput != 0) {
m_deadTimeSteer = m_currTime;
}
/* If move stick has not been pressed for 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;
@@ -61,35 +71,48 @@ 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);
}
resetGyroTarget();
m_drive.driveWithInput(moveOutput, steerOutput);
System.out.println("Driving With Input");
/* If steer stick has not been used for less than 1 sec */
if (m_currTime - m_deadTimeSteer < m_deadTimeout) {
runDriveWithInput(moveOutput, steerInput);
m_deadTimeSteer = m_currTime;
}
/* If only the move stick is being used */
/* If steer stick has not been used for 1 sec */
else {
updateGyroTarget(steerInput);
m_drive.driveWithInputAux(moveOutput, m_targetGyro);
System.out.println("Driving with Input Aux with Target: " + m_targetGyro);
runDriveStraight(moveOutput);
}
}
/* If the move stick is not being used */
/* If the move stick has not been used for 1 sec */
else {
updateGyroTarget(steerInput);
m_drive.runDriveStraightVelocityPID(0, m_targetGyro);
System.out.println("Driving with Velocity PID with Target: " + m_targetGyro);
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);
}
resetGyroTarget();
m_drive.driveWithInput(move, steerOutput);
System.out.println("Driving With Input");
}
private void runDriveStraight(double move) {
m_drive.driveWithInputAux(move, m_targetGyro);
System.out.println("Driving with Input Aux with Target: " + m_targetGyro);
}
private void runStoppedTurn(double steer) {
updateGyroTarget(steer);
m_drive.runDriveVelocityPID(0, m_targetGyro);
System.out.println("Driving with Velocity PID with Target: " + m_targetGyro);
}
/**
* If AuxPID is enabled, then update using the steer input
*/
src/main/java/frc4388/robot/subsystems/Drive.java
+4 -4
View File
@@ -278,10 +278,10 @@ public class Drive extends SubsystemBase {
SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
//SmartDashboard.putNumber("Right Front Motor Current", m_rightFrontMotor.getSupplyCurrent());
//SmartDashboard.putNumber("Left Front Motor Current", m_leftFrontMotor.getSupplyCurrent());
//SmartDashboard.putNumber("Right Back Motor Current", m_rightFrontMotor.getSupplyCurrent());
//SmartDashboard.putNumber("Left Back Motor Current", m_leftFrontMotor.getSupplyCurrent());
SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent());
SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent());
SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent());
SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getSupplyCurrent());
SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY));
SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));