public static void init() {
// This MUST be here. If the OI creates Commands (which it very likely
// will), constructing it during the construction of CommandBase (from
// which commands extend), subsystems are not guaranteed to be
// yet. Thus, their requires() statements may grab null pointers. Bad
// news. Don't move it.
chassis =
new Chassis(
RobotMap.FRONT_LEFT, RobotMap.FRONT_RIGHT, RobotMap.BACK_LEFT, RobotMap.BACK_RIGHT);
sensorBase = new SensorBase();
// NEW ARM
arm = new PotArm(0.065, 0, 0.001);
arm.enable();
arm.setSetpoint(CommandBase.sensorBase.getElbowAngle());
SmartDashboard.putInt("Manual Arm Elbow", 0);
// arm = new Arm(RobotMap.ELBOW_MOTOR, RobotMap.WRIST_MOTOR);
// OLD ARM
// DONT FORGET TO .enable() ANY SUBSYSTEM WITH PID!!!!!!!!!!!!!!!!!!
// PIDElbow = new PIDArmElbow(.055, 0, 0);
// PIDElbow.enable();
// PIDElbow.setAbsoluteTolerance(4);
PIDWrist = new PIDArmWrist(.05, .001, 0);
PIDWrist.enable();
PIDWrist.setAbsoluteTolerance(.5);
SmartDashboard.putInt("Manual Wrist Angle", 0);
// OI always instantiated LAST!!!
oi = new OI();
}
// Called just before this Command runs the first time
protected void initialize() {
if (SmartDashboard.getNumber("centerAge", 1) < 0.5)
angle = SmartDashboard.getNumber("angleToCenter", 0);
else angle = 0;
super.initialize();
// System.out.println("Angle to Center: " + angle);
}
@Override
public void run() {
m_visionSystemReceiver = new JReceiver();
m_visionSystemTargetInfo = new JTargetInfo();
m_visionSystemReceiver.init();
SmartDashboard.putNumber("visionSystemPixelX", Robot.g_visionSystemPixelX);
String textInput;
m_continueRunning = true;
while (m_continueRunning) {
textInput = m_visionSystemReceiver.getOneLineFromSocket();
if (textInput != null) {
Robot.g_nSequenceVisionSystem++;
SmartDashboard.putNumber("visionSystemPixelX", Robot.g_visionSystemPixelX);
// System.out.println(textInput);
Robot.g_isVisionSystemGoalDetected = m_visionSystemTargetInfo.m_isUpperGoalFound;
Robot.g_visionSystemAngleRobotToGoal =
m_visionSystemTargetInfo.m_angleFromStraightAheadToUpperGoal;
Robot.g_visionSystemPixelX = m_visionSystemTargetInfo.m_pixelX;
SmartDashboard.putBoolean("isVisionSystemGoalDetected", Robot.g_isVisionSystemGoalDetected);
SmartDashboard.putNumber(
"visionSystemAngleRobotToGoal", Robot.g_visionSystemAngleRobotToGoal);
SmartDashboard.putNumber("visionSystemPixelX", Robot.g_visionSystemPixelX);
} else {
Robot.g_isVisionSystemGoalDetected = false;
}
}
}
/** The log method puts interesting information to the SmartDashboard. */
public void log() {
SmartDashboard.putNumber("Left Distance", left_encoder.getDistance());
SmartDashboard.putNumber("Right Distance", right_encoder.getDistance());
SmartDashboard.putNumber("Left Speed", left_encoder.getRate());
SmartDashboard.putNumber("Right Speed", right_encoder.getRate());
SmartDashboard.putNumber("Gyro", gyro.getAngle());
}
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
public void robotInit() {
RobotMap.init();
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
chassisrotate = new Chassisrotate();
vision = new Vision();
chassis = new Chassis();
elevator = new Elevator();
shooter = new Shooter();
pickupBelt = new PickupBelt();
pickupShaft = new PickupShaft();
climber = new Climber();
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
autoChooser = new SendableChooser();
autoChooser.addDefault("Autonomous Position 1", new AutonomousCommand1());
autoChooser.addObject("Autonomous Position 2", new AutonomousCommand2());
SmartDashboard.putData("Autonomous Mode Chooser", autoChooser);
// This MUST be here. If the OI creates Commands (which it very likely
// will), constructing it during the construction of CommandBase (from
// which commands extend), subsystems are not guaranteed to be
// yet. Thus, their requires() statements may grab null pointers. Bad
// news. Don't move it.
oi = new OI();
SmartDashboard.putData(Robot.chassis);
// instantiate the command used for the autonomous period
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=AUTONOMOUS
autonomousCommand = new AutonomousCommand1();
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=AUTONOMOUS
}
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
public void robotInit() {
chooser = new SendableChooser();
chooser.addDefault("Default Auto", defaultAuto);
chooser.addObject("My Auto", customAuto);
SmartDashboard.putData("Auto choices", chooser);
controlMethod = new SendableChooser();
controlMethod.addDefault("Tank Control", tankControl);
controlMethod.addObject("Stick Control", stickControl);
controlMethod.addObject("Clayton Control", claytonControl);
SmartDashboard.putData("Control method", controlMethod);
rightMotor = new CANTalon(RIGHT_INDEX);
rightSlave = new CANTalon(RIGHT_INDEX + 1);
rightSlave.changeControlMode(CANTalon.TalonControlMode.Follower);
rightSlave.set(RIGHT_INDEX);
rightMotor.setInverted(true);
leftMotor = new CANTalon(LEFT_INDEX);
leftSlave = new CANTalon(LEFT_INDEX + 1);
leftSlave.changeControlMode(CANTalon.TalonControlMode.Follower);
leftSlave.set(LEFT_INDEX);
intakeMotor = new CANTalon(INTAKE_INDEX);
// Arm motor is currently disabled
// armMotor = new CANTalon(ARM_INDEX);
compressor = new Compressor(0);
compressor.setClosedLoopControl(true);
intakePneumatic = new DoubleSolenoid(1, 2);
intakePneumatic.set(DoubleSolenoid.Value.kOff);
gamepad = new Gamepad(0);
}
public void autoDrive() {
double speed = 0;
if (distPID.isEnable()) {
speed = distPID.get();
}
double turn = 0;
if (turnPID.isEnable()) {
turn = turnPID.get();
}
if (speed > 0) {
drive.drive(speed, turn);
} else {
if (turn > 0) {
// turn right
tankDrive(turn, -turn);
} else if (turn < 0) {
tankDrive(-turn, turn);
}
}
SmartDashboard.putDouble("left pwm", leftJag.get());
SmartDashboard.putDouble("right pwm", rightJag.get());
SmartDashboard.putDouble("speed", RobotMap.roundtoTwo(speed));
SmartDashboard.putDouble("turn", RobotMap.roundtoTwo(turn));
SmartDashboard.putDouble("turn error", turnPID.getError());
// System.out.println("driving " + speed + " " + turn);
}
/* (non-Javadoc)
* @see java.lang.Runnable#run()
*/
@Override
public void run() {
SmartDashboard.putNumber("X Acceleration", accelerometer.accelerationX());
SmartDashboard.putNumber("Y Acceleration", accelerometer.accelerationY());
SmartDashboard.putNumber("Z Acceleration", accelerometer.accelerationZ());
SmartDashboard.putNumber("Heading", gyro.heading());
}
/** This function is called periodically during operator control */
public void teleopPeriodic() {
Scheduler.getInstance().run();
if (RobotMap.debugMode) {
SmartDashboard.putData("Drive Train", driveTrain);
SmartDashboard.putData("Elevator", elevator);
}
}
/** This function is called periodically during operator control */
public void teleopPeriodic() {
// The order of the update methods is important. Besides making a nice slope of periods, the
// motors and sensors have to update first
controller.update(motorManagers);
sensors.update();
shoot.update(motorManagers, controller, sensors, SSS);
arm.update(motorManagers, controller);
// System.out.println("Encoder Raw: " + sensors.getEncoderValues());
// System.out.println("Encoder Count: " + sensors.getEncoderCount());
// System.out.println("Encoder Rate of Rotation: " + sensors.getEncoderRate());
// System.out.println("Encoder Distance: " + sensors.getEncoderDistance());
// System.out.println("Degrees per Second: " + sensors.getEncoderAngularSpeed());
// System.out.println("StringPot: " + sensors.getStringPot());
System.out.println("Ultrasonic Range Finder (Inches): " + sensors.getSonicRangeInches());
// System.out.println("Servo: " + shoot.pusher.getAngle());
sensors.resetGyroAngles(controller);
System.out.println("Gyro XY: " + sensors.getGyroXYAngle());
System.out.println("Gyro Z: " + sensors.getGyroZAngle());
System.out.println("wheel Angle: " + sensors.encoder.getEncoderAngle());
SmartDashboard.putNumber("Gyro XY", sensors.getGyroXYAngle());
SmartDashboard.putNumber("Gyro Z", sensors.getGyroZAngle());
SmartDashboard.putNumber("Distance", sensors.getSonicRangeInches());
}
public void displayShooterWheelsDataSD() {
SmartDashboard.putNumber(
"ShooterWheels_LeftShooterEncoder", Robot.shooterWheels.returnPIDInputLeft());
SmartDashboard.putNumber(
"ShooterWheels_RightShooterEncoder", Robot.shooterWheels.returnPIDInputRight());
SmartDashboard.putNumber("ShooterWheels_Setpoint", Robot.shooterWheels.getSetpoint());
}
// basic arcadeDrive: y=forward/backward speed, x=left/right speed
public void arcadeDrive(double y, double x) {
SmartDashboard.putNumber("RightEncoder", rightEncoder.get());
SmartDashboard.putNumber("LeftEncoder", leftEncoder.get());
Preferences p = Preferences.getInstance();
final boolean kReverseDirection = p.getBoolean("DriveTrainReverseDirection", false);
robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, kReverseDirection);
robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, kReverseDirection);
robotDrive.arcadeDrive(y, x);
} // end arcadeDrive
public void displayUltrasonicDataSD() {
SmartDashboard.putNumber("Ultrasonic_Raw", Robot.ultrasonicSensor.getValue());
SmartDashboard.putNumber("Ultrasonic_Volts", Robot.ultrasonicSensor.getVoltage());
SmartDashboard.putNumber("Ultrasonic_AvgRaw", Robot.ultrasonicSensor.getAverageValue());
SmartDashboard.putNumber("Ultrasonic_AvgVolts", Robot.ultrasonicSensor.getAverageVoltage());
SmartDashboard.putNumber(
"Ultrasonic_Range(In.)", Robot.ultrasonicSensor.getVoltage() / Robot.RANGE_SCALE);
}
/** This function is called periodically during operator control */
@SuppressWarnings("deprecation")
public void teleopPeriodic() {
Scheduler.getInstance().run();
// centerValue = table.getNumber("centerX",-1);
// SmartDashboard.putNumber("Center Value", centerValue);
SmartDashboard.putBoolean("Winch", !winchServo.toggle); // Down = true
SmartDashboard.putNumber("4Bar Encoder", Robot.fourBar.encoder.getDistance());
SmartDashboard.putBoolean("CD Enabled", HybridDrive.cd);
}
// basic arcadeDrive: y=forward/backward speed, x=left/right speed
public void arcadeDrive(double y, double x) {
Preferences p = Preferences.getInstance();
final boolean kReverseDirection = p.getBoolean("DriveTrainReverseDirection", false);
robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, kReverseDirection);
robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, kReverseDirection);
robotDrive.arcadeDrive(y, x + 0.05);
SmartDashboard.putNumber("DriveTrainGyro", -gyro.getAngle()); // upside down
SmartDashboard.putNumber("RangefinderVoltage", ultraDist.getVoltage());
} // end arcadeDrive
public void set(DoubleSolenoid.Value value) {
piston.set(value);
if (value == extend) {
SmartDashboard.putString("Loader", "Extend");
} else if (value == retract) {
SmartDashboard.putString("Loader", "Retract");
} else {
SmartDashboard.putString("Loader", "Off");
}
}
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
public void robotInit() {
oi = new OI();
VersionInformation vi = new VersionInformation();
SmartDashboard.putString("Git-Hash", vi.getHash());
SmartDashboard.putString("Git-Timestamp", vi.getTimestamp());
SmartDashboard.putString("Git-Tag", vi.getTag());
SmartDashboard.putString("Git-Author", vi.getAuthor());
SmartDashboard.putString("BuildType", vi.getBuildType());
}
public void displayAccelerometerDataSD() {
SmartDashboard.putNumber("Accel_X", RobotMap.accelerometer.getX());
SmartDashboard.putNumber("Accel_Y", RobotMap.accelerometer.getY());
SmartDashboard.putNumber("Accel_Z", RobotMap.accelerometer.getZ());
SmartDashboard.putNumber("Accel_Angle", Robot.accelerometerSampling.getAngleToGround());
// SmartDashboard.putNumber("Accel_Angle", (Math.atan2(Robot.accelerometerSampling.getYAvg(),
// Robot.accelerometerSampling.getXAvg()) * 180.0) / Math.PI);
}
public DriveAtom(double _dist) {
p = SmartDashboard.getNumber("Drive P: ");
i = SmartDashboard.getNumber("Drive I: ");
d = SmartDashboard.getNumber("Drive D: ");
distance = _dist;
incrementer = Definitions.DRIVE_ATOM_INCREMENTER;
threshold = 75; // we can be within approx. half an inch
}
/** This function is called periodically during operator control */
public void teleopPeriodic() {
robotDrive.mecanumDrive_Polar();
lifter.upLift();
lifter.downLift();
lifter.QuickRaise();
lifter.toteLowering(); // lowers 6 totes and a can at .1 speed
// lifter.manualUplift();
// lifter.manualDownlift();
servo.runBolt();
pusher.PushandRetract(); // push one button to push and retract
pusher.Push(); // full push with one button press
pusher.Retract(); // full retract with one button press
// pusher.ManualPush();
// pusher.ManualRetract();
SmartDashboard.putBoolean("switch 1", DigitalInput1.get());
SmartDashboard.putBoolean("switch 2", DigitalInput2.get());
SmartDashboard.putBoolean("switch 3", DigitalInput3.get());
SmartDashboard.putBoolean("switch 4", DigitalInput4.get());
SmartDashboard.putBoolean("switch 5", DigitalInput5.get());
SmartDashboard.putBoolean("switch 6", DigitalInput6.get());
SmartDashboard.putNumber("Lifting Talon Speed", LifterTalon.get());
SmartDashboard.putNumber("Pushing Talon Speed", PushingTalon.get());
SmartDashboard.putNumber("servo position zero means brake is on", servo.get());
}
public OI() {
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
joystick = new Joystick(0);
// SmartDashboard Buttons
SmartDashboard.putData("Autonomous Command", new AutonomousCommand());
SmartDashboard.putData("Arcade Drive", new ArcadeDrive());
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
}
public void RotateFrame(double s, double distance, double dir) {
reset();
left.set(dir * s);
right.set(dir * s);
do {
SmartDashboard.putNumber("left distance", lEncoder.getDistance());
SmartDashboard.putNumber("right distance", rEncoder.getDistance());
} while (rEncoder.getDistance() <= distance);
left.set(0);
right.set(0);
}
public boolean alignStraight() {
double angleOut = gyro.getAngle() * Preferences.getInstance().getDouble("Gyro_kP", 0.0);
SmartDashboard.putNumber("gyroErr", gyro.getAngle());
SmartDashboard.putNumber("angleOut", angleOut);
robotDrive.arcadeDrive(0, angleOut);
if (Math.abs(gyro.getAngle()) < Preferences.getInstance().getDouble("AngleBuffer", 100)) {
arcadeDrive(0, 0);
return true;
}
return false;
}
public void DriveForward(double s, double distance) {
reset();
left.set(s);
right.set(-1 * s);
do {
SmartDashboard.putNumber("left distance", lEncoder.getDistance());
SmartDashboard.putNumber("right distance", rEncoder.getDistance());
} while (rEncoder.getDistance() <= distance);
left.set(0);
right.set(0);
reset();
}
// Called just before this Command runs the first time
protected void initialize() {
System.out.println("Initialize timedPullBackHammer");
setTimeout(SmartDashboard.getNumber("Timed Hammer Pullback"));
if (!Robot.hammer.isHammerAtLatch()) {
System.out.println(
"Pulling back hammer for " + SmartDashboard.getNumber("Timed Hammer Pullback") + " sec");
if (Robot.hammer.isLatchReady()) Robot.hammer.releaseLatch();
Robot.hammer.pullBackHammer((float) SmartDashboard.getNumber("Hammer Motor Speed"));
} else {
System.out.println("Error: hammer is at latch");
}
}
public void updateDashboard() {
// Drive
if (RobotMap.imu != null) {
SmartDashboard.putBoolean("IMU_Connected", RobotMap.imu.isConnected());
SmartDashboard.putNumber("IMU_Yaw", RobotMap.imu.getYaw());
SmartDashboard.putNumber("IMU_CompassHeading", RobotMap.imu.getCompassHeading());
}
if (Robot.drive != null) {
SmartDashboard.putBoolean("FOD_Enabled", Robot.drive.getFODEnabled());
}
}
@Override
public void report() {
if (null != serialPort) {
SmartDashboard.putNumber(model + " Serial (cm)", getSerialRangeCm());
SmartDashboard.putNumber(model + " Serial (inch)", getSerialRangeInches());
SmartDashboard.putNumber(model + " Serial Rate", serialSampleRate.getSampleRate());
}
if (null != analogPotentiometer) {
SmartDashboard.putNumber(model + " Analog (cm)", getAnalogRangeCm());
SmartDashboard.putNumber(model + " Analog (inch)", getAnalogRangeInches());
}
}
public void tankDrive(double leftSpeed, double rightSpeed) {
Preferences p = Preferences.getInstance();
final boolean kReverseDirection = p.getBoolean("DriveTrainReverseDirection", false);
robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, kReverseDirection);
robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, kReverseDirection);
// if (kReverseDirection) {
// robotDrive.tankDrive(rightSpeed, leftSpeed);
// return;
// }
robotDrive.tankDrive(leftSpeed, rightSpeed);
SmartDashboard.putNumber("gyroErr", gyro.getAngle());
SmartDashboard.putNumber("encoder", rightEncoder.get());
} // end tankDrive
public void teleopPeriodic() {
armpid.setPID(
SmartDashboard.getNumber("kP"),
SmartDashboard.getNumber("kI"),
SmartDashboard.getNumber("kD"));
if (joy1.getRawButton(1)) {
armpid.enable();
} else {
armpid.disable();
arm.set(0);
}
SmartDashboard.putBoolean("Checkbox 2", limit.get());
System.out.println("limit " + limit.get());
}
/*
* Code to support update of PIDF constants from a button on the SmartDashboard
*/
public void updatePIDF() {
// Assign
double p = SmartDashboard.getNumber(m_name + " P");
double i = SmartDashboard.getNumber(m_name + " I");
double d = SmartDashboard.getNumber(m_name + " D");
if (m_hasFeedForward) {
double f = SmartDashboard.getNumber(m_name + " F");
setPID(p, i, d, f);
} else setPID(p, i, d);
// Update Tolerance
setTolerance(SmartDashboard.getNumber(m_name + " Tolerance"));
}
