项目:Robot_2016
文件:Robot.java
public void disabledPeriodic() {
RobotMap.lightRing.set(Relay.Value.kOff);
Scheduler.getInstance().run();
recordedID = (String) (oi.index.getSelected());
recordedAuton = SmartDashboard.getBoolean("Use Recorded Autonomous");
aimer.toPositionMode();
RobotMap.winchMotor.setEncPosition(0);
RobotMap.winchMotor.setPosition(0);
RobotMap.winchMotor.set(0);
DashboardOutput.putPeriodicData();
isBlue = (DriverStation.getInstance().getAlliance() == Alliance.Blue);
sendStatetoLights(false,false);
}
protected void initialize() {
logger.info("Initialize");
startAngle = Robot.driveTrain.getYaw();
if (DriverStation.getInstance().getAlliance() == Alliance.Red) {
targetAngle = DriveTrain.fixdegrees(startAngle + 120);
clockwise = true;
} else {
targetAngle = DriveTrain.fixdegrees(startAngle - 120);
clockwise = false;
}
}
项目:Steamworks2017Robot
文件:FieldMap.java
/**
* Gets the current alliance FieldMap.
*
* @return Either red map or blue map
*/
public static FieldMap getAllianceMap() {
Alliance alliance = DriverStation.getInstance().getAlliance();
if (alliance == Alliance.Blue) {
return getBlue();
} else if (alliance == Alliance.Red) {
return getRed();
} else {
logger.error("Invalid alliance reported by DS!");
return getBlue();
}
}
项目:Steamworks2017Robot
文件:FieldMap.java
/**
* Generates navigation to the boiler from the far side gear lift.
* @param currentPosition The current robot position
*/
public static DrivePathCommand navigateFeederSideLiftToBoiler(RobotPosition currentPosition) {
logger.info(String.format("Calculating path,start=%s",currentPosition));
FieldMap map = getAllianceMap();
FieldPosition boiler = map.boiler;
Alliance alliance = DriverStation.getInstance().getAlliance();
FieldPosition spline0 = currentPosition.add(alliance == Alliance.Red ? 1 : -1,0);
final double clearX = FieldPosition.CLEAR_DIVIDERS_TO_CENTER;
FieldPosition clearOfDividersPosition =
new FieldPosition(alliance == Alliance.Red ? -clearX : clearX,currentPosition.y);
double distancetoBoiler = clearOfDividersPosition.distanceto(boiler);
List<FieldPosition> controlPoints = new LinkedList<FieldPosition>();
controlPoints.add(spline0);
controlPoints.add(currentPosition);
controlPoints.add(clearOfDividersPosition);
double ratio = RobotMap.SHOOTING_disTANCE / distancetoBoiler;
controlPoints.add(clearOfDividersPosition.multiply(1 - ratio).add(boiler.multiply(ratio)));
controlPoints.add(boiler);
List<FieldPosition> splinePoints = generateCatmullRomSpline(controlPoints);
DrivePathCommand drivePathCommand = new DrivePathCommand(splinePoints);
return drivePathCommand;
}
项目:Steamworks2017Robot
文件:FieldMap.java
/**
* Generates navigation to the boiler from a starting position.
* @param startingSide The robot starting configuration
*/
public static DrivePathCommand navigateStartToBoiler(FieldSide startingSide) {
logger.info(String.format("Calculating path,startingSide.toString()));
FieldMap map = getAllianceMap();
final Alliance alliance = DriverStation.getInstance().getAlliance();
final FieldPosition startingPosition = map.startingPositions[startingSide.id];
List<FieldPosition> controlPoints = new LinkedList<FieldPosition>();
FieldPosition spline0 = startingPosition.add(alliance == Alliance.Red ? -12 : 12,0);
controlPoints.add(spline0);
controlPoints.add(startingPosition);
FieldPosition initialForward = startingPosition.add(alliance == Alliance.Red ? 24 : -24,0);
controlPoints.add(initialForward);
if (startingSide != FieldSide.BOILER) {
FieldPosition closetoAllianceWallPoint =
new FieldPosition(alliance == Alliance.Red ? -297.545 : 297.545,-66);
controlPoints.add(closetoAllianceWallPoint);
}
FieldPosition finalPoint =
new FieldPosition(alliance == Alliance.Red ? -225.977 : 225.977,-94.018);
controlPoints.add(finalPoint);
controlPoints.add(new FieldPosition(alliance == Alliance.Red ? -200 : 200,-94.018));
List<FieldPosition> splinePoints = generateCatmullRomSpline(controlPoints);
DrivePathCommand drivePathCommand = new DrivePathCommand(splinePoints);
return drivePathCommand;
}
private static byte [] createGamePacket() {
DriverStation ds = DriverStation.getInstance();
float matchTime = (float)ds.getMatchTime();
float batteryVoltage = (float)ds.getBatteryVoltage();
byte dsNumber = (byte)ds.getLocation();
int teamNumber = ds.getTeamNumber();
byte [] matchTimeRaw = intToByteArray(Float.floatToIntBits(matchTime));
byte [] batteryVoltageRaw = intToByteArray(Float.floatToIntBits(batteryVoltage));
byte [] teamNumberRaw = intToByteArray(teamNumber);
byte miscData = 0;
final byte [] ref = {(byte)0x80,0x40,0x20,0x10,0x08,0x04,0x02,0x01};
miscData ^= (ds.getAlliance().value==Alliance.kBlue_val)?ref[0]:0; //(Blue,Red)(True,False)
miscData ^= !ds.isEnabled() ? ref[1] : 0; // (disabled,Enabled)
miscData ^= ds.isAutonomous()? ref[2] : 0; // (Autonomous,)
miscData ^= ds.isOperatorControl() ? ref[3] : 0; // (Operator Control,)
miscData ^= ds.istest() ? ref[4] : 0; // (Test,)
miscData ^= ds.isFMSAttached() ? ref[5] : 0; // (FMS,)
byte [] data = new byte[14 + headerSize];
int pos = 0;
generateHeader(data,headerSize,0);
pos += headerSize;
System.arraycopy(matchTimeRaw,data,pos,4); pos += 4;
System.arraycopy(batteryVoltageRaw,4); pos += 4;
data[pos] = dsNumber; pos++;
System.arraycopy(teamNumberRaw,4); pos += 4;
data[pos] = miscData; pos++;
return data;
}
private static byte [] createGamePacket() {
DriverStation ds = DriverStation.getInstance();
float matchTime = (float)ds.getMatchTime();
float batteryVoltage = (float)ds.getBatteryVoltage();
byte dsNumber = (byte)ds.getLocation();
int teamNumber = ds.getTeamNumber();
byte [] matchTimeRaw = intToByteArray(Float.floatToIntBits(matchTime));
byte [] batteryVoltageRaw = intToByteArray(Float.floatToIntBits(batteryVoltage));
byte [] teamNumberRaw = intToByteArray(teamNumber);
byte miscData = 0;
final byte [] ref = {(byte)0x80,4); pos += 4;
data[pos] = miscData; pos++;
return data;
}
项目:StormRobotics2017
文件:LEDz.java
public void update() {
if (Robot.ds.getAlliance().equals(Alliance.Blue)) {
ledBlueAlliance = true;
ledRedAlliance = false;
} else if (Robot.ds.getAlliance().equals(Alliance.Red)) {
ledBlueAlliance = false;
ledRedAlliance = true;
} else {
ledBlueAlliance = false;
ledRedAlliance = false;
}
double angle = table.getNumber("p_angle",0);
double targets = table.getNumber("targets",0);
if (targets < 2)
vision = 0;
else if (angle > 1)
vision = 1;
else if (angle < -1)
vision = 3;
else
vision = 2;
table.putNumber("vision",vision);
LEDZ.putNumber("ledGearOn",ledGearOn);
byte[] ff = new byte[1];
// if(ledStatus != 0){
// Robot.driveTrain.tankDrive(0,0.25);
// }
if (!Robot.gear.getHaltGear()) {
ff[0] = (byte) 255;
ledOut.write(ff,1);
} else if (vision == 1) {
ff[0] = (byte) 110;
ledOut.write(ff,1);
} else if (vision == 2) {
ff[0] = (byte) 140;
ledOut.write(ff,1);
} else if (vision == 3) {
ff[0] = (byte) 120;
ledOut.write(ff,1);
} else if (ledGearOn == 1) {
ff[0] = (byte) 130;
ledOut.write(ff,1);
}
// else if(ledGearOn == 0){
// ff[0] = (byte) 255;
// ledOut.write(ff,1);
// }
// else{
// ff[0] = (byte) 130;
// ledOut.write(ff,1);
// }
else if (ledHang) {
ff[0] = (byte) 253;
ledOut.write(ff,1);
} else if (ledBlueAlliance) {
ff[0] = (byte) 10;
ledOut.write(ff,1);
} else if (ledRedAlliance) {
ff[0] = (byte) 20;
ledOut.write(ff,1);
} else {
ff[0] = (byte) 0;
ledOut.write(ff,1);
}
// if(ledOff){
// ff[0] = (byte) 0;
// ledOut.write(ff,1);
// }
}
项目:Steamworks2017Robot
文件:FieldMap.java
/**
* Generates navigation from a starting position to the hopper.
* @param startingPositionId The starting position ID
* @return A DrivetoPathCommand that drives the generated path
*/
public static DrivePathCommand navigateStartToHopper(int startingPositionId) {
FieldMap map = getAllianceMap();
final Alliance alliance = DriverStation.getInstance().getAlliance();
final FieldPosition hopperPosition =
alliance == Alliance.Red ? red.hopperBoilerRed : blue.hopperBoilerBlue;
if (startingPositionId != 0) {
startingPositionId = 0;
logger.error("New starting position: " + startingPositionId);
}
final List<FieldPosition> controlPoints = new LinkedList<FieldPosition>();
FieldPosition startingPosition = map.startingPositions[startingPositionId];
logger.debug(String.format("Starting position %s",startingPosition));
// add a point behind us so the C-R spline generates correctly
controlPoints.add(startingPosition.add(alliance == Alliance.Red ? -12 : 12,0));
// drive forward 2 feet
FieldPosition initialForwardPosition =
startingPosition.add(alliance == Alliance.Red ? 24 : -24,0);
logger.debug(String.format("2ft forward position %s",initialForwardPosition));
controlPoints.add(initialForwardPosition);
double initialDrivetoX;
double initialDrivetoY;
initialDrivetoX = alliance == Alliance.Red
? hopperPosition.getX() - HOPPER_TRIGGER_WIDTH / 2 - RobotMap.ROBOT_WIDTH / 2 + 2.0
: hopperPosition.getX() + HOPPER_TRIGGER_WIDTH / 2 + RobotMap.ROBOT_WIDTH / 2 - 2.0;
initialDrivetoY = hopperPosition.getY()
- (RobotMap.ROBOT_LENGTH - RobotMap.ROBOT_SHOOTER_TO_TURN_CENTER_disT);
double splinePointX = initialDrivetoX;
double splinePointY = initialDrivetoY - 24.0;
FieldPosition initialDrivetoPosition = new FieldPosition(initialDrivetoX,initialDrivetoY);
logger.debug(String.format("Drive to position %s",initialDrivetoPosition));
controlPoints.add(initialDrivetoPosition);
FieldPosition splinePoint = new FieldPosition(splinePointX,splinePointY);
logger.debug(String.format("Spline point %s",splinePoint));
controlPoints.add(splinePoint);
logger.info(controlPoints.toString());
List<FieldPosition> splinePoints = generateCatmullRomSpline(controlPoints);
logger.info(splinePoints.toString());
DrivePathCommand drivePathCommand = new DrivePathCommand(splinePoints);
return drivePathCommand;
}
项目:turtleshell
文件:TurtleSafeDriverStation.java
public static Alliance getAlliance() {
return (dsSet() ? ds.getAlliance() : Alliance.Invalid);
}
public static boolean isRed() {
return driverStation.getAlliance().value == Alliance.kRed_val;
}
/**
* Updates the pattern that the LEDs are displaying. This method checks the
* status of varIoUs robot components to determine what pattern to display.
*/
public void updatePattern() {
int oldPattern = pattern;
int oldOffset = offset;
// LEDs are solid by default
pattern = SOLID_CODE;
DriverStation ds = DriverStation.getInstance();
if (ds.isEnabled()) {
// Enabled
if (ds.isAutonomous()) {
// Autonomous mode
pattern = RANDOM_PATTERN_CODE;
} else if (ds.isOperatorControl()) {
// Teleop
if (Robot.catcherSubsytem.isExtended()) {
// Catcher extended
pattern = BLINK_CODE;
} else {
switch (Robot.sweeperSubsystem.getState().state) {
case sweeperSubsystem.MotorState.SWEEPING_VALUE:
// sweeper sweeping
pattern = diveRGE_CODE;
break;
case sweeperSubsystem.MotorState.EJECTING_VALUE:
// sweeper ejecting
pattern = CONVERGE_CODE;
break;
default:
case sweeperSubsystem.MotorState.OFF_VALUE:
// sweeper off
if (Robot.sweeperSubsystem.isExtended()) {
// sweeper extended
pattern = pulse_CODE;
}
break;
}
}
}
}
// Update the color offset based on the current alliance.
switch (DriverStation.getInstance().getAlliance().value) {
case Alliance.kRed_val:
offset = RED_OFFSET;
break;
case Alliance.kBlue_val:
offset = BLUE_OFFSET;
break;
default:
case Alliance.kInvalid_val:
// If the alliance is unkNown (ie. not connected to FMS),make
// the LEDs green.
offset = GREEN_OFFSET;
break;
}
// If the pattern or offset has changed,send the new data to the
// Arduino.
if (oldPattern != pattern || oldOffset != offset) {
switch (pattern) {
// disABLE_CODE and RANDOM_PATTERN_CODE do not have different
// colors,so do not factor in the offset.
case disABLE_CODE:
case RANDOM_PATTERN_CODE:
sendData(pattern);
break;
default:
// Every other code is available in red,blue or green.
sendData(pattern + offset);
}
}
}
项目:BadRobot2013
文件:OI.java
public void init() {
primaryXBoxController = new Joystick(PRIMARY_JOY);
secondaryXBoxController = new Joystick(SECONDARY_JOY);
ALLIANCE_COLOR = DriverStation.getInstance().getAlliance().value;
SmartDashboard.putBoolean("Alliance",ALLIANCE_COLOR == DriverStation.Alliance.kBlue_val);
preferencesManagers = BadPreferences.getInstance();
//button that senses seconadry Right bumper press for shooter injection
/*if (CommandBase.frisbeePusher != null)
{
Button injectFrisbee = new Button() {
public boolean get()
{
return (secondaryXBoxController.getRawButton(RB));
}
};
injectFrisbee.whenpressed(new InjectFrisbee());
}*/
//press A to climb
//if (CommandBase.climberArticulator != null) {
Button climb = new Button() {
public boolean get() {
return (OI.getPrimaryRightTrigger() > 0);
}
};
climb.whenpressed(new climbForTenPoints());
//}
if (CommandBase.shooterarticulator != null)
{
Button aim = new Button()
{
public boolean get()
{
return (isPrimaryYButtonpressed());
}
};
aim.whenpressed(new aimWithCamera());
}
if (!this.CONSOLE_OUTPUT_ENABLED) {
System.out.println("Console output has been disabled from OI");
}
}