@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;
      }
    }
  }
Exemple #2
0
 /** 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());
 }
Exemple #3
0
  /** 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());
  }
Exemple #4
0
  /** 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);

  }
Exemple #12
0
 @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();
 }
Exemple #16
0
  /** 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());
  }
Exemple #23
0
  /** 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);
  }
Exemple #24
0
 public void testCounter() {
   double counter = 0.0;
   while (true) {
     SmartDashboard.putNumber("Counter", counter++);
     Timer.delay(.10);
   }
 }
Exemple #25
0
  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());
    }
  }
Exemple #28
0
  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());
  }
Exemple #30
0
 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();
 }