team2059/ford-java
2
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

untested drive straight auto

Browse Source
This commit is contained in:
Adam Long 2016-09-26 21:07:15 +00:00
parent 8d30a24e89
commit 1468a634b3
8 changed files with 50 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/org/usfirst/frc/team2059/robot/OI.java
View File

@ -46,9 +46,6 @@ public class OI {
joystickButtons[1][3].whileHeld(new SetArmPosition(RobotMap.mainArmPresetTraverse));
joystickButtons[1][4].whileHeld(new SetArmPosition(RobotMap.mainArmPresetCloseShot));
joystickButtons[1][5].whileHeld(new SetArmPosition(RobotMap.mainArmPresetLowShot));
joystickButtons[1][7].whileHeld(new SetArmStopState(true));
// joystickButtons[1][10].whileHeld(new ResetLower(-1));
// joystickButtons[1][11].whileHeld(new ResetUpper(1));
}
public Joystick[] getJoysticks() {
return joysticks;

2
src/org/usfirst/frc/team2059/robot/Robot.java
View File

@ -22,10 +22,12 @@ public class Robot extends IterativeRobot {
cameraServer.startAutomaticCapture("cam0");
chooser.addDefault("Nothing", new RoutineNothing());
chooser.addObject("Time based low bar", new RoutineDriveTime());
chooser.addObject("Time based straight low bar", new RoutineDriveStraightTime());
chooser.addObject("Time based defense", new RoutineDefenseTime());
SmartDashboard.putData("Auto mode", chooser);
SmartDashboard.putData("MainArm", CommandBase.mainArm.getPIDController());
SmartDashboard.putData("LeftEncoderController", CommandBase.driveBase.getLeftController());
SmartDashboard.putNumber("GyroCorrection",0.0);
SmartDashboard.putBoolean("CompressorEnabled", true);
//Automatically determine if rolling in or rolling out
SmartDashboard.putBoolean("SmartRollers", false);

5
src/org/usfirst/frc/team2059/robot/RobotMap.java
View File

@ -7,9 +7,10 @@ public class RobotMap {
public static int driveRightMotorTwo = 4;
public static int driveLeftEncoderA = 0;
public static int driveLeftEncoderB = 1;
public static int gyro = 1;
//Arm
public static double zeroDegrees = 1.622;
public static double ninetyDegrees = 3.234;
public static double zeroDegrees = 1.520;
public static double ninetyDegrees = 3.419;
public static int armPot = 0;
public static int armLeftMotor = 5;
public static int armRightMotor = 6;

26
src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDriveStraightTime.java Normal file
View File

@ -0,0 +1,26 @@
package org.usfirst.frc.team2059.robot.commands.autonomous;
import org.usfirst.frc.team2059.robot.commands.CommandBase;
import org.usfirst.frc.team2059.robot.Robot;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class AutoDriveStraightTime extends CommandBase {
public AutoDriveStraightTime(double time) {
requires(driveBase);
setTimeout(time);
}
protected void initialize() {
driveBase.resetGyro();
}
protected void execute() {
driveBase.driveStraight(-0.5,SmartDashboard.getNumber("GyroCorrection"));
}
protected void end() {
driveBase.stopDriving();
}
protected void interrupted() {
end();
}
protected boolean isFinished() {
return isTimedOut();
}
}
// vim: sw=2:ts=2:sts=2

2
src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDefenseTime.java
View File

@ -6,6 +6,6 @@ public class RoutineDefenseTime extends CommandGroup {
public RoutineDefenseTime() {
addSequential(new AutoSetArmStopState(false));
addSequential(new AutoSetArmPosition(6));
addSequential(new AutoDriveTime(1.5, 0.8));
addSequential(new AutoDriveTime(2.5, 0.8));
}
}

11
src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveStraightTime.java Normal file
View File

@ -0,0 +1,11 @@
package org.usfirst.frc.team2059.robot.commands.autonomous;
import org.usfirst.frc.team2059.robot.commands.CommandBase;
import edu.wpi.first.wpilibj.command.CommandGroup;
import org.usfirst.frc.team2059.robot.Robot;
public class RoutineDriveStraightTime extends CommandGroup {
public RoutineDriveStraightTime() {
addSequential(new AutoSetArmStopState(true));
addSequential(new AutoResetLower(-1));
addSequential(new AutoDriveStraightTime(2));
}
}

2
src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveTime.java
View File

@ -6,6 +6,6 @@ public class RoutineDriveTime extends CommandGroup {
public RoutineDriveTime() {
addSequential(new AutoSetArmStopState(true));
addSequential(new AutoResetLower(-1));
addSequential(new AutoDriveTime(1.5, 0.75));
addSequential(new AutoDriveTime(2.5, 0.75));
}
}

7
src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java
View File

@ -1,6 +1,7 @@
package org.usfirst.frc.team2059.robot.subsystems;
import org.usfirst.frc.team2059.robot.RobotMap;
import org.usfirst.frc.team2059.robot.commands.drivetrain.Drive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDOutput;
@ -14,7 +15,7 @@ public class DriveBase extends Subsystem {
CANTalon rightMotorTwo = new CANTalon(RobotMap.driveRightMotorTwo);
Encoder leftEncoder = new Encoder(RobotMap.driveLeftEncoderA, RobotMap.driveLeftEncoderB, false, Encoder.EncodingType.k2X);
AnalogGyro gyro = new AnalogGyro(RobotMap.gyro);
PIDController gyroController = new PIDController(0.0,0.0,0.0, gyro, new MotorsPIDOutput());
PIDController leftEncoderController = new PIDController(0.02, 0.002, 0.017, leftEncoder, new MotorsPIDOutput());
public void initDefaultCommand() {
setDefaultCommand(new Drive());
@ -32,6 +33,7 @@ public class DriveBase extends Subsystem {
rightMotorTwo.set((y + (x + z)) * sensitivity);
}
public void driveStraight(double y, double correction){
SmartDashboard.putNumber("GyroAngle",gyro.getAngle());
leftMotorOne.set((-y + gyro.getAngle()*correction));
leftMotorTwo.set((-y + gyro.getAngle()*correction));
rightMotorOne.set((y + gyro.getAngle()*correction));
@ -48,6 +50,9 @@ public class DriveBase extends Subsystem {
public double getLeftRotations() {
return leftEncoder.get();
}
public void resetGyro(){
gyro.reset();
}
public class MotorsPIDOutput implements PIDOutput {
@Override
public void pidWrite(double output) {