diff --git a/.gitignore b/.gitignore index d37aceb..d972147 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,5 @@ /TestAutotune/nbproject/private/ /WiredCats2415/nbproject/private/ WiredCats2014/nbproject/project.bak.xml +WiredCats2014/nbproject/project.xml +/WiredCats2014/build/ \ No newline at end of file diff --git a/WiredCats2014/nbproject/private/private.xml b/WiredCats2014/nbproject/private/private.xml index 12a3e99..93e0583 100644 --- a/WiredCats2014/nbproject/private/private.xml +++ b/WiredCats2014/nbproject/private/private.xml @@ -3,11 +3,8 @@ - file:/C:/Users/WiredCats/Documents/NetBeansProjects/WiredCats2014/WiredCats2415/src/edu/wpi/first/wpilibj/templates/commands/CommandArcadeDrive.java - file:/C:/Users/WiredCats/Documents/NetBeansProjects/WiredCats2014/WiredCats2415/src/Utilities/TXTReader.java - file:/C:/Users/WiredCats/Documents/NetBeansProjects/WiredCats2014/WiredCats2415/src/edu/wpi/first/wpilibj/templates/WiredCatsRobot.java - file:/C:/Users/WiredCats/Documents/NetBeansProjects/WiredCats2014/WiredCats2415/src/edu/wpi/first/wpilibj/templates/commands/CommandBase.java - file:/C:/Users/WiredCats/Documents/NetBeansProjects/WiredCats2014/WiredCats2415/src/Utilities/GamePad.java + file:/C:/Users/WiredCats/Documents/GitHub/WiredCats2014/WiredCats2014/src/edu/wpi/first/wpilibj/templates/RobotMap.java + file:/C:/Users/WiredCats/Documents/GitHub/WiredCats2014/WiredCats2014/src/edu/wpi/first/wpilibj/templates/subsystems/SubSystemDrive.java diff --git a/WiredCats2014/nbproject/project.bak.xml b/WiredCats2014/nbproject/project.bak.xml index 91b63c9..922c93a 100644 --- a/WiredCats2014/nbproject/project.bak.xml +++ b/WiredCats2014/nbproject/project.bak.xml @@ -3,91 +3,89 @@ This is a sample netbeans project file for a Sun Spot Application project. You may edit it freely, it doesn't affect the ant-powered build. - - --> - - org.netbeans.modules.ant.freeform - - - WiredCats2014 - - ${user.home}/.sunspotfrc.properties - build.properties - ${sunspot.home}/default.properties - - - - - java - src - - - - - jar-app - - - clean - - - deploy - run - - - clean - jar-app - - - deploy - debug-run - - - javadoc - - - - folder - build - jar-app - - - - - - src - - - build.xml - - - - - - - - - - - - deploy - - - - jar-deploy - - - - - - - - - src - ${sunspot.home}/lib/squawk.jar - ${sunspot.home}/lib/wpilibj.jar:${sunspot.home}/lib/networktables-crio.jar - build - 1.4 - - - - +--> + org.netbeans.modules.ant.freeform + + + WiredCats2415 + + ${user.home}/.sunspotfrc.properties + build.properties + ${sunspot.home}/default.properties + + + + + java + src + + + + + jar-app + + + clean + + + deploy + run + + + clean + jar-app + + + deploy + debug-run + + + javadoc + + + + folder + build + jar-app + + + + + + src + + + build.xml + + + + + + + + + + + + deploy + + + + jar-deploy + + + + + + + + + src + ${sunspot.home}\lib\squawk.jar + ${sunspot.home}\lib\wpilibj.jar;${sunspot.home}\lib\networktables-crio.jar + build + 1.4 + + + + \ No newline at end of file diff --git a/WiredCats2014/nbproject/project.xml b/WiredCats2014/nbproject/project.xml index 91b63c9..27c35ce 100644 --- a/WiredCats2014/nbproject/project.xml +++ b/WiredCats2014/nbproject/project.xml @@ -83,8 +83,8 @@ src - ${sunspot.home}/lib/squawk.jar - ${sunspot.home}/lib/wpilibj.jar:${sunspot.home}/lib/networktables-crio.jar + ${sunspot.home}\lib\squawk.jar + ${sunspot.home}\lib\wpilibj.jar;${sunspot.home}\lib\networktables-crio.jar build 1.4 diff --git a/WiredCats2014/src/Utilities/ChezyGyro.java b/WiredCats2014/src/Utilities/ChezyGyro.java index aba72ab..8d1289d 100644 --- a/WiredCats2014/src/Utilities/ChezyGyro.java +++ b/WiredCats2014/src/Utilities/ChezyGyro.java @@ -157,6 +157,17 @@ public double getAngle() { return scaledValue; } } + /** + * Returns the angular velocity of the robot. + * @author Sam Crane + * @return + */ + public double getAngularVelocity(){ + if (m_analog == null){ + return 0.0; + } + return m_analog.getAverageValue(); + } /** * Set the gyro type based on the sensitivity. diff --git a/WiredCats2014/src/Utilities/WiredVector.java b/WiredCats2014/src/Utilities/WiredVector.java index c3f4e89..8c35c28 100644 --- a/WiredCats2014/src/Utilities/WiredVector.java +++ b/WiredCats2014/src/Utilities/WiredVector.java @@ -24,26 +24,30 @@ public WiredVector() { * This function adds a value to the Blackbox's list of values * @param input Value to be added */ - public void addVal(int input) { + public void addVal(float input) { valueList.addElement(new OurInteger(input)); } - public int getVal(int index){ + public float getVal(int index){ return((OurInteger)valueList.elementAt(index)).i; } - public int lastElement(){ + public float lastElement(){ return ((OurInteger)valueList.lastElement()).i; } + public void removeFirst(){ + if (valueList.elementAt(0) != null) valueList.removeElementAt(0); + } + public int size(){ return valueList.size(); } private class OurInteger { - public int i; + public float i; - public OurInteger(int i){ + public OurInteger(float i){ this.i = i; } } diff --git a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/RobotMap.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/RobotMap.java index 17c53a6..74bcd47 100644 --- a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/RobotMap.java +++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/RobotMap.java @@ -14,18 +14,19 @@ public class RobotMap { public static final int DRIVER = 1; - public static final int DRIVE_LEFT = 2; - public static final int DRIVE_RIGHT = 1; - public static final int DRIVE_GYRO = 2; - public static final int DRIVE_ACCEL = 10; - - public static final int ARM_MOTOR = 3; - public static final int ARM_ENCODER_A = 9; - public static final int ARM_ENCODER_B = 8; - - - // If you are using multiple modules, make sure to define both the port - // number and the module. For example you with a rangefinder: - // public static final int rangefinderPort = 1; - // public static final int rangefinderModule = 1; + //PWM + public static final int DRIVE_LEFT_MOTOR_1 = 1; + public static final int DRIVE_LEFT_MOTOR_2 = 2; + public static final int DRIVE_RIGHT_MOTOR_1 = 3; + public static final int DRIVE_RIGHT_MOTOR_2 = 4; + //analog + public static final int DRIVE_GYRO = 1; +// public static final int DRIVE_ACCEL = 10; + public static final int DRIVE_LOW_SPEED_SOLENOID = 1; + public static final int DRIVE_HIGH_SPEED_SOLENOID = 2; + //digital + public static final int DRIVE_LEFT_ENCODER_A = 1; + public static final int DRIVE_LEFT_ENCODER_B = 2; + public static final int DRIVE_RIGHT_ENCODER_A = 3; + public static final int DRIVE_RIGHT_ENCODER_B = 4; } diff --git a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/WiredCatsRobot.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/WiredCatsRobot.java index b3ace3d..5fd6edd 100644 --- a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/WiredCatsRobot.java +++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/WiredCatsRobot.java @@ -54,7 +54,7 @@ public void teleopInit() { // continue until interrupted by another command, remove // this line or comment it out. //autonomousCommand.cancel(); - + CommandBase.resources.getFromFile("wiredCatsConfig.txt"); } /** diff --git a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/AutonomousCommands/CommandStraightDrive.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/AutonomousCommands/CommandStraightDrive.java new file mode 100644 index 0000000..2332516 --- /dev/null +++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/AutonomousCommands/CommandStraightDrive.java @@ -0,0 +1,37 @@ +/* + * To change this license header, choose License Headers in Project Properties. + * To change this template file, choose Tools | Templates + * and open the template in the editor. + */ + +package edu.wpi.first.wpilibj.templates.commands.AutonomousCommands; + +import edu.wpi.first.wpilibj.templates.commands.CommandBase; + +/** + * Drives in a straight line from whatever orientation + * the robot is in upon execution of the command. + * @author WiredCats + */ +public class CommandStraightDrive extends CommandBase { + + public CommandStraightDrive(float distance){ + + } + + protected void initialize() { + } + + protected void execute() { + } + + protected boolean isFinished() { + return false; + } + + protected void end() { + } + + protected void interrupted() { + } +} diff --git a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/AutonomousCommands/CommandTurn.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/AutonomousCommands/CommandTurn.java new file mode 100644 index 0000000..52f39d2 --- /dev/null +++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/AutonomousCommands/CommandTurn.java @@ -0,0 +1,46 @@ +/* + * To change this license header, choose License Headers in Project Properties. + * To change this template file, choose Tools | Templates + * and open the template in the editor. + */ + +package edu.wpi.first.wpilibj.templates.commands.AutonomousCommands; + +import edu.wpi.first.wpilibj.templates.commands.CommandBase; + +/** + * + * @author WiredCats + */ +public class CommandTurn extends CommandBase { + + /** + * Turns this many degrees from the current orientation. + * @param goal the desired orientation. + */ + public CommandTurn(float goal){ + + } + + protected void initialize() { + + } + + protected void execute() { + + } + + protected boolean isFinished() { + //TODO + return false; + } + + protected void end() { + + } + + protected void interrupted() { + + } + +} diff --git a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/AutonomousCommands/CommandWait.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/AutonomousCommands/CommandWait.java new file mode 100644 index 0000000..981439c --- /dev/null +++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/AutonomousCommands/CommandWait.java @@ -0,0 +1,42 @@ +/* + * To change this license header, choose License Headers in Project Properties. + * To change this template file, choose Tools | Templates + * and open the template in the editor. + */ + +package edu.wpi.first.wpilibj.templates.commands.AutonomousCommands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.templates.commands.CommandBase; + +/** + * + * @author WiredCats + */ +public class CommandWait extends CommandBase{ + + private double time; + private Timer t; + + public CommandWait(double time){ + this.time = time; + } + + protected void initialize() { + t = new Timer(); + t.start(); + } + + protected void execute() { + } + + protected boolean isFinished() { + return t.get() > time; + } + + protected void end() { + } + + protected void interrupted() { + } +} diff --git a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandArcadeDrive.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandArcadeDrive.java index 2c5309a..97f34a7 100644 --- a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandArcadeDrive.java +++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandArcadeDrive.java @@ -3,51 +3,87 @@ * and open the template in the editor. */ package edu.wpi.first.wpilibj.templates.commands; -import edu.wpi.first.wpilibj.command.Scheduler; -import java.lang.Math; +import Utilities.PID; +import Utilities.WiredVector; /** * * @author benbarber */ public class CommandArcadeDrive extends CommandBase{ - public static float PRIMARY_TURN_COEFFICIENT = 0.8f; - public static float SECONDARY_TURN_COEFFICIENT = 1.0f; + private float primaryTurnCoefficient = 0.8f; + private float jsDeadband = 0.06f; + private float interpolationBias = 0.7f; - double DEADBAND = 0.06; + private float upperShiftLimit = 7.0f; // ft/s + private float lowerShiftLimit = 4.0f; // ft/s + + private WiredVector speeds; public CommandArcadeDrive() { // Use requires() here to declare subsystem dependencies // eg. requires(chassis); requires(drivesubsystem); + speeds = new WiredVector(); } // Called just before this Command runs the first time protected void initialize() { + primaryTurnCoefficient = (float)resources.getValue("primaryTurnCoefficient"); + jsDeadband = (float)resources.getValue("jsDeadband"); + interpolationBias = (float)resources.getValue("interpolationBias"); + upperShiftLimit = (float)resources.getValue("upperShiftLimit"); + lowerShiftLimit = (float)resources.getValue("lowerShiftLimit"); } - // Called repeatedly when this Command is scheduled to run protected void execute() { double y = jsdriver.leftY(); double x = jsdriver.rightX(); - if(x <= -DEADBAND && x >= DEADBAND) x = 0; - if(y <= -DEADBAND && y >= DEADBAND) y = 0; + System.out.println(drivesubsystem.getSpeed()); + + if(Math.abs(x) < jsDeadband) x = 0; + if(Math.abs(y) < jsDeadband) y = 0; + + y = interpolationBias*y + (1-interpolationBias)*y*y*y; double left; double right; - if(Math.abs(y) <= DEADBAND){ - left = y - SECONDARY_TURN_COEFFICIENT*x; - right = y + SECONDARY_TURN_COEFFICIENT*x; + if(Math.abs(y) <= jsDeadband){ + left = y + x; + right = y - x; }else{ - left = y - PRIMARY_TURN_COEFFICIENT*x; - right = y + PRIMARY_TURN_COEFFICIENT*x; + left = y + primaryTurnCoefficient*x; + right = y - primaryTurnCoefficient*x; + } + + //shifting code. + float avgSpd = getAverageSpeed(); + if (avgSpd > upperShiftLimit && !drivesubsystem.isHighSpeed()){ + drivesubsystem.setHighSpeed(); + } else if (avgSpd < lowerShiftLimit && drivesubsystem.isHighSpeed()){ + drivesubsystem.setLowSpeed(); } + + //System.out.println("[WiredCats] Gyroscope: " + drivesubsystem.getAngle()); - drivesubsystem.setLeft(left); - drivesubsystem.setRight(right); + drivesubsystem.setLeftRight(left,right); + } + + /** + * Returns the average current speed of the drivetrain in feet/second. + */ + public float getAverageSpeed(){ + //TODO + float sum = 0; + speeds.addVal(drivesubsystem.getSpeed()); + for (int i = 0; i < speeds.size(); i++){ + sum+= speeds.getVal(i); + } + if (speeds.size() > 10) speeds.removeFirst(); + return sum / speeds.size(); } // Make this return true when this Command no longer needs to run execute() @@ -59,9 +95,13 @@ protected boolean isFinished() { protected void end() { } - // Called when another command which requires one or more of the same - // subsystems is scheduled to run + /** + * Called when arcade drive is interrupted by + * another command. Make sure to power down any + * motors to avoid any nasty loss of control. + */ protected void interrupted() { System.out.println("Arcade Drive interrupted."); + drivesubsystem.setLeftRight(0,0); } } diff --git a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandBase.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandBase.java index c2461ad..de5c5cb 100644 --- a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandBase.java +++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandBase.java @@ -30,8 +30,10 @@ public static void init() { // which commands extend), subsystems are not guaranteed to be // yet. Thus, their requires() statements may grab null pointers. Bad // news. Don't move it. - jsdriver.a_button.whenReleased(new CommandTankDrive()); - jsdriver.b_button.whenReleased(new CommandArcadeDrive()); + resources.getFromFile("wiredCatsConfig.txt"); + jsdriver.b_button.whenReleased(new CommandShift()); + + drivesubsystem.init(); // Show what command your subsystem is running on the SmartDashboard //SmartDashboard.putData("Autotune PID", new CommandAutotunePID()); } diff --git a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandShift.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandShift.java new file mode 100644 index 0000000..c4cb1d3 --- /dev/null +++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandShift.java @@ -0,0 +1,39 @@ +/* + * To change this license header, choose License Headers in Project Properties. + * To change this template file, choose Tools | Templates + * and open the template in the editor. + */ + +package edu.wpi.first.wpilibj.templates.commands; + +/** + * + * @author WiredCats + */ +public class CommandShift extends CommandBase{ + + protected void initialize() { + if (drivesubsystem.isHighSpeed()){ + drivesubsystem.setLowSpeed(); + } else { + drivesubsystem.setHighSpeed(); + } + } + + protected void execute() { + + } + + protected boolean isFinished() { + return true; + } + + protected void end() { + + } + + protected void interrupted() { + + } + +} diff --git a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandTankDrive.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandTankDrive.java index aba7676..9d312db 100644 --- a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandTankDrive.java +++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandTankDrive.java @@ -33,8 +33,7 @@ protected void execute() { float actualLeftMotor = (float)actualMotor(theorMotor(theorJS(actualLeftJS))); float actualRightMotor = (float)actualMotor(theorMotor(theorJS(actualRightJS))); - drivesubsystem.setLeft(actualLeftMotor); - drivesubsystem.setRight(actualRightMotor); + drivesubsystem.setLeftRight(actualLeftMotor,actualRightMotor); } diff --git a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/subsystems/SubSystemDrive.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/subsystems/SubSystemDrive.java index 7458368..2943e60 100644 --- a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/subsystems/SubSystemDrive.java +++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/subsystems/SubSystemDrive.java @@ -2,6 +2,7 @@ package edu.wpi.first.wpilibj.templates.subsystems; import Utilities.ChezyGyro; +import Utilities.PID; import edu.wpi.first.wpilibj.Gyro; import edu.wpi.first.wpilibj.Accelerometer; import edu.wpi.first.wpilibj.Encoder; @@ -18,28 +19,37 @@ public class SubSystemDrive extends Subsystem { // Put methods for controlling this subsystem // here. Call these from Commands. - public static final int TICKS_PER_ROTATION = 240; - - public static final float WHEEL_CIRCUMFERENCE_FEET = (float)((2*2*Math.PI)/12); - public static final float TICKS_TO_FEET = WHEEL_CIRCUMFERENCE_FEET / TICKS_PER_ROTATION; + public static final int TICKS_PER_REVOLUTION = 120; + public static final int WHEEL_RADIUS = 2; + public static final float WHEEL_CIRCUMFERENCE_FEET = (float)((WHEEL_RADIUS*2*Math.PI)/12); + public static final float TICKS_TO_FEET_PER_SECOND = WHEEL_CIRCUMFERENCE_FEET / TICKS_PER_REVOLUTION; - - Talon left = new Talon(RobotMap.DRIVE_LEFT_MOTOR_1); - Talon left2 = new Talon(RobotMap.DRIVE_LEFT_MOTOR_2); - Talon right = new Talon(RobotMap.DRIVE_RIGHT_MOTOR_1); - Talon right2 = new Talon(RobotMap.DRIVE_RIGHT_MOTOR_2); - ChezyGyro gyro = new ChezyGyro(RobotMap.DRIVE_GYRO); - Accelerometer accel = new Accelerometer(RobotMap.DRIVE_ACCEL); - Encoder leftEncoder = new Encoder(RobotMap.DRIVE_LEFT_ENCODER_A, + private Talon left = new Talon(RobotMap.DRIVE_LEFT_MOTOR_1); + private Talon left2 = new Talon(RobotMap.DRIVE_LEFT_MOTOR_2); + private Talon right = new Talon(RobotMap.DRIVE_RIGHT_MOTOR_1); + private Talon right2 = new Talon(RobotMap.DRIVE_RIGHT_MOTOR_2); + private ChezyGyro gyro = new ChezyGyro(RobotMap.DRIVE_GYRO); + //Accelerometer accel = new Accelerometer(RobotMap.DRIVE_ACCEL); + private Encoder leftEncoder = new Encoder(RobotMap.DRIVE_LEFT_ENCODER_A, RobotMap.DRIVE_LEFT_ENCODER_B); - Encoder rightEncoder = new Encoder(RobotMap.DRIVE_RIGHT_ENCODER_A, + private Encoder rightEncoder = new Encoder(RobotMap.DRIVE_RIGHT_ENCODER_A, RobotMap.DRIVE_RIGHT_ENCODER_B); - - Solenoid lowSpeedSolenoid = new Solenoid(RobotMap.DRIVE_LOW_SPEED_SOLENOID); - Solenoid highSpeedSolenoid = new Solenoid(RobotMap.DRIVE_HIGH_SPEED_SOLENOID); + private Solenoid lowSpeedSolenoid = new Solenoid(RobotMap.DRIVE_LOW_SPEED_SOLENOID); + private Solenoid highSpeedSolenoid = new Solenoid(RobotMap.DRIVE_HIGH_SPEED_SOLENOID); + + public PID lowStraightPID = new PID(0.5f,0,0); + public PID highStraightPID = new PID(0.5f,0,0); + public PID lowTurnPID = new PID(0.5f,0,0); + public PID highTurnPID = new PID(0.5f,0,0); + public PID straightPID; + public PID turnPID; + public void init(){ gyro.initGyro(); + leftEncoder.start(); + rightEncoder.start(); + setLowSpeed(); } public void initDefaultCommand() { @@ -47,20 +57,26 @@ public void initDefaultCommand() { setDefaultCommand(new CommandArcadeDrive()); } - - public void setLeft(double power){ - this.left.set(power); - left2.set(power); + /** + * Sets the left and right side of the drivetrain. + * @param l + * @param r + */ + public void setLeftRight(double l, double r){ + left.set(l); + left2.set(l); + right.set(-r); + right2.set(-r); } - public void setRight(double power){ - this.right.set(-power); - right2.set(-power); - } - public void setHighSpeed(){ highSpeedSolenoid.set(true); lowSpeedSolenoid.set(false); + } + + public void setLowSpeed(){ + lowSpeedSolenoid.set(true); + highSpeedSolenoid.set(false); } /** @@ -68,48 +84,19 @@ public void setHighSpeed(){ * @return */ public float getSpeed(){ - float ticks = (float)(leftEncoder.getRate() + rightEncoder.getRate()); + float ticks = (float)(leftEncoder.getRate() + -rightEncoder.getRate()); ticks /= 2; - return TICKS_TO_FEET * ticks; + return TICKS_TO_FEET_PER_SECOND * ticks; } - public void setLowSpeed(){ - lowSpeedSolenoid.set(true); - highSpeedSolenoid.set(false); - } - - public float getGyroAngle(){ + public float getAngle(){ return (float)gyro.getAngle(); } - -// public float getAngularVelocity(){ -// return (float)gyro. -// } -// - public void setHighSpeed(){ - lowSpeedSolenoid.set(false); - highSpeedSolenoid.set(true); - } - - public void setLowSpeed(){ - lowSpeedSolenoid.set(true); - highSpeedSolenoid.set(false); - } - + public boolean isHighSpeed(){ return highSpeedSolenoid.get(); } - /** - * Returns the robots speed in ft/s. - * @return - */ - public float getSpeed(){ - float ticks = (float)(leftEncoder.getRate() + rightEncoder.getRate()); - ticks /= 2; - return TICKS_TO_FEET * ticks; - } - public void resetGyro(){ gyro.reset(); }