package org.usfirst.frc.team3501.robot;
+import edu.wpi.first.wpilibj.SPI;
+
/**
* The Constants stores constant values for all subsystems. This includes the
* port values for motors and sensors, as well as important operational
public static class OI {
public final static int LEFT_STICK_PORT = 0;
public final static int RIGHT_STICK_PORT = 1;
+ public final static int TOGGLE_WINCH_PORT = 0;
+ public final static int TOGGLE_FLYWHEEL_PORT = 0;
+ public final static int TOGGLE_INDEXWHEEL_PORT = 0;
+ }
+
+ public static class Shooter {
+ // MOTOR CONTROLLERS
+ public static final int FLY_WHEEL = 0;
+ public static final int INDEX_WHEEL = 0;
+ public final static int TOGGLE_WINCH_PORT = 0;
+
+ public final static int TOGGLE_FLYWHEEL_PORT = 0;
+ public final static int TOGGLE_INDEXWHEEL_PORT = 0;
}
public static class DriveTrain {
public static final int REAR_RIGHT = 4;
// ENCODERS
- public static final int ENCODER_LEFT_A = 0;
- public static final int ENCODER_LEFT_B = 1;
+ public static final int ENCODER_LEFT_A = 1;
+ public static final int ENCODER_LEFT_B = 0;
public static final int ENCODER_RIGHT_A = 2;
public static final int ENCODER_RIGHT_B = 3;
+
+ public static final SPI.Port GYRO_PORT = SPI.Port.kOnboardCS0;
+ }
+
+ public static class Climber {
+ // MOTOR CONTROLLERS
+ public static final int MOTOR_VAL = 1;
+ }
+
+ public static class Intake {
+ public static final int INTAKE_ROLLER_PORT = 0;
}
public static enum Direction {
*/
public static double getSpeedForConstantAccel(double minSpeed,
double maxSpeed, double percentComplete) {
- return maxSpeed + 2 * (minSpeed - maxSpeed)
- * Math.abs(percentComplete - 0.5);
+ return maxSpeed
+ + 2 * (minSpeed - maxSpeed) * Math.abs(percentComplete - 0.5);
}
/***
/***
* Returns true if val is in the range [low, high]
- *
+ *
* @param val
* value to test
* @param low
public static boolean inRange(double val, double low, double high) {
return (val <= high) && (val >= low);
}
+
+ public static double limitValue(double val) {
+ return limitValue(val, 1.0);
+ }
+
+ public static double limitValue(double val, double max) {
+ if (val > max) {
+ return max;
+ } else if (val < -max) {
+ return -max;
+ } else {
+ return val;
+ }
+ }
+
+ public static double limitValue(double val, double max, double min) {
+ if (val > max) {
+ return max;
+ } else if (val < min) {
+ return min;
+ } else {
+ return val;
+ }
+ }
+
+ public static double calcLeftTankDrive(double x, double y) {
+ return limitValue(y + x);
+ }
+
+ public static double calcRightTankDrive(double x, double y) {
+ return limitValue(y - x);
+ }
}
package org.usfirst.frc.team3501.robot;
import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.buttons.Button;
+import edu.wpi.first.wpilibj.buttons.JoystickButton;
public class OI {
private static OI oi;
public static Joystick leftJoystick;
public static Joystick rightJoystick;
+ public static Button toggleWinch;
+
+ public static Button toggleIndexWheel;
+ public static Button toggleFlyWheel;
public OI() {
leftJoystick = new Joystick(Constants.OI.LEFT_STICK_PORT);
rightJoystick = new Joystick(Constants.OI.RIGHT_STICK_PORT);
-
+ toggleWinch = new JoystickButton(leftJoystick,
+ Constants.OI.TOGGLE_WINCH_PORT);
+ toggleIndexWheel = new JoystickButton(leftJoystick,
+ Constants.OI.TOGGLE_INDEXWHEEL_PORT);
+ toggleFlyWheel = new JoystickButton(leftJoystick,
+ Constants.OI.TOGGLE_FLYWHEEL_PORT);
}
-
- public static OI getOI(){
- if(oi == null)
- oi = new OI();
- return oi;
+
+ public static OI getOI() {
+ if (oi == null)
+ oi = new OI();
+ return oi;
}
}
package org.usfirst.frc.team3501.robot;
import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
+import org.usfirst.frc.team3501.robot.subsystems.Intake;
+import org.usfirst.frc.team3501.robot.subsystems.Shooter;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.Scheduler;
public class Robot extends IterativeRobot {
+ private static DriveTrain driveTrain;
+ private static Shooter shooter;
+ private static OI oi;
+ private static Intake intake;
+
@Override
public void robotInit() {
+ driveTrain = DriveTrain.getDriveTrain();
+ oi = OI.getOI();
+ shooter = Shooter.getShooter();
+ intake = Intake.getIntake();
+ // init
}
public static DriveTrain getDriveTrain() {
return DriveTrain.getDriveTrain();
}
+ public static Shooter getShooter() {
+ return Shooter.getShooter();
+ }
+
public static OI getOI() {
return OI.getOI();
}
+ public static Intake getIntake() {
+ return Intake.getIntake();
+ }
+
@Override
public void autonomousInit() {
Scheduler.getInstance();
--- /dev/null
+package org.usfirst.frc.team3501.robot.commandgroups;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class PrepareToShoot extends CommandGroup {
+
+ public PrepareToShoot() {
+ // Add Commands here:
+ // e.g. addSequential(new Command1());
+ // addSequential(new Command2());
+ // these will run in order.
+
+ // To run multiple commands at the same time,
+ // use addParallel()
+ // e.g. addParallel(new Command1());
+ // addSequential(new Command2());
+ // Command1 and Command2 will run in parallel.
+
+ // A command group will require all of the subsystems that each member
+ // would require.
+ // e.g. if Command1 requires chassis, and Command2 requires arm,
+ // a CommandGroup containing them would require both the chassis and the
+ // arm.
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commandgroups;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class Shoot extends CommandGroup {
+
+ public Shoot() {
+ // Add Commands here:
+ // e.g. addSequential(new Command1());
+ // addSequential(new Command2());
+ // these will run in order.
+
+ // To run multiple commands at the same time,
+ // use addParallel()
+ // e.g. addParallel(new Command1());
+ // addSequential(new Command2());
+ // Command1 and Command2 will run in parallel.
+
+ // A command group will require all of the subsystems that each member
+ // would require.
+ // e.g. if Command1 requires chassis, and Command2 requires arm,
+ // a CommandGroup containing them would require both the chassis and the
+ // arm.
+ }
+}
package org.usfirst.frc.team3501.robot.commands.climber;
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.Command;
/**
- * Runs the winch for a given time and motor value
+ * This command runs the winch at a specified speed and time in seconds when the
+ * button triggering it is pressed. This command also makes the drive train
+ * motors run because the winch is controlled by the drive train.
+ *
+ * pre-condition: This command is run by a button in OI. The robot must be
+ * attached to the rope.
+ *
+ * post-condition: Winch motor set to a specified speed for a specified time.
*
+ * @param motorVal
+ * value range is from -1 to 1
+ * @param time
+ * in seconds
* @author shivanighanta
*
*/
+
public class RunWinch extends Command {
+ Timer timer;
private double time;
private double motorVal;
+ /**
+ * See JavaDoc comment in class for details
+ *
+ * @param time
+ * time in seconds to run the winch
+ * @param motorVal
+ * value range is from -1 to 1
+ */
public RunWinch(double time, double motorVal) {
+ requires(Robot.getDriveTrain());
this.time = time;
this.motorVal = motorVal;
}
@Override
protected void initialize() {
+ timer.start();
}
@Override
protected void execute() {
+ Robot.getDriveTrain().setMotorValues(motorVal, motorVal);
}
@Override
protected boolean isFinished() {
- return false;
+ return timer.get() >= time;
}
@Override
protected void end() {
-
+ Robot.getDriveTrain().stop();
}
@Override
protected void interrupted() {
+ end();
}
}
package org.usfirst.frc.team3501.robot.commands.climber;
+import org.usfirst.frc.team3501.robot.Robot;
+
import edu.wpi.first.wpilibj.command.Command;
/**
- * Runs the winch continuously at a given motor value
+ * This command runs the drive train motors (which runs the winch) continuously
+ * at a specified speed until the button triggering it is released
+ *
+ * pre-condition: This command must be run by a button in OI. The robot must be
+ * attached to the rope.
+ *
+ * post-condition: Drive train motors set to a specified speed.
*
* @author shivanighanta
*
public class RunWinchContinuous extends Command {
private double motorVal;
+ /**
+ * See JavaDoc comment in class for details
+ *
+ * @param motorVal
+ * value range is from -1 to 1
+ */
public RunWinchContinuous(double motorVal) {
+ requires(Robot.getDriveTrain());
this.motorVal = motorVal;
}
@Override
protected void initialize() {
+ Robot.getDriveTrain().setMotorValues(motorVal, motorVal);
}
@Override
@Override
protected void interrupted() {
+ end();
}
}
package org.usfirst.frc.team3501.robot.commands.climber;
+import org.usfirst.frc.team3501.robot.Robot;
+
import edu.wpi.first.wpilibj.command.Command;
/**
public class StopWinch extends Command {
public StopWinch() {
+ requires(Robot.getDriveTrain());
}
@Override
@Override
protected void execute() {
-
}
@Override
protected boolean isFinished() {
- return false;
+ return true;
}
@Override
protected void end() {
+ Robot.getDriveTrain().stop();
}
@Override
protected void interrupted() {
+ end();
}
}
package org.usfirst.frc.team3501.robot.commands.driving;
import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
+import org.usfirst.frc.team3501.robot.utils.PIDController;
+import edu.wpi.first.wpilibj.Preferences;
import edu.wpi.first.wpilibj.command.Command;
/**
* This command makes the robot drive a specified distance using encoders on the
* robot and using a feedback loop
*
- * parameters:
- * distance the robot will move in inches
- * motorVal: the motor input to set the motors to
+ * parameters: distance the robot will move in inches motorVal: the motor input
+ * to set the motors to
*/
public class DriveDistance extends Command {
+ private DriveTrain driveTrain = Robot.getDriveTrain();
+ private double maxTimeOut;
+ private PIDController driveController;
+ private PIDController gyroController;
+ private Preferences prefs;
- public DriveDistance(double distance, double motorVal) {
- requires(Robot.getDriveTrain());
+ private double target;
+ private double gyroP;
+ private double gyroI;
+ private double gyroD;
+
+ private double driveP;
+ private double driveI;
+ private double driveD;
+
+ public DriveDistance(double distance, double maxTimeOut) {
+ requires(driveTrain);
+ this.maxTimeOut = maxTimeOut;
+ this.target = distance;
+
+ this.driveP = driveTrain.driveP;
+ this.driveI = driveTrain.driveI;
+ this.driveD = driveTrain.driveD;
+ this.gyroP = driveTrain.defaultGyroP;
+ this.gyroI = driveTrain.defaultGyroI;
+ this.gyroD = driveTrain.defaultGyroD;
+ this.driveController = new PIDController(this.driveP, this.driveI,
+ this.driveD);
+ this.driveController.setDoneRange(0.5);
+ this.driveController.setMaxOutput(1.0);
+ this.driveController.setMinDoneCycles(5);
+
+ this.gyroController = new PIDController(this.gyroP, this.gyroI, this.gyroD);
+ this.gyroController.setDoneRange(1);
+ this.gyroController.setMinDoneCycles(5);
}
- // Called just before this Command runs the first time
@Override
protected void initialize() {
+ this.driveTrain.resetEncoders();
+ this.driveTrain.resetGyro();
+ this.driveController.setSetPoint(this.target);
+ this.gyroController.setSetPoint(this.driveTrain.getZeroAngle());
}
- // Called repeatedly when this Command is scheduled to run
@Override
protected void execute() {
+ double xVal = 0;
+ double yVal = this.driveController
+ .calcPID(this.driveTrain.getAvgEncoderDistance());
+
+ if (this.driveTrain.getAngle() - this.driveTrain.getZeroAngle() < 30) {
+ xVal = -this.gyroController
+ .calcPID(this.driveTrain.getAngle() - this.driveTrain.getZeroAngle());
+ }
+ double leftDrive = yVal - xVal;
+ double rightDrive = yVal + xVal;
+
+ this.driveTrain.setMotorValues(leftDrive, rightDrive);
+
+ driveTrain.printEncoderOutput();
}
- // Make this return true when this Command no longer needs to run execute()
@Override
protected boolean isFinished() {
- return false;
+ boolean isDone = this.driveController.isDone();
+ if (timeSinceInitialized() >= maxTimeOut || isDone)
+ System.out.println("time: " + timeSinceInitialized());
+ return timeSinceInitialized() >= maxTimeOut || isDone;
}
- // Called once after isFinished returns true
@Override
protected void end() {
+ driveTrain.stop();
}
- // Called when another command which requires one or more of the same
- // subsystems is scheduled to run
@Override
protected void interrupted() {
+ end();
}
}
/**
* This command will run throughout teleop and listens for joystick inputs to
- * drive the driveTrain. This never finishes until teleop ends.
- * - works in conjunction with OI.java
+ * drive the driveTrain. This never finishes until teleop ends. - works in
+ * conjunction with OI.java
*/
public class JoystickDrive extends Command {
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.driving;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * This command toggles the speed at which the drive train runs at
+ *
+ * post-condition: if the drivetrain is running at high gear, it will be made to
+ * run at low gear, and vice versa
+ */
+public class ToggleGear extends Command {
+
+ public ToggleGear() {
+ requires(Robot.getDriveTrain());
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ @Override
+ protected void execute() {
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // Called once after isFinished returns true
+ @Override
+ protected void end() {
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ @Override
+ protected void interrupted() {
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.driving;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * This command turns the robot exactly 90 degrees towards the left
+ *
+ * parameters: none
+ */
+public class Turn90Left extends Command {
+
+ public Turn90Left() {
+ requires(Robot.getDriveTrain());
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ @Override
+ protected void execute() {
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // Called once after isFinished returns true
+ @Override
+ protected void end() {
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ @Override
+ protected void interrupted() {
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.driving;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * This command turns the robot exactly 90 degrees towards the right
+ *
+ * parameters: none
+ */
+public class Turn90Right extends Command {
+
+ public Turn90Right() {
+ requires(Robot.getDriveTrain());
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ @Override
+ protected void execute() {
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // Called once after isFinished returns true
+ @Override
+ protected void end() {
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ @Override
+ protected void interrupted() {
+ }
+}
package org.usfirst.frc.team3501.robot.commands.driving;
+import org.usfirst.frc.team3501.robot.Constants;
import org.usfirst.frc.team3501.robot.Constants.Direction;
import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
+import org.usfirst.frc.team3501.robot.utils.PIDController;
import edu.wpi.first.wpilibj.command.Command;
* This command turns the robot for a specified angle in specified direction -
* either left or right
*
- * parameters:
- * angle: the robot will turn - in degrees
- * direction: the robot will turn - either right or left
- * motorVal: the motor input to set the motors to
+ * parameters: angle: the robot will turn - in degrees direction: the robot will
+ * turn - either right or left maxTimeOut: the max time this command will be
+ * allowed to run on for
*/
public class TurnForAngle extends Command {
+ private DriveTrain driveTrain = Robot.getDriveTrain();
- public TurnForAngle(double angle, Direction direction, double motorVal) {
+ Direction direction;
+ private double maxTimeOut;
+ private PIDController gyroController;
+
+ private double target;
+ private double gyroP;
+ private double gyroI;
+ private double gyroD;
+
+ private double zeroAngle;
+
+ public TurnForAngle(double angle, Direction direction, double maxTimeOut) {
requires(Robot.getDriveTrain());
+ this.direction = direction;
+ this.maxTimeOut = maxTimeOut;
+ this.target = Math.abs(angle);
+ this.zeroAngle = driveTrain.getAngle();
+
+ this.gyroP = driveTrain.defaultGyroP;
+ this.gyroI = driveTrain.defaultGyroI;
+ this.gyroD = driveTrain.defaultGyroD;
+
+ this.gyroController = new PIDController(this.gyroP, this.gyroI, this.gyroD);
+ this.gyroController.setDoneRange(1);
+ this.gyroController.setMinDoneCycles(5);
}
- // Called just before this Command runs the first time
@Override
protected void initialize() {
+ this.driveTrain.resetEncoders();
+ this.gyroController.setSetPoint(this.target);
}
- // Called repeatedly when this Command is scheduled to run
@Override
protected void execute() {
+ double xVal = 0;
+
+ xVal = this.gyroController
+ .calcPID(Math.abs(this.driveTrain.getAngle() - this.zeroAngle));
+
+ double leftDrive = 0;
+ double rightDrive = 0;
+ if (direction == Constants.Direction.RIGHT) {
+ leftDrive = xVal;
+ rightDrive = -xVal;
+ } else if (direction == Constants.Direction.LEFT) {
+ leftDrive = -xVal;
+ rightDrive = xVal;
+ }
+
+ this.driveTrain.setMotorValues(leftDrive, rightDrive);
}
- // Make this return true when this Command no longer needs to run execute()
@Override
protected boolean isFinished() {
- return false;
+ return timeSinceInitialized() >= maxTimeOut || gyroController.isDone();
}
- // Called once after isFinished returns true
@Override
protected void end() {
}
- // Called when another command which requires one or more of the same
- // subsystems is scheduled to run
@Override
protected void interrupted() {
+ end();
}
}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.intake;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * Reverses the intake roller for a specified amount of time in seconds
+ *
+ * parameters: time to run intake
+ */
+public class ReverseIntake extends Command {
+ private double timeToMove;
+ public Timer timer;
+
+ public ReverseIntake(double timeToMove) {
+ requires(Robot.getIntake());
+ timer = new Timer();
+ this.timeToMove = timeToMove;
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ timer.start();
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ @Override
+ protected void execute() {
+ Robot.getIntake().runReverseIntake();
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return timer.get() >= timeToMove;
+ }
+
+ // Called once after isFinished returns true
+ @Override
+ protected void end() {
+ Robot.getIntake().stopIntake();
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ @Override
+ protected void interrupted() {
+ end();
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.intake;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * Reverses the intake until the button triggering this command is released
+ *
+ * pre-condition: button is pressed
+ */
+public class ReverseIntakeContinuous extends Command {
+
+ public ReverseIntakeContinuous() {
+ requires(Robot.getIntake());
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ @Override
+ protected void execute() {
+ Robot.getIntake().runReverseIntake();
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // Called once after isFinished returns true
+ @Override
+ protected void end() {
+ Robot.getIntake().stopIntake();
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ @Override
+ protected void interrupted() {
+ end();
+ }
+}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands.intake;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-/***
- *
- * Runs the intake continuously.
- *
- * @author Meeta
- *
- */
-public class RunContinuous extends Command {
-
- public RunContinuous() {
-
- }
-
- @Override
- protected boolean isFinished() {
- // TODO Auto-generated method stub
- return false;
- }
-
- // Called just before this Command runs the first time
- @Override
- protected void initialize() {
- }
-
- // Called repeatedly when this Command is scheduled to run
- @Override
- protected void execute() {
- }
-
- // Called once after isFinished returns true
- @Override
- protected void end() {
- }
-
- // Called when another command which requires one or more of the same
- // subsystems is scheduled to run
- @Override
- protected void interrupted() {
- }
-
-}
package org.usfirst.frc.team3501.robot.commands.intake;
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.Command;
/**
*/
public class RunIntake extends Command {
private double timeToMove;
+ public Timer timer;
+ /**
+ *
+ * @param timeToMove
+ */
public RunIntake(double timeToMove) {
+ requires(Robot.getIntake());
+ timer = new Timer();
this.timeToMove = timeToMove;
}
@Override
protected boolean isFinished() {
- // TODO Auto-generated method stub
- return false;
+ return timer.get() >= timeToMove;
}
// Called just before this Command runs the first time
@Override
protected void initialize() {
+ timer.start();
}
// Called repeatedly when this Command is scheduled to run
@Override
protected void execute() {
+ Robot.getIntake().runIntake();
}
// Called once after isFinished returns true
@Override
protected void end() {
+ timer.stop();
+ Robot.getIntake().stopIntake();
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
@Override
protected void interrupted() {
+ end();
}
}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.intake;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/***
+ *
+ * Runs the intake continuously when button is pressed, and when button is not
+ * pressed does not run.
+ *
+ * Intended to be run inside a .whileHeld() call on a button in OI
+ *
+ * @author Meeta
+ *
+ */
+public class RunIntakeContinuous extends Command {
+ // create setter method for speed, use setSpeed method to do end() by setting
+ // speed to 0
+
+ public RunIntakeContinuous() {
+ requires(Robot.getIntake());
+ }
+
+ @Override
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ @Override
+ protected void execute() {
+ Robot.getIntake().runIntake();
+ }
+
+ // Called once after isFinished returns true
+ @Override
+ protected void end() {
+ Robot.getIntake().stopIntake();
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ @Override
+ protected void interrupted() {
+ end();
+ }
+
+}
package org.usfirst.frc.team3501.robot.commands.intake;
+import org.usfirst.frc.team3501.robot.Robot;
+
import edu.wpi.first.wpilibj.command.Command;
/**
*/
public class StopIntake extends Command {
public StopIntake() {
-
+ requires(Robot.getIntake());
}
// Called just before this Command runs the first time
@Override
protected void initialize() {
+ Robot.getIntake().stopIntake();
}
// Called repeatedly when this Command is scheduled to run
// subsystems is scheduled to run
@Override
protected void interrupted() {
+ end();
}
@Override
protected boolean isFinished() {
- // TODO Auto-generated method stub
- return false;
+ return true;
}
}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.shooter;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * This command decreases the speed at which the flywheel runs at by a fixed
+ * constant
+ *
+ * post-condition: the shooting speed is decremented, such that whenever the
+ * flywheel is run, it will run at the decreased shooting speed
+ */
+public class DecreaseShootingSpeed extends Command {
+ public DecreaseShootingSpeed() {
+
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ @Override
+ protected void execute() {
+ }
+
+ // Called once after isFinished returns true
+ @Override
+ protected void end() {
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ @Override
+ protected void interrupted() {
+ }
+
+ @Override
+ protected boolean isFinished() {
+ // TODO Auto-generated method stub
+ return false;
+ }
+
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.shooter;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * This command increases the speed at which the flywheel runs at by a fixed
+ * constant
+ *
+ * post-condition: the shooting speed is incremented, such that whenever the
+ * flywheel is run, it will run at the increased shooting speed
+ */
+public class IncreaseShootingSpeed extends Command {
+ public IncreaseShootingSpeed() {
+
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ @Override
+ protected void execute() {
+ }
+
+ // Called once after isFinished returns true
+ @Override
+ protected void end() {
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ @Override
+ protected void interrupted() {
+ }
+
+ @Override
+ protected boolean isFinished() {
+ // TODO Auto-generated method stub
+ return false;
+ }
+
+}
package org.usfirst.frc.team3501.robot.commands.shooter;
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.Command;
/**
- * Runs the fly wheel at a given speed for a given time (sec)
+ * This command runs the fly wheel at a given speed for a given time. The fly
+ * wheel is intended to shoot balls fed by the intake wheel.
*
- * @param motorVal
- * [-1,1]
- * @param time
- * in seconds
- * @author shaina
+ * @author Shaina
*/
public class RunFlyWheel extends Command {
+ Timer timer;
private double motorVal;
private double time;
+ /**
+ * See JavaDoc comment in class for details
+ *
+ * @param motorVal
+ * value range from -1 to 1
+ * @param time
+ * in seconds, amount of time to run fly wheel motor
+ */
public RunFlyWheel(double motorVal, double time) {
+ requires(Robot.getShooter());
+
+ timer = new Timer();
this.motorVal = motorVal;
this.time = time;
}
// Called just before this Command runs the first time
@Override
protected void initialize() {
+ timer.start();
+ Robot.getShooter().setFlyWheelMotorVal(motorVal);
}
// Called repeatedly when this Command is scheduled to run
// Called once after isFinished returns true
@Override
protected void end() {
+ Robot.getShooter().stopFlyWheel();
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
@Override
protected void interrupted() {
+ end();
}
@Override
protected boolean isFinished() {
- return false;
+ return timer.get() >= time;
}
}
package org.usfirst.frc.team3501.robot.commands.shooter;
+import org.usfirst.frc.team3501.robot.Robot;
+
import edu.wpi.first.wpilibj.command.Command;
/**
- * Runs fly wheel continuously when corresponding button pressed
+ * This command runs the fly wheel continuously when OI button managing fly
+ * wheel is pressed. The command will run the fly wheel motor until the button
+ * triggering it is released.
+ *
+ * Should only be run from the operator interface.
*
- * Run stopFlyWheel command to stop
+ * pre-condition: This command must be run by a button in OI, with
+ * button.whileHeld(...).
*
- * @param motorVal
- * [-1,1]
- * @author shaina
+ * @author Shaina
*/
public class RunFlyWheelContinuous extends Command {
private double motorVal;
+ /**
+ * See JavaDoc comment in class for details
+ *
+ * @param motorVal
+ * value range from -1 to 1
+ */
public RunFlyWheelContinuous(double motorVal) {
this.motorVal = motorVal;
}
// Called just before this Command runs the first time
@Override
protected void initialize() {
+ Robot.getShooter().setFlyWheelMotorVal(motorVal);
}
// Called repeatedly when this Command is scheduled to run
// subsystems is scheduled to run
@Override
protected void interrupted() {
+ end();
}
@Override
protected boolean isFinished() {
- return false;
+ return !Robot.getOI().toggleFlyWheel.get();
}
}
package org.usfirst.frc.team3501.robot.commands.shooter;
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.Command;
/**
- * Runs index wheel at a given speed for given time in seconds
+ * This command runs index wheel at a given speed for given time in seconds.
+ *
+ * pre-condition: fly wheel is running at full speed to prepare for shooting
+ * fuel
*
- * @param motorVal
- * [-1,1]
- * @param time
- * in seconds
- * @author shaina
+ * @author Shaina
*/
public class RunIndexWheel extends Command {
+ Timer timer;
private double time;
private double motorVal;
+ /**
+ * See JavaDoc comment in class for details
+ *
+ * @param motorVal
+ * value range from -1 to 1
+ * @param time
+ * in seconds, amount of time to run index wheel motor
+ */
public RunIndexWheel(double motorVal, double time) {
+ requires(Robot.getShooter());
+
+ timer = new Timer();
this.motorVal = motorVal;
this.time = time;
}
// Called just before this Command runs the first time
@Override
protected void initialize() {
+ timer.start();
+ Robot.getShooter().setIndexWheelMotorVal(motorVal);
}
// Called repeatedly when this Command is scheduled to run
// Called once after isFinished returns true
@Override
protected void end() {
+ Robot.getShooter().stopIndexWheel();
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
@Override
protected void interrupted() {
+ end();
}
@Override
protected boolean isFinished() {
- return false;
+ return timer.get() >= time;
}
}
package org.usfirst.frc.team3501.robot.commands.shooter;
+import org.usfirst.frc.team3501.robot.Robot;
+
import edu.wpi.first.wpilibj.command.Command;
/**
- * Runs index wheel continuously when corresponding button is pressed
+ * This command runs index wheel continuously when OI button managing index
+ * wheel is pressed. The command will run the index wheel motor until the button
+ * triggering it is released.
+ *
+ * Should only be run from the operator interface.
*
- * Run stopIndexWheel to stop
+ * pre-condition: This command must be run by a button in OI with
+ * button.whileHeld(...).
*
- * @param motorVal
- * [-1,1]
- * @author shaina
+ * @author Shaina
*/
public class RunIndexWheelContinuous extends Command {
private double motorVal;
+ /**
+ * See JavaDoc comment in class for details
+ *
+ * @param motorVal
+ * value range from -1 to 1
+ */
public RunIndexWheelContinuous(double motorVal) {
this.motorVal = motorVal;
}
// Called just before this Command runs the first time
@Override
protected void initialize() {
+ Robot.getShooter().setIndexWheelMotorVal(motorVal);
}
// Called repeatedly when this Command is scheduled to run
// subsystems is scheduled to run
@Override
protected void interrupted() {
+ end();
}
@Override
protected boolean isFinished() {
- return false;
+ return Robot.getOI().toggleIndexWheel.get();
+
}
}
package org.usfirst.frc.team3501.robot.commands.shooter;
+import org.usfirst.frc.team3501.robot.Robot;
+
import edu.wpi.first.wpilibj.command.Command;
/**
- * Stops fly wheel
+ * This command stops the fly wheel. Do not call unless the trigger button has
+ * been released.
+ *
+ *
+ * @author Shaina
*/
public class StopFlyWheel extends Command {
- public StopFlyWheel() {
-
- }
-
- // Called just before this Command runs the first time
- @Override
- protected void initialize() {
- }
-
- // Called repeatedly when this Command is scheduled to run
- @Override
- protected void execute() {
- }
-
- // Called once after isFinished returns true
- @Override
- protected void end() {
- }
-
- // Called when another command which requires one or more of the same
- // subsystems is scheduled to run
- @Override
- protected void interrupted() {
- }
-
- @Override
- protected boolean isFinished() {
- // TODO Auto-generated method stub
- return false;
- }
+ /**
+ * This command stops the fly wheel.
+ */
+ public StopFlyWheel() {
+
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ @Override
+ protected void execute() {
+ }
+
+ // Called once after isFinished returns true
+ @Override
+ protected void end() {
+ Robot.getShooter().stopFlyWheel();
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ @Override
+ protected void interrupted() {
+ }
+
+ @Override
+ protected boolean isFinished() {
+ return true;
+ }
}
package org.usfirst.frc.team3501.robot.commands.shooter;
+import org.usfirst.frc.team3501.robot.Robot;
+
import edu.wpi.first.wpilibj.command.Command;
/**
- * Stops index wheel
+ * This command stops the index wheel. Do not call unless the trigger button has
+ * been released.
+ *
+ * @author Shaina
*/
public class StopIndexWheel extends Command {
- public StopIndexWheel() {
-
- }
-
- // Called just before this Command runs the first time
- @Override
- protected void initialize() {
- }
-
- // Called repeatedly when this Command is scheduled to run
- @Override
- protected void execute() {
- }
-
- // Called once after isFinished returns true
- @Override
- protected void end() {
- }
-
- // Called when another command which requires one or more of the same
- // subsystems is scheduled to run
- @Override
- protected void interrupted() {
- }
-
- @Override
- protected boolean isFinished() {
- // TODO Auto-generated method stub
- return false;
- }
+ /**
+ * This command stops the index wheel.
+ */
+ public StopIndexWheel() {
+
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ @Override
+ protected void execute() {
+ }
+
+ // Called once after isFinished returns true
+ @Override
+ protected void end() {
+ Robot.getShooter().stopIndexWheel();
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ @Override
+ protected void interrupted() {
+ }
+
+ @Override
+ protected boolean isFinished() {
+ return true;
+ }
}
+++ /dev/null
-package org.usfirst.frc.team3501.robot.subsystems;
-
-public class Climber {
- private static Climber climber;
-
- private Climber() {
-
- }
-
- public static Climber getClimber() {
- if (climber == null) {
- climber = new Climber();
- }
- return climber;
- }
-}
import com.ctre.CANTalon;
+import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.command.Subsystem;
public class DriveTrain extends Subsystem {
+ public static double driveP = 0.006, driveI = 0.001, driveD = -0.002;
+ public static double defaultGyroP = 0.004, defaultGyroI = 0.0013,
+ defaultGyroD = -0.005;
+ private double gyroZero = 0;
+
public static final double WHEEL_DIAMETER = 6; // inches
public static final int ENCODER_PULSES_PER_REVOLUTION = 256;
public static final double INCHES_PER_PULSE = WHEEL_DIAMETER * Math.PI
private final RobotDrive robotDrive;
private final Encoder leftEncoder, rightEncoder;
+ private ADXRS450_Gyro imu;
+
private DriveTrain() {
// MOTOR CONTROLLERS
frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT);
// ROBOT DRIVE
robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
+
+ this.imu = new ADXRS450_Gyro(Constants.DriveTrain.GYRO_PORT);
}
public static DriveTrain getDriveTrain() {
setMotorValues(0, 0);
}
- public double getFrontLeftMotorVal() {
- return frontLeft.get();
- }
-
- public double getFrontRightMotorVal() {
- return frontRight.get();
- }
-
- public double getRearLeftMotorVal() {
- return frontLeft.get();
- }
-
- public double getRearRightMotorVal() {
- return frontLeft.get();
+ public double getLeftMotorVal() {
+ return (frontLeft.get() + rearLeft.get()) / 2;
}
- public CANTalon getFrontLeft() {
- return frontLeft;
- }
-
- public CANTalon getFrontRight() {
- return frontRight;
- }
-
- public CANTalon getRearLeft() {
- return rearLeft;
- }
-
- public CANTalon getRearRight() {
- return rearRight;
+ public double getRightMotorVal() {
+ return (frontRight.get() + rearRight.get()) / 2;
}
// ENCODER METHODS
return rightEncoder.getDistance();
}
+ public void printEncoderOutput() {
+ // System.out.println("left: " + getLeftEncoderDistance());
+ // System.out.println("right: " + getRightEncoderDistance());
+ System.out.println(getAvgEncoderDistance());
+ }
+
public double getAvgEncoderDistance() {
return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2;
}
return (getLeftSpeed() + getRightSpeed()) / 2.0;
}
+ // ------Gyro------//
+ public double getAngle() {
+ return this.imu.getAngle() - this.gyroZero;
+ }
+
+ public void resetGyro() {
+ this.imu.reset();
+ }
+
+ public double getZeroAngle() {
+ return this.gyroZero;
+ }
+
@Override
protected void initDefaultCommand() {
setDefaultCommand(new JoystickDrive());
package org.usfirst.frc.team3501.robot.subsystems;
+import org.usfirst.frc.team3501.robot.Constants;
+
+import com.ctre.CANTalon;
+
import edu.wpi.first.wpilibj.command.Subsystem;
-/***
- *
- *
+/**
* @author Meeta
- *
*/
public class Intake extends Subsystem {
- public Intake() {
+ private static Intake intake = null;
+ private CANTalon intakeWheel;
+ public static final double INTAKE_SPEED = 0;
+ public static final double REVERSE_SPEED = 0;
+ public Intake() {
+ intakeWheel = new CANTalon(Constants.Intake.INTAKE_ROLLER_PORT);
}
- /**
- * Runs the intake continuously
+ /***
+ * It gets the intake instance, and if intake has not been initialized, then
+ * it will be initialized.
+ *
+ * @returns intake
*/
- public void RunContinous() {
+ public static Intake getIntake() {
+ if (intake == null) {
+ intake = new Intake();
+ }
+ return intake;
+ }
+
+ @Override
+ protected void initDefaultCommand() {
}
- /**
- * Starts running the intake for a specific period of time that the user
- * inputs.
+ /***
+ * Sets speed of intake wheel to input speed
*
- * @param timeToMove
- * in seconds
+ * @param speed
+ * from -1 to 1
*/
- public void RunIntake(double timeToMove) {
-
+ private void setSpeed(double speed) {
+ intakeWheel.set(speed);
}
- /**
- * Stops the intake
+ /***
+ * Runs the intake wheel at the set intake speed.
*/
- public void StopIntake() {
-
+ public void runIntake() {
+ setSpeed(INTAKE_SPEED);
}
- @Override
- protected void initDefaultCommand() {
- // TODO Auto-generated method stub
+ /***
+ * Stops the intake wheel by setting intake wheel's speed to 0.
+ */
+ public void stopIntake() {
+ setSpeed(0);
+ }
+ /***
+ * Purpose is to release all balls from the ball container to the outside of
+ * the robot. Reverses intake wheel by setting wheel speed to reverse speed.
+ *
+ */
+ public void runReverseIntake() {
+ setSpeed(REVERSE_SPEED);
}
}
package org.usfirst.frc.team3501.robot.subsystems;
-public class Shooter {
- public Shooter() {
-
- }
-
- /**
- * Stops fly wheel
- */
- public void stopFlywheel() {
-
- }
-
- /**
- * Runs the fly wheel at a given speed in () for input time in seconds
- *
- * @param speed
- * in ()
- * @param time
- * in seconds
- */
- public void runFlywheel(double speed, double time) {
-
- }
-
- /**
- * Stops index wheel
- */
- public void stopIndexWheel() {
-
- }
-
- /**
- * Runs index wheel at a given speed in () for input time in seconds
- *
- * @param speed
- * in ()
- * @param time
- * in seconds
- */
- public void runIndexWheel(double speed, double time) {
-
- }
-
- /**
- * Runs fly wheel continuously until ________
- */
- public void runFlywheelContinuous() {
-
- }
-
- /**
- * Runs index wheel continuously until ________
- */
- public void runIndexWheelContinuous() {
-
- }
+import org.usfirst.frc.team3501.robot.Constants;
+
+import com.ctre.CANTalon;
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+
+public class Shooter extends Subsystem {
+ private static Shooter shooter;
+ private final CANTalon flyWheel, indexWheel;
+
+ private Shooter() {
+ flyWheel = new CANTalon(Constants.Shooter.FLY_WHEEL);
+ indexWheel = new CANTalon(Constants.Shooter.INDEX_WHEEL);
+ }
+
+ /**
+ * Returns shooter object
+ *
+ * @return Shooter object
+ */
+ public static Shooter getShooter() {
+ if (shooter == null) {
+ shooter = new Shooter();
+ }
+ return shooter;
+ }
+
+ /**
+ * Sets fly wheel motor value to input.
+ *
+ * @param val
+ * motor value from -1 to 1(fastest forward)
+ */
+ public void setFlyWheelMotorVal(final double val) {
+ flyWheel.set(val);
+ }
+
+ /**
+ * Stops fly wheel motor.
+ */
+ public void stopFlyWheel() {
+ flyWheel.set(0);
+ }
+
+ /**
+ * Sets index wheel motor value to input.
+ *
+ * @param val
+ * motor value from -1 to 1(fastest forward)
+ */
+ public void setIndexWheelMotorVal(final double val) {
+ indexWheel.set(val);
+ }
+
+ /**
+ * Stops index wheel motor.
+ */
+ public void stopIndexWheel() {
+ indexWheel.set(0);
+ }
+
+ @Override
+ protected void initDefaultCommand() {
+
+ }
}
--- /dev/null
+package org.usfirst.frc.team3501.robot.utils;
+
+import java.util.TimerTask;
+
+import edu.wpi.first.wpilibj.I2C;
+import edu.wpi.first.wpilibj.Timer;
+
+/**
+ * BNO055 IMU for the FIRST Robotics Competition. References throughout the code
+ * are to the following sensor documentation: http://git.io/vuOl1
+ *
+ * To use the sensor, wire up to it over I2C on the roboRIO. Creating an
+ * instance of this class will cause communications with the sensor to being.All
+ * communications with the sensor occur in a separate thread from your robot
+ * code to avoid blocking the main robot program execution.
+ *
+ * Example: private static BNO055 imu;
+ *
+ * public Robot() { imu =
+ * BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS,
+ * BNO055.vector_type_t.VECTOR_EULER); }
+ *
+ * You can check the status of the sensor by using the following methods:
+ * isSensorPresent(); //Checks if the code can talk to the sensor over I2C // If
+ * this returns false, check your wiring. isInitialized(); //Checks if the
+ * sensor initialization has completed. // Initialization takes about 3 seconds.
+ * You won't get // position data back from the sensor until its init'd.
+ * isCalibrated(); //The BNO055 will return position data after its init'd, //
+ * but the position data may be inaccurate until all // required sensors report
+ * they are calibrated. Some // Calibration sequences require you to move the
+ * BNO055 // around. See the method comments for more info.
+ *
+ * Once the sensor calibration is complete , you can get position data by by
+ * using the getVector() method. See this method definiton for usage info.
+ *
+ * This code was originally ported from arduino source developed by Adafruit.
+ * See the original comment header below.
+ *
+ * @author james@team2168.org
+ *
+ *
+ * ORIGINAL ADAFRUIT HEADER -
+ * https://github.com/adafruit/Adafruit_BNO055/
+ * ====================================================================
+ * ===
+ * This is a library for the BNO055 orientation sensor
+ *
+ * Designed specifically to work with the Adafruit BNO055 Breakout.
+ *
+ * Pick one up today in the adafruit shop! ------>
+ * http://www.adafruit.com/products
+ *
+ * These sensors use I2C to communicate, 2 pins are required to
+ * interface.
+ *
+ * Adafruit invests time and resources providing this open source code,
+ * please support Adafruit and open-source hardware by purchasing
+ * products from Adafruit!
+ *
+ * Written by KTOWN for Adafruit Industries.
+ *
+ * MIT license, all text above must be included in any redistribution
+ *
+ */
+public class BNO055 {
+ // Tread variables
+ private java.util.Timer executor;
+ private static final long THREAD_PERIOD = 20; // ms - max poll rate on sensor.
+
+ public static final byte BNO055_ADDRESS_A = 0x28;
+ public static final byte BNO055_ADDRESS_B = 0x29;
+ public static final int BNO055_ID = 0xA0;
+
+ private static BNO055 instance;
+
+ private static I2C imu;
+ private static int _mode;
+ private static opmode_t requestedMode; // user requested mode of operation.
+ private static vector_type_t requestedVectorType;
+
+ // State machine variables
+ private volatile int state = 0;
+ private volatile boolean sensorPresent = false;
+ private volatile boolean initialized = false;
+ private volatile double currentTime; // seconds
+ private volatile double nextTime; // seconds
+ private volatile byte[] positionVector = new byte[6];
+ private volatile long turns = 0;
+ private volatile double[] xyz = new double[3];
+
+ private double zeroReferenceConst = 0;
+
+ public class SystemStatus {
+ public int system_status;
+ public int self_test_result;
+ public int system_error;
+ }
+
+ public enum reg_t {
+ /* Page id register definition */
+ BNO055_PAGE_ID_ADDR(0X07),
+
+ /* PAGE0 REGISTER DEFINITION START */
+ BNO055_CHIP_ID_ADDR(0x00), BNO055_ACCEL_REV_ID_ADDR(
+ 0x01), BNO055_MAG_REV_ID_ADDR(0x02), BNO055_GYRO_REV_ID_ADDR(
+ 0x03), BNO055_SW_REV_ID_LSB_ADDR(0x04), BNO055_SW_REV_ID_MSB_ADDR(
+ 0x05), BNO055_BL_REV_ID_ADDR(0X06),
+
+ /* Accel data register */
+ BNO055_ACCEL_DATA_X_LSB_ADDR(0X08), BNO055_ACCEL_DATA_X_MSB_ADDR(
+ 0X09), BNO055_ACCEL_DATA_Y_LSB_ADDR(0X0A), BNO055_ACCEL_DATA_Y_MSB_ADDR(
+ 0X0B), BNO055_ACCEL_DATA_Z_LSB_ADDR(
+ 0X0C), BNO055_ACCEL_DATA_Z_MSB_ADDR(0X0D),
+
+ /* Mag data register */
+ BNO055_MAG_DATA_X_LSB_ADDR(0X0E), BNO055_MAG_DATA_X_MSB_ADDR(
+ 0X0F), BNO055_MAG_DATA_Y_LSB_ADDR(0X10), BNO055_MAG_DATA_Y_MSB_ADDR(
+ 0X11), BNO055_MAG_DATA_Z_LSB_ADDR(
+ 0X12), BNO055_MAG_DATA_Z_MSB_ADDR(0X13),
+
+ /* Gyro data registers */
+ BNO055_GYRO_DATA_X_LSB_ADDR(0X14), BNO055_GYRO_DATA_X_MSB_ADDR(
+ 0X15), BNO055_GYRO_DATA_Y_LSB_ADDR(0X16), BNO055_GYRO_DATA_Y_MSB_ADDR(
+ 0X17), BNO055_GYRO_DATA_Z_LSB_ADDR(
+ 0X18), BNO055_GYRO_DATA_Z_MSB_ADDR(0X19),
+
+ /* Euler data registers */
+ BNO055_EULER_H_LSB_ADDR(0X1A), BNO055_EULER_H_MSB_ADDR(
+ 0X1B), BNO055_EULER_R_LSB_ADDR(0X1C), BNO055_EULER_R_MSB_ADDR(
+ 0X1D), BNO055_EULER_P_LSB_ADDR(0X1E), BNO055_EULER_P_MSB_ADDR(0X1F),
+
+ /* Quaternion data registers */
+ BNO055_QUATERNION_DATA_W_LSB_ADDR(0X20), BNO055_QUATERNION_DATA_W_MSB_ADDR(
+ 0X21), BNO055_QUATERNION_DATA_X_LSB_ADDR(
+ 0X22), BNO055_QUATERNION_DATA_X_MSB_ADDR(
+ 0X23), BNO055_QUATERNION_DATA_Y_LSB_ADDR(
+ 0X24), BNO055_QUATERNION_DATA_Y_MSB_ADDR(
+ 0X25), BNO055_QUATERNION_DATA_Z_LSB_ADDR(
+ 0X26), BNO055_QUATERNION_DATA_Z_MSB_ADDR(0X27),
+
+ /* Linear acceleration data registers */
+ BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR(
+ 0X28), BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR(
+ 0X29), BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR(
+ 0X2A), BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR(
+ 0X2B), BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR(
+ 0X2C), BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR(0X2D),
+
+ /* Gravity data registers */
+ BNO055_GRAVITY_DATA_X_LSB_ADDR(0X2E), BNO055_GRAVITY_DATA_X_MSB_ADDR(
+ 0X2F), BNO055_GRAVITY_DATA_Y_LSB_ADDR(
+ 0X30), BNO055_GRAVITY_DATA_Y_MSB_ADDR(
+ 0X31), BNO055_GRAVITY_DATA_Z_LSB_ADDR(
+ 0X32), BNO055_GRAVITY_DATA_Z_MSB_ADDR(0X33),
+
+ /* Temperature data register */
+ BNO055_TEMP_ADDR(0X34),
+
+ /* Status registers */
+ BNO055_CALIB_STAT_ADDR(0X35), BNO055_SELFTEST_RESULT_ADDR(
+ 0X36), BNO055_INTR_STAT_ADDR(0X37),
+
+ BNO055_SYS_CLK_STAT_ADDR(0X38), BNO055_SYS_STAT_ADDR(
+ 0X39), BNO055_SYS_ERR_ADDR(0X3A),
+
+ /* Unit selection register */
+ BNO055_UNIT_SEL_ADDR(0X3B), BNO055_DATA_SELECT_ADDR(0X3C),
+
+ /* Mode registers */
+ BNO055_OPR_MODE_ADDR(0X3D), BNO055_PWR_MODE_ADDR(0X3E),
+
+ BNO055_SYS_TRIGGER_ADDR(0X3F), BNO055_TEMP_SOURCE_ADDR(0X40),
+
+ /* Axis remap registers */
+ BNO055_AXIS_MAP_CONFIG_ADDR(0X41), BNO055_AXIS_MAP_SIGN_ADDR(0X42),
+
+ /* SIC registers */
+ BNO055_SIC_MATRIX_0_LSB_ADDR(0X43), BNO055_SIC_MATRIX_0_MSB_ADDR(
+ 0X44), BNO055_SIC_MATRIX_1_LSB_ADDR(0X45), BNO055_SIC_MATRIX_1_MSB_ADDR(
+ 0X46), BNO055_SIC_MATRIX_2_LSB_ADDR(
+ 0X47), BNO055_SIC_MATRIX_2_MSB_ADDR(
+ 0X48), BNO055_SIC_MATRIX_3_LSB_ADDR(
+ 0X49), BNO055_SIC_MATRIX_3_MSB_ADDR(
+ 0X4A), BNO055_SIC_MATRIX_4_LSB_ADDR(
+ 0X4B), BNO055_SIC_MATRIX_4_MSB_ADDR(
+ 0X4C), BNO055_SIC_MATRIX_5_LSB_ADDR(
+ 0X4D), BNO055_SIC_MATRIX_5_MSB_ADDR(
+ 0X4E), BNO055_SIC_MATRIX_6_LSB_ADDR(
+ 0X4F), BNO055_SIC_MATRIX_6_MSB_ADDR(
+ 0X50), BNO055_SIC_MATRIX_7_LSB_ADDR(
+ 0X51), BNO055_SIC_MATRIX_7_MSB_ADDR(
+ 0X52), BNO055_SIC_MATRIX_8_LSB_ADDR(
+ 0X53), BNO055_SIC_MATRIX_8_MSB_ADDR(
+ 0X54),
+
+ /* Accelerometer Offset registers */
+ ACCEL_OFFSET_X_LSB_ADDR(0X55), ACCEL_OFFSET_X_MSB_ADDR(
+ 0X56), ACCEL_OFFSET_Y_LSB_ADDR(0X57), ACCEL_OFFSET_Y_MSB_ADDR(
+ 0X58), ACCEL_OFFSET_Z_LSB_ADDR(0X59), ACCEL_OFFSET_Z_MSB_ADDR(0X5A),
+
+ /* Magnetometer Offset registers */
+ MAG_OFFSET_X_LSB_ADDR(0X5B), MAG_OFFSET_X_MSB_ADDR(
+ 0X5C), MAG_OFFSET_Y_LSB_ADDR(0X5D), MAG_OFFSET_Y_MSB_ADDR(
+ 0X5E), MAG_OFFSET_Z_LSB_ADDR(0X5F), MAG_OFFSET_Z_MSB_ADDR(0X60),
+
+ /* Gyroscope Offset register s */
+ GYRO_OFFSET_X_LSB_ADDR(0X61), GYRO_OFFSET_X_MSB_ADDR(
+ 0X62), GYRO_OFFSET_Y_LSB_ADDR(0X63), GYRO_OFFSET_Y_MSB_ADDR(
+ 0X64), GYRO_OFFSET_Z_LSB_ADDR(0X65), GYRO_OFFSET_Z_MSB_ADDR(0X66),
+
+ /* Radius registers */
+ ACCEL_RADIUS_LSB_ADDR(0X67), ACCEL_RADIUS_MSB_ADDR(
+ 0X68), MAG_RADIUS_LSB_ADDR(0X69), MAG_RADIUS_MSB_ADDR(0X6A);
+
+ private final int val;
+
+ reg_t(int val) {
+ this.val = val;
+ }
+
+ public int getVal() {
+ return val;
+ }
+ };
+
+ public enum powermode_t {
+ POWER_MODE_NORMAL(0X00), POWER_MODE_LOWPOWER(0X01), POWER_MODE_SUSPEND(
+ 0X02);
+
+ private final int val;
+
+ powermode_t(int val) {
+ this.val = val;
+ }
+
+ public int getVal() {
+ return val;
+ }
+ };
+
+ public enum opmode_t {
+ /* Operation mode settings */
+ OPERATION_MODE_CONFIG(0X00), OPERATION_MODE_ACCONLY(
+ 0X01), OPERATION_MODE_MAGONLY(0X02), OPERATION_MODE_GYRONLY(
+ 0X03), OPERATION_MODE_ACCMAG(0X04), OPERATION_MODE_ACCGYRO(
+ 0X05), OPERATION_MODE_MAGGYRO(
+ 0X06), OPERATION_MODE_AMG(0X07), OPERATION_MODE_IMUPLUS(
+ 0X08), OPERATION_MODE_COMPASS(0X09), OPERATION_MODE_M4G(
+ 0X0A), OPERATION_MODE_NDOF_FMC_OFF(
+ 0X0B), OPERATION_MODE_NDOF(0X0C);
+
+ private final int val;
+
+ opmode_t(int val) {
+ this.val = val;
+ }
+
+ public int getVal() {
+ return val;
+ }
+ }
+
+ public class RevInfo {
+ public byte accel_rev;
+ public byte mag_rev;
+ public byte gyro_rev;
+ public short sw_rev;
+ public byte bl_rev;
+ }
+
+ public class CalData {
+ public byte sys;
+ public byte gyro;
+ public byte accel;
+ public byte mag;
+ }
+
+ public enum vector_type_t {
+ VECTOR_ACCELEROMETER(
+ reg_t.BNO055_ACCEL_DATA_X_LSB_ADDR.getVal()), VECTOR_MAGNETOMETER(
+ reg_t.BNO055_MAG_DATA_X_LSB_ADDR.getVal()), VECTOR_GYROSCOPE(
+ reg_t.BNO055_GYRO_DATA_X_LSB_ADDR.getVal()), VECTOR_EULER(
+ reg_t.BNO055_EULER_H_LSB_ADDR.getVal()), VECTOR_LINEARACCEL(
+ reg_t.BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR
+ .getVal()), VECTOR_GRAVITY(
+ reg_t.BNO055_GRAVITY_DATA_X_LSB_ADDR.getVal());
+
+ private final int val;
+
+ vector_type_t(int val) {
+ this.val = val;
+ }
+
+ public int getVal() {
+ return val;
+ }
+ };
+
+ /**
+ * Instantiates a new BNO055 class.
+ *
+ * @param port
+ * the physical port the sensor is plugged into on the roboRio
+ * @param address
+ * the address the sensor is at (0x28 or 0x29)
+ */
+ private BNO055(I2C.Port port, byte address) {
+ imu = new I2C(port, address);
+
+ executor = new java.util.Timer();
+ executor.schedule(new BNO055UpdateTask(this), 0L, THREAD_PERIOD);
+ }
+
+ /**
+ * Get an instance of the IMU object.
+ *
+ * @param mode
+ * the operating mode to run the sensor in.
+ * @param port
+ * the physical port the sensor is plugged into on the roboRio
+ * @param address
+ * the address the sensor is at (0x28 or 0x29)
+ * @return the instantiated BNO055 object
+ */
+ public static BNO055 getInstance(opmode_t mode, vector_type_t vectorType,
+ I2C.Port port, byte address) {
+
+ if (instance == null) {
+
+ instance = new BNO055(port, address);
+ }
+
+ requestedMode = mode;
+ requestedVectorType = vectorType;
+ return instance;
+ }
+
+ /**
+ * Get an instance of the IMU object plugged into the onboard I2C header.
+ * Using the default address (0x28)
+ *
+ * @param mode
+ * the operating mode to run the sensor in.
+ * @param vectorType
+ * the format the position vector data should be returned in (if you
+ * don't know use VECTOR_EULER).
+ * @return the instantiated BNO055 object
+ */
+ public static BNO055 getInstance(opmode_t mode, vector_type_t vectorType) {
+ return getInstance(mode, vectorType, I2C.Port.kOnboard, BNO055_ADDRESS_A);
+ }
+
+ /**
+ * Called periodically. Communicates with the sensor, and checks its state.
+ */
+ private void update() {
+ currentTime = Timer.getFPGATimestamp(); // seconds
+ if (!initialized) {
+ // System.out.println("State: " + state + ". curr: " + currentTime
+ // + ", next: " + nextTime);
+
+ // Step through process of initializing the sensor in a non-
+ // blocking manner. This sequence of events follows the process
+ // defined in the original adafruit source as closely as possible.
+ // XXX: It's likely some of these delays can be optimized out.
+ switch (state) {
+ case 0:
+ // Wait for the sensor to be present
+ if ((0xFF & read8(reg_t.BNO055_CHIP_ID_ADDR)) != BNO055_ID) {
+ // Sensor not present, keep trying
+ sensorPresent = false;
+ } else {
+ // Sensor present, go to next state
+ sensorPresent = true;
+ state++;
+ nextTime = Timer.getFPGATimestamp() + 0.050;
+ }
+ break;
+ case 1:
+ if (currentTime >= nextTime) {
+ // Switch to config mode (just in case since this is the default)
+ setMode(opmode_t.OPERATION_MODE_CONFIG.getVal());
+ nextTime = Timer.getFPGATimestamp() + 0.050;
+ state++;
+ }
+ break;
+ case 2:
+ // Reset
+ if (currentTime >= nextTime) {
+ write8(reg_t.BNO055_SYS_TRIGGER_ADDR, (byte) 0x20);
+ state++;
+ }
+ break;
+ case 3:
+ // Wait for the sensor to be present
+ if ((0xFF & read8(reg_t.BNO055_CHIP_ID_ADDR)) == BNO055_ID) {
+ // Sensor present, go to next state
+ state++;
+ // Log current time
+ nextTime = Timer.getFPGATimestamp() + 0.050;
+ }
+ break;
+ case 4:
+ // Wait at least 50ms
+ if (currentTime >= nextTime) {
+ /* Set to normal power mode */
+ write8(reg_t.BNO055_PWR_MODE_ADDR,
+ (byte) powermode_t.POWER_MODE_NORMAL.getVal());
+ nextTime = Timer.getFPGATimestamp() + 0.050;
+ state++;
+ }
+ break;
+ case 5:
+ // Use external crystal - 32.768 kHz
+ if (currentTime >= nextTime) {
+ write8(reg_t.BNO055_PAGE_ID_ADDR, (byte) 0x00);
+ nextTime = Timer.getFPGATimestamp() + 0.050;
+ state++;
+ }
+ break;
+ case 6:
+ if (currentTime >= nextTime) {
+ write8(reg_t.BNO055_SYS_TRIGGER_ADDR, (byte) 0x80);
+ nextTime = Timer.getFPGATimestamp() + 0.500;
+ state++;
+ }
+ break;
+ case 7:
+ // Set operating mode to mode requested at instantiation
+ if (currentTime >= nextTime) {
+ setMode(requestedMode);
+ nextTime = Timer.getFPGATimestamp() + 1.05;
+ state++;
+ }
+ break;
+ case 8:
+ if (currentTime >= nextTime) {
+ nextTime = Timer.getFPGATimestamp() + 1.05;
+ state++;
+ }
+ case 9:
+ if (currentTime >= nextTime) {
+ calculateVector();
+ zeroReferenceConst = getDefaultHeading();
+ initialized = true;
+ }
+ break;
+ default:
+ // Should never get here - Fail safe
+ initialized = false;
+ }
+ } else {
+ // Sensor is initialized, periodically query position data
+ calculateVector();
+ }
+ }
+
+ /**
+ * Query the sensor for position data.
+ */
+ private void calculateVector() {
+ double[] pos = new double[3];
+ short x = 0, y = 0, z = 0;
+ double headingDiff = 0.0;
+
+ // Read vector data (6 bytes)
+ readLen(requestedVectorType.getVal(), positionVector);
+
+ x = (short) ((positionVector[0] & 0xFF)
+ | ((positionVector[1] << 8) & 0xFF00));
+ y = (short) ((positionVector[2] & 0xFF)
+ | ((positionVector[3] << 8) & 0xFF00));
+ z = (short) ((positionVector[4] & 0xFF)
+ | ((positionVector[5] << 8) & 0xFF00));
+
+ /* Convert the value to an appropriate range (section 3.6.4) */
+ /* and assign the value to the Vector type */
+ switch (requestedVectorType) {
+ case VECTOR_MAGNETOMETER:
+ /* 1uT = 16 LSB */
+ pos[0] = (x) / 16.0;
+ pos[1] = (y) / 16.0;
+ pos[2] = (z) / 16.0;
+ break;
+ case VECTOR_GYROSCOPE:
+ /* 1rps = 900 LSB */
+ pos[0] = (x) / 900.0;
+ pos[1] = (y) / 900.0;
+ pos[2] = (z) / 900.0;
+ break;
+ case VECTOR_EULER:
+ /* 1 degree = 16 LSB */
+ pos[0] = (x) / 16.0;
+ pos[1] = (y) / 16.0;
+ pos[2] = (z) / 16.0;
+ break;
+ case VECTOR_ACCELEROMETER:
+ case VECTOR_LINEARACCEL:
+ case VECTOR_GRAVITY:
+ /* 1m/s^2 = 100 LSB */
+ pos[0] = (x) / 100.0;
+ pos[1] = (y) / 100.0;
+ pos[2] = (z) / 100.0;
+ break;
+ }
+
+ // calculate turns
+ headingDiff = xyz[0] - pos[0];
+ if (Math.abs(headingDiff) >= 350) {
+ // We've traveled past the zero heading position
+ if (headingDiff > 0) {
+ turns++;
+ } else {
+ turns--;
+ }
+ }
+
+ // Update position vectors
+ xyz = pos;
+ }
+
+ /**
+ * Puts the chip in the specified operating mode
+ *
+ * @param mode
+ */
+ public void setMode(opmode_t mode) {
+ setMode(mode.getVal());
+ }
+
+ private void setMode(int mode) {
+ _mode = mode;
+ write8(reg_t.BNO055_OPR_MODE_ADDR, (byte) _mode);
+ }
+
+ /**
+ * Gets the latest system status info
+ *
+ * @return
+ */
+ public SystemStatus getSystemStatus() {
+ SystemStatus status = new SystemStatus();
+
+ write8(reg_t.BNO055_PAGE_ID_ADDR, (byte) 0x00);
+
+ /*
+ * System Status (see section 4.3.58) --------------------------------- 0 =
+ * Idle 1 = System Error 2 = Initializing Peripherals 3 = System
+ * Initalization 4 = Executing Self-Test 5 = Sensor fusion algorithm running
+ * 6 = System running without fusion algorithms
+ */
+
+ status.system_status = read8(reg_t.BNO055_SYS_STAT_ADDR);
+
+ /*
+ * Self Test Results (see section ) -------------------------------- 1 =
+ * test passed, 0 = test failed
+ *
+ * Bit 0 = Accelerometer self test Bit 1 = Magnetometer self test Bit 2 =
+ * Gyroscope self test Bit 3 = MCU self test
+ *
+ * 0x0F = all good!
+ */
+
+ status.self_test_result = read8(reg_t.BNO055_SELFTEST_RESULT_ADDR);
+
+ /*
+ * System Error (see section 4.3.59) --------------------------------- 0 =
+ * No error 1 = Peripheral initialization error 2 = System initialization
+ * error 3 = Self test result failed 4 = Register map value out of range 5 =
+ * Register map address out of range 6 = Register map write error 7 = BNO
+ * low power mode not available for selected operation mode 8 =
+ * Accelerometer power mode not available 9 = Fusion algorithm configuration
+ * error A = Sensor configuration error
+ */
+ status.system_error = read8(reg_t.BNO055_SYS_ERR_ADDR);
+ return status;
+ }
+
+ /**
+ * Gets the chip revision numbers
+ *
+ * @return the chips revision information
+ */
+ public RevInfo getRevInfo() {
+ int a = 0, b = 0;
+ RevInfo info = new RevInfo();
+
+ /* Check the accelerometer revision */
+ info.accel_rev = read8(reg_t.BNO055_ACCEL_REV_ID_ADDR);
+
+ /* Check the magnetometer revision */
+ info.mag_rev = read8(reg_t.BNO055_MAG_REV_ID_ADDR);
+
+ /* Check the gyroscope revision */
+ info.gyro_rev = read8(reg_t.BNO055_GYRO_REV_ID_ADDR);
+
+ /* Check the SW revision */
+ info.bl_rev = read8(reg_t.BNO055_BL_REV_ID_ADDR);
+
+ a = read8(reg_t.BNO055_SW_REV_ID_LSB_ADDR);
+ b = read8(reg_t.BNO055_SW_REV_ID_MSB_ADDR);
+ info.sw_rev = (short) ((b << 8) | a);
+
+ return info;
+ }
+
+ /**
+ * Diagnostic method to determine if communications with the sensor are
+ * active. Note this method returns true after first establishing
+ * communications with the sensor. Communications are not actively monitored
+ * once sensor initialization has started.
+ *
+ * @return true if the sensor is found on the I2C bus
+ */
+ public boolean isSensorPresent() {
+ return sensorPresent;
+ }
+
+ /**
+ * After power is applied, the sensor needs to be configured for use. During
+ * this initialization period the sensor will not return position vector data.
+ * Once initialization is complete, data can be read, although the sensor may
+ * not have completed calibration. See isCalibrated.
+ *
+ * @return true when the sensor is initialized.
+ */
+ public boolean isInitialized() {
+ return initialized;
+ }
+
+ /**
+ * Gets current calibration state.
+ *
+ * @return each value will be set to 0 if not calibrated, 3 if fully
+ * calibrated.
+ */
+ public CalData getCalibration() {
+ CalData data = new CalData();
+ int rawCalData = read8(reg_t.BNO055_CALIB_STAT_ADDR);
+
+ data.sys = (byte) ((rawCalData >> 6) & 0x03);
+ data.gyro = (byte) ((rawCalData >> 4) & 0x03);
+ data.accel = (byte) ((rawCalData >> 2) & 0x03);
+ data.mag = (byte) (rawCalData & 0x03);
+
+ return data;
+ }
+
+ /**
+ * Returns true if all required sensors (accelerometer, magnetometer,
+ * gyroscope) have completed their respective calibration sequence. Only
+ * sensors required by the current operating mode are checked. See Section
+ * 3.3.
+ *
+ * @return true if calibration is complete for all sensors required for the
+ * mode the sensor is currently operating in.
+ */
+ public boolean isCalibrated() {
+ boolean retVal = true;
+
+ // Per Table 3-3
+ boolean[][] sensorModeMap = new boolean[][] {
+ // {accel, mag, gyro}
+ { false, false, false }, // OPERATION_MODE_CONFIG
+ { true, false, false }, // OPERATION_MODE_ACCONLY
+ { false, true, false }, // OPERATION_MODE_MAGONLY
+ { false, false, true }, // OPERATION_MODE_GYRONLY
+ { true, true, false }, // OPERATION_MODE_ACCMAG
+ { true, false, true }, // OPERATION_MODE_ACCGYRO
+ { false, true, true }, // OPERATION_MODE_MAGGYRO
+ { true, true, true }, // OPERATION_MODE_AMG
+ { true, false, true }, // OPERATION_MODE_IMUPLUS
+ { true, true, false }, // OPERATION_MODE_COMPASS
+ { true, true, false }, // OPERATION_MODE_M4G
+ { true, true, true }, // OPERATION_MODE_NDOF_FMC_OFF
+ { true, true, true } // OPERATION_MODE_NDOF
+ };
+
+ CalData data = getCalibration();
+
+ if (sensorModeMap[_mode][0]) // Accelerometer used
+ retVal = retVal && (data.accel >= 3);
+ if (sensorModeMap[_mode][1]) // Magnetometer used
+ retVal = retVal && (data.mag >= 3);
+ if (sensorModeMap[_mode][2]) // Gyroscope used
+ retVal = retVal && (data.gyro >= 3);
+
+ return retVal;
+ }
+
+ /**
+ * Get the sensors internal temperature.
+ *
+ * @return temperature in degrees celsius.
+ */
+ public int getTemp() {
+ return (read8(reg_t.BNO055_TEMP_ADDR));
+ }
+
+ /**
+ * Gets a vector representing the sensors position (heading, roll, pitch).
+ * heading: 0 to 360 degrees roll: -90 to +90 degrees pitch: -180 to +180
+ * degrees
+ *
+ * For continuous rotation heading (doesn't roll over between 360/0) see the
+ * getHeading() method.
+ *
+ * Maximum data output rates for Fusion modes - See 3.6.3
+ *
+ * Operating Mode Data Output Rate IMU 100 Hz COMPASS 20 Hz M4G 50 Hz
+ * NDOF_FMC_OFF 100 Hz NDOF 100 Hz
+ *
+ * @return a vector [heading, roll, pitch]
+ */
+ public double[] getVector() {
+ return xyz;
+ }
+
+ /**
+ * The default sensor heading not relative to the starting angle of the robot.
+ *
+ * @return
+ */
+ public double getDefaultHeading() {
+ return xyz[0] + turns * 360;
+ }
+
+ /**
+ * The heading of the sensor (x axis) in continuous format relative to the
+ * initial heading of the robot. Eg rotating the sensor clockwise two full
+ * rotations will return a value of 720 degrees. The getVector method will
+ * return heading in a constrained 0 - 360 deg format if required.
+ *
+ * @return heading in degrees
+ */
+ public double getHeading() {
+ double reading = getDefaultHeading();
+
+ return reading - zeroReferenceConst;
+ }
+
+ /**
+ * Writes an 8 bit value over I2C
+ *
+ * @param reg
+ * the register to write the data to
+ * @param value
+ * a byte of data to write
+ * @return whatever I2CJNI.i2CWrite returns. It's not documented in the wpilib
+ * javadocs!
+ */
+ private boolean write8(reg_t reg, byte value) {
+ boolean retVal = false;
+
+ retVal = imu.write(reg.getVal(), value);
+
+ return retVal;
+ }
+
+ /**
+ * Reads an 8 bit value over I2C
+ *
+ * @param reg
+ * the register to read from.
+ * @return
+ */
+ private byte read8(reg_t reg) {
+ byte[] vals = new byte[1];
+
+ readLen(reg, vals);
+ return vals[0];
+ }
+
+ /**
+ * Reads the specified number of bytes over I2C
+ *
+ * @param reg
+ * the address to read from
+ * @param buffer
+ * to store the read data into
+ * @return true on success
+ */
+ private boolean readLen(reg_t reg, byte[] buffer) {
+ return readLen(reg.getVal(), buffer);
+ }
+
+ /**
+ * Reads the specified number of bytes over I2C
+ *
+ * @param reg
+ * the address to read from
+ * @param buffer
+ * the size of the data to read
+ * @return true on success
+ */
+ private boolean readLen(int reg, byte[] buffer) {
+ boolean retVal = true;
+
+ if (buffer == null || buffer.length < 1) {
+ return false;
+ }
+
+ retVal = !imu.read(reg, buffer.length, buffer);
+
+ return retVal;
+ }
+
+ private class BNO055UpdateTask extends TimerTask {
+ private BNO055 imu;
+
+ private BNO055UpdateTask(BNO055 imu) {
+ if (imu == null) {
+ throw new NullPointerException("BNO055 pointer null");
+ }
+ this.imu = imu;
+ }
+
+ /**
+ * Called periodically in its own thread
+ */
+ @Override
+ public void run() {
+ imu.update();
+ }
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.utils;
+
+import org.usfirst.frc.team3501.robot.MathLib;
+
+public class PIDController {
+
+ private double pConst;
+ private double iConst;
+ private double dConst;
+ private double setPoint;
+ private double previousError;
+ private double errorSum;
+ private double errorIncrement;
+ private double iRange;
+ private double doneRange;
+ private boolean firstCycle;
+ private double maxOutput;
+ private int minDoneCycleCount;
+ private int doneCycleCount;
+
+ public PIDController() {
+ this(0.0, 0.0, 0.0, 0.0, 0.0);
+ }
+
+ public PIDController(double p, double i, double d, double eps,
+ double iRange) {
+ this.pConst = p;
+ this.iConst = i;
+ this.dConst = d;
+ this.setPoint = 0.0;
+
+ this.iRange = iRange;
+ this.errorIncrement = 1.0;
+
+ this.doneRange = eps;
+ this.doneCycleCount = 0;
+ this.minDoneCycleCount = 5;
+
+ this.firstCycle = true;
+
+ this.maxOutput = 1.0;
+
+ }
+
+ public PIDController(double p, double i, double d, double eps) {
+ this(p, i, d, eps, eps * 0.8);
+ }
+
+ public PIDController(double p, double i, double d) {
+ this(p, i, d, 1.0);
+ }
+
+ public void setConstants(double p, double i, double d) {
+ this.pConst = p;
+ this.iConst = i;
+ this.dConst = d;
+ }
+
+ public void setConstants(double p, double i, double d, double eps,
+ double iRange) {
+ this.pConst = p;
+ this.iConst = i;
+ this.dConst = d;
+ this.doneRange = eps; // range of error
+ this.iRange = iRange;
+ }
+
+ public void setConstants(double p, double i, double d, double eps) {
+ setConstants(p, i, d, eps, eps * 0.8);
+ }
+
+ public void setDoneRange(double range) {
+ this.doneRange = range;
+ }
+
+ public void setIRange(double eps) {
+ this.iRange = eps;
+ }
+
+ public void setSetPoint(double val) {
+ this.setPoint = val;
+ }
+
+ public void setMaxOutput(double max) {
+ if (max < 0.0) {
+ this.maxOutput = 0.0;
+ } else if (max > 1.0) {
+ this.maxOutput = 1.0;
+ } else {
+ this.maxOutput = max;
+ }
+ }
+
+ public void setMinDoneCycles(int num) {
+ this.minDoneCycleCount = num;
+ }
+
+ public void resetErrorSum() {
+ this.errorSum = 0.0;
+ }
+
+ public double getDesiredVal() {
+ return this.setPoint;
+ }
+
+ public double calcPID(double current) {
+ return calcPIDError(this.setPoint - current);
+ }
+
+ public double calcPIDError(double error) {
+ double pVal = 0.0;
+ double iVal = 0.0;
+ double dVal = 0.0;
+
+ if (this.firstCycle) {
+ this.previousError = error;
+ this.firstCycle = false;
+ }
+
+ pVal = this.pConst * error;
+
+ // + error outside of acceptable range which might distort the sum calc
+ if (error > this.iRange) {
+ // check if error sum was in the wrong direction
+ if (this.errorSum < 0.0) {
+ this.errorSum = 0.0;
+ }
+ // only allow up to the max contribution per cycle
+ this.errorSum += Math.min(error, this.errorIncrement);
+ } // - error outside of acceptable range
+ else if (error < -1.0 * this.iRange) {
+ // error sum was in the wrong direction
+ if (this.errorSum > 0.0) {
+ this.errorSum = 0.0;
+ }
+ // add either the full error or the max allowable amount to sum
+ this.errorSum += Math.max(error, -1.0 * this.errorIncrement);
+ }
+
+ // i contribution (final) calculation
+ iVal = this.iConst * this.errorSum;
+
+ // /////D Calc///////
+ double deriv = error - this.previousError;
+ dVal = this.dConst * deriv;
+
+ // overal PID calc
+ double output = pVal + iVal + dVal;
+
+ // limit the output
+ output = MathLib.limitValue(output, this.maxOutput);
+
+ // store current value as previous for next cycle
+ this.previousError = error;
+ return output;
+ }
+
+ public boolean isDone() {
+ double currError = Math.abs(this.previousError);
+
+ // close enough to target
+ if (currError <= this.doneRange) {
+ this.doneCycleCount++;
+ }
+ // not close enough to target
+ else {
+ this.doneCycleCount = 0;
+ }
+
+ return this.doneCycleCount >= this.minDoneCycleCount;
+ }
+}
projects / 3501 / 2017steamworks / commitdiff