示例#1
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());
  }
示例#2
0
  @Override
  public void update(SensorManager sensors) {
    if (sensors.getGyroZAngle() <= 5 && !selectorCheck) caseSelector = 1;

    switch (caseSelector) {
      case 1: // Approach the defense          ______
        controller.forward(speed1); //   |     |
        //									V     |
        if (sensors.getSonicRangeInches() == 6) // How far away from the drawbridge to stop
        {
          controller.stop();
          caseSelector = 2;
          // time = System.currentTimeMillis();
        }

        break;

      case 2: // Manage arm movement and prepare to cross - steps 3-7 on sheet
        // Lower arm
        // Move backward
        // Lower arm further
        // Lower shooter
        // Raise arm
        // Move forward

        if (joy.getTiltArm() == basedegrees1) dr.tiltArm(0);
        else if (joy.getTiltArm() < basedegrees1) dr.tiltArm(0.5); // plz test numbers
        else if (joy.getTiltArm() > basedegrees1)
          dr.tiltArm(-0.5); // plz test numbers, idk if positve or negative

        if (joy.getTiltJoint() == jointdegrees1) dr.tiltJoint(0);
        else if (joy.getTiltJoint() < jointdegrees1) dr.tiltJoint(0.5); // plz test numbers
        else if (joy.getTiltJoint() > jointdegrees1)
          dr.tiltJoint(-0.5); // plz test numbers, idk if positve or negative

        if (joy.getTiltArm() == basedegrees1 && joy.getTiltJoint() == jointdegrees1) {
          dr.tiltArm(0);
          dr.tiltJoint(0);
          time = System.currentTimeMillis();
          caseSelector = 3;
        }

        break;

      case 3:
        controller.forward(-speed2);

        if (joy.getTiltArm() == basedegrees2) dr.tiltArm(0);
        else if (joy.getTiltArm() < basedegrees2) dr.tiltArm(0.5); // plz test numbers
        else if (joy.getTiltArm() > basedegrees2)
          dr.tiltArm(-0.5); // plz test numbers, idk if positve or negative

        if (joy.getTiltJoint() == jointdegrees2) dr.tiltJoint(0);
        else if (joy.getTiltJoint() < jointdegrees2) dr.tiltJoint(0.5); // plz test numbers
        else if (joy.getTiltJoint() > jointdegrees2)
          dr.tiltJoint(-0.5); // plz test numbers, idk if positve or negative

        if (joy.getTiltArm() == basedegrees2
            && joy.getTiltJoint() == jointdegrees2
            && System.currentTimeMillis() - time >= 3000) {
          dr.tiltArm(0);
          dr.tiltJoint(0);
          controller.stop();
          caseSelector = 4;
        }
        break;

      case 4:
        if (joy.getTiltArm() == 180 - basedegrees2) dr.tiltArm(0);
        else if (joy.getTiltArm() < 180 - basedegrees2) dr.tiltArm(0.5); // plz test numbers
        else if (joy.getTiltArm() > 180 - basedegrees2)
          dr.tiltArm(-0.5); // plz test numbers, idk if positve or negative

        if (joy.getTiltJoint() == 180 - jointdegrees2) dr.tiltJoint(0);
        else if (joy.getTiltJoint() < 180 - jointdegrees2) dr.tiltJoint(0.5); // plz test numbers
        else if (joy.getTiltJoint() > 180 - jointdegrees2)
          dr.tiltJoint(-0.5); // plz test numbers, idk if positve or negative

        if (joy.getTiltArm() == 180 - basedegrees2 && joy.getTiltJoint() == 180 - jointdegrees2) {
          dr.tiltArm(0);
          dr.tiltJoint(0);
        }

        encoder.setShooterAngle(shooterangle);

        if (joy.getTiltArm() == 180 - basedegrees2
            && joy.getTiltJoint() == 180 - jointdegrees2
            && encoder.getEncoderAngle() == shooterangle) {
          caseSelector = 5;
        }

        break;

      case 5:
        controller.forward(speed3);

        if (sensors.getGyroZAngle() > 5) {
          caseSelector = 6;
        }

      case 6:
        encoder.setShooterAngle(180 - shooterangle);
        if (sensors.getGyroZAngle() < -5 && encoder.getEncoderAngle() == 180 - shooterangle) {
          caseSelector = 7;
        }
        break;

      case 7:
        controller.forward(speed3);
        if (sensors.getGyroZAngle() == 0) {
          caseSelector = 8;
        }
        break;

      case 8: // Stop
        controller.stop();
        System.out.println("Done!");
        break;
    }
  }