@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 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());
}
/** 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());
}
/* (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());
}
public static void Dashboard() {
SmartDashboard.putNumber("arm_value", talon_arm.get());
SmartDashboard.putInt("PulseWidthpos", PulseWidthpos);
SmartDashboard.putInt("PulseWidthus", PulseWidthus);
SmartDashboard.putInt("periodus", periodus);
SmartDashboard.putInt("PulseWidthVel", PulseWidthVel);
SmartDashboard.putNumber("val", JoyDrive.val);
}
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);
}
// 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 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);
}
@Override
public void sendToSmartDash() {
SmartDashboard.putNumber(this.getName() + " kP", kP);
SmartDashboard.putNumber(this.getName() + " kI", kI);
SmartDashboard.putNumber(this.getName() + " kD", kD);
SmartDashboard.putNumber(this.getName() + " ERROR", this.previousError);
kP = SmartDashboard.getNumber(this.getName() + " kP", kP);
kI = SmartDashboard.getNumber(this.getName() + " kI", kI);
kD = SmartDashboard.getNumber(this.getName() + " kD", kD);
}
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 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 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();
}
/** This function is called periodically during test mode */
public void testPeriodic() {
driveTrainTester.run();
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());
}
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
@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 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());
}
}
public void displayJoystickDataSD() {
SmartDashboard.putNumber("Joy1_X", Robot.oi.getJoystick1().getX());
SmartDashboard.putNumber("Joy1_Y", Robot.oi.getJoystick1().getY());
SmartDashboard.putNumber("Joy1_Z", Robot.oi.getJoystick1().getZ());
SmartDashboard.putNumber("Joy1_Axis4", Robot.oi.getJoystick1().getRawAxis(4));
SmartDashboard.putNumber("Joy1_Twist", Robot.oi.getJoystick1().getTwist());
SmartDashboard.putNumber("Joy2_X", Robot.oi.getJoystick2().getX());
SmartDashboard.putNumber("Joy2_Y", Robot.oi.getJoystick2().getY());
SmartDashboard.putNumber("Joy2_Z", Robot.oi.getJoystick2().getZ());
SmartDashboard.putNumber("Joy2_Axis4", Robot.oi.getJoystick2().getRawAxis(4));
SmartDashboard.putNumber("Joy2_Twist", Robot.oi.getJoystick2().getTwist());
}
public boolean shiftDone() {
double diff = two.pidGet();
SmartDashboard.putNumber("diff", diff);
SmartDashboard.putNumber("Setpoint", setPoint);
System.out.println("In shiftDone(), diff: " + diff + " Setpoint: " + setPoint);
if (diff < setPoint) setSpeed(0.1);
else setSpeed(-0.1);
if (Math.abs(diff - setPoint) < 0.03) return true;
return false;
}
private void initSmartDashboard() {
SmartDashboard.putNumber(m_name + " P", m_talon.getP());
SmartDashboard.putNumber(m_name + " I", m_talon.getI());
SmartDashboard.putNumber(m_name + " D", m_talon.getD());
SmartDashboard.putNumber(m_name + " F", m_talon.getF());
SmartDashboard.putNumber(m_name + " I*100", (m_talon.getI()) * 100);
SmartDashboard.putNumber(m_name + " Talon Setpoint", m_setpoint);
SmartDashboard.putNumber(m_name + " Position", m_talon.getPosition());
SmartDashboard.putNumber(m_name + " Tolerance", m_tolerance);
SmartDashboard.putNumber(m_name + " Error", m_talon.getClosedLoopError());
SmartDashboard.putNumber(m_name + " Output", m_talon.get());
SmartDashboard.putBoolean(m_name + " Enabled", m_talon.isControlEnabled());
}
/** This function is called periodically during operator control */
public void teleopPeriodic() {
Scheduler.getInstance().run();
SmartDashboard.putNumber("Encoder-Value", drivetrain.encoderGet(0));
// SmartDashboard.putNumber("Timer", System.currentTimeMillis() - time);
elevatorPID.safetyCheck();
claw0PID.safetyCheck();
elevatorPID.showPosition();
SmartDashboard.putNumber("Encoder-Value", drivetrain.encoderGet(0));
SmartDashboard.putNumber("Selected Floor", Robot.elevatorPID.getSelectedFloor());
SmartDashboard.putBoolean("Compensation On?", Robot.elevatorPID.isCompSelected());
// camera stuff
// camera.getImage(frame);
// CameraServer.getInstance().setImage(frame);
}
public void testCounter() {
double counter = 0.0;
while (true) {
SmartDashboard.putNumber("Counter", counter++);
Timer.delay(.10);
}
}
public void updateStatus() {
SmartDashboard.putNumber("shooting RPM", getRPM());
// SmartDashboard.putNumber("shooting raw", getEncoderRaw());
// SmartDashboard.getNumber("Threashold",
// (Math.abs(this.getPosition()-this.getSetpoint()))/this.getSetpoint());
}
public void autonomousPeriodic() {
relay.set(Relay.Value.kOn);
smart.putNumber("distance", ultrasonic.getVoltage());
drive.setRun(false);
loadAndShoot.setRun(false);
// relay.set(Relay.Value.kOn);
if (autoTimer.get() < autoMoveForwardTime) {
System.out.println(
"Moving forward, Timer at "
+ autoTimer.get()
+ ", Ultrasonic at "
+ ultrasonic.getAverageVoltage());
}
if (autoTimer.get() >= autoMoveForwardTime && ultrasonic.getAverageVoltage() <= AUTO_DISTANCE) {
stop();
if (!autonomousStopped) {
autonomousStopped = true;
autoTimeAtStop = autoTimer.get();
}
System.out.println("Stopped, Ultrasonic at " + ultrasonic.getAverageVoltage());
}
if (autoTimer.get() >= autoTimeAtStop + 1 && ultrasonic.getAverageVoltage() <= AUTO_DISTANCE) {
autoStop = true;
shoot();
System.out.println("Shooting, Ultrasonic at " + ultrasonic.getAverageVoltage());
}
}
/**
* Set the PID Controller gain parameters. Set the proportional, integral, and differential
* coefficients.
*
* @param p Proportional coefficient
* @param i Integral coefficient
* @param d Differential coefficient
*/
public synchronized void setPID(double p, double i, double d) {
m_P = p;
m_I = i;
m_D = d;
m_talon.setProfile(0);
m_talon.setP(p);
m_talon.setI(i);
m_talon.setD(d);
if (m_debugging) {
SmartDashboard.putNumber(m_name + " P", m_talon.getP());
SmartDashboard.putNumber(m_name + " I", m_talon.getI());
SmartDashboard.putNumber(m_name + " D", m_talon.getD());
}
}
public void disabledPeriodic() {
// gyroSubAvg += gyro.getAngle();
// gyroSubAvg /= 2;
SmartDashboard.putNumber("Gyro", gyro.getAngle());
// SmartDashboard.putNumber("Gyro Scaling Value", gyroSubAvg);
}
public void arcadeDrive() {
lT = OI.getDriveStick().getLeftX() - OI.getDriveStick().getLeftY();
rT = OI.getDriveStick().getLeftX() + OI.getDriveStick().getLeftY();
// maxV = Math.max(Math.abs(lT), Math.abs(rT));
if (maxV > 1) {
lT = lT / maxV;
rT = rT / maxV;
}
left.set(lT);
right.set(rT);
SmartDashboard.putNumber("left distance while driving", lEncoder.getDistance());
SmartDashboard.putNumber("right distance while driving", rEncoder.getDistance());
}
public double getRightY() {
// set threshold value to 0.09
if (Math.abs(rightJoy.getY()) <= 0.09) {
return 0;
}
SmartDashboard.putNumber("SPEED RIGHT TRAIN", (-1) * rightJoy.getY());
return (-1) * rightJoy.getY();
}