/** * *************************************************************************** * * Robot.java (FRC 2017/2018 State -- project 2017-state-updated-for-2018) * * @author Alec Smith, Team 698 Microbots, Hamilton High School, Chandler AZ. * @version 1.0 * * Competition Robot: * 28.5" long by 31" wide (including bumpers). * Lidars are 5" (13cm) behind the front bumper. * Lidar index 0 (0x30) on right-front and index 1 (0x31) on left-front. * Axle track (distance between left and right wheel centerlines) is 21.0" * Wheelbase (distance between front and rear axles) is 15.75" with a center axle. * The center axle is dropped 1/16" (0.0625). * Has 3.875" wheels and 360CPR encoders on EVO shifting gearbox which has 3:1 output-shaft-to-encoder ratio. * Inches-per-count: 3.875 * pi * 3 / 360 = 0.1015" per count. * Based on empirical tests, we tweaked this to 0.1078" per count. * * Proto bot * 24.7" long by 22.5" wide (no bumpers). * Lidars are 5" (13cm) behind the front bumper. * Lidar index 0 (0x30) on right-front and index 1 (0x31) on left-front. * Axle track (distance between left and right wheel centerlines) is 20.25" * Wheelbase is n/a since it has front/rear casters. * Has 8.625" wheels and 360CPR encoders directly on the wheel output shafts. * Inches-per-count: 8.625 * pi / 360 = 0.07527" per count. * Based on empirical tests, we tweaked this to 0.07659" per count. * * *************************************************************************** */ package org.usfirst.frc.team698.robot; import edu.wpi.cscore.UsbCamera; import edu.wpi.first.wpilibj.CameraServer; import edu.wpi.first.wpilibj.Compressor; import edu.wpi.first.wpilibj.DigitalOutput; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.I2C; import edu.wpi.first.wpilibj.I2C.Port; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.SampleRobot; import edu.wpi.first.wpilibj.SpeedControllerGroup; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.VictorSP; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import com.ctre.CANTalon; import com.ctre.CANTalon.TalonControlMode; import com.ctre.CANTalon.FeedbackDevice; /** * ******************************************************************************* * Robot class run automatically by JVM * ******************************************************************************* */ @SuppressWarnings("deprecation") // For 2018, SampleRobot has been deprecated but still functions. // Next year re-write this for TimedRobot. public class Robot extends SampleRobot { // ----- declare class constants (global): // ========== hard-coded bot-specific stuff: // static final boolean COMPETITION_BOT = true; // true for comp-bot which has some things different than proto-bot static final boolean USE_USB_CAMERA = false; // true to allow camera; false for less bandwidth // ===== competition-bot: static final double BOT_LENGTH = 28.5; ////////////////////////////////////////////////////////////////////////////////// static final double AXLE_TRACK = 28; // actual is 21.0 (set wider for more turning) static final double INCHES_PER_COUNT = 0.1078; // theoretical: 0.1015 static final double ENCODER_KP = 0.001675; static final double AUTO_ST_SPEED = 0.50; // 0.50 is half speed // ensure that (AUTO_ST_SPEED * AUTO_MAX_CORRECT) + AUTO_ST_SPEED <= 1.0 (max pwm drive) static final double AUTO_MAX_CORRECT = 0.20; // max correction (0.10 is 10%) ////////////////////////////////////////////////////////////////////////////////// static final double AUTO_TURN_SPEED = 0.75; // //===== proto-bot: static final double BOT_LENGTH_PROTO = 24.7; static final double AXLE_TRACK_PROTO = 20.25; static final double INCHES_PER_COUNT_PROTO = 0.07659; // theoretical: 0.07527 static final double ENCODER_KP_PROTO = 0.010; static final double AUTO_ST_SPEED_PROTO = 0.50; // ensure that (AUTO_ST_SPEED_PROTO * AUTO_MAX_CORRECT_PROTO) + AUTO_ST_SPEED_PROTO <= 1.0 (max pwm drive) static final double AUTO_MAX_CORRECT_PROTO = 0.20; static final double AUTO_TURN_SPEED_PROTO = 0.40; // // ========== (end of bot-specific stuff) static final double JOYSTICK_DEADBAND = 0.15; // hard-code these also static final double JOYSTICK_SHAPE = 0.00; static final double TURN_TWEAK = 0.90; static final int THRESH_FAR_CM = 76; // far threshold (30" to lidar which is 25" to bumper) static final int THRESH_NEAR_CM = 38; // near threshold (15" to lidar which is 10" to bumper) static final int THRESH_TOUCHING_CM = 10; // touching threshold (5" to lidar which is 0" to bumper) // (s/b 13cm but tweak for better up-close calibration) static final int LEFT_LIDAR = 1; // pod index (can be 0 for 0x30, to 7 for 0x37) static final int RIGHT_LIDAR = 0; static final double INCHES_TO_BASELINE = 93.25; // 93.25 inches (7' 9.25") from rear wall to baseline static final double INCHES_TO_CENTER_GEAR = 114.5; // straight-only segment of center-autonomous (rear wall to gear wall) ////////////////////////////////////////////////////////////////////////////////// static final double INCHES_TO_TURN = 110.5; // 110.5 from arena drawing -- initial straight segment of turning-autonomous (rear wall to center of robot) // (turning arc length of turning-autonomous, about robot center -- calculated later from AXLE_TRACK) ////////////////////////////////////////////////////////////////////////////////// static final double INCHES_TURN_TO_GEAR = 42.0; // 42.0 from arena drawing -- final straight segment of turning-autonomous (gear wall to center of robot) static final int TEENSYRIO_ADDR = 0x17; // 7-bit addresses used for roborio and arduino static final int TEENSYRIO_ID = TEENSYRIO_ADDR; // teensyrio just returns its address for ID ping // I2C Command IDs: static final int CMD_NEXT_GET = 0x01; // ID of next get (master read) START,ADDR_WR,CMD,GET_ID,STOP // I2C Get-Next IDs: static final int GET_LIDAR_CM = 0x00; // eight distances in cm START,ADDR_RD,P0,P1,P2,P3,P4,P5,P6,P7,STOP static final int GET_TEENSYID = 0x01; // teensyRIO ID, just to ping START,ADDR_RD,ID,STOP static final int XBOX_BTN_A = 1; // xbox switches are 1 to 8 static final int XBOX_BTN_B = 2; static final int XBOX_BTN_X = 3; static final int XBOX_BTN_Y = 4; static final int XBOX_BUMPER_L = 5; static final int XBOX_BUMPER_R = 6; static final int XBOX_BTN_BACK = 7; static final int XBOX_BTN_START = 8; static final int XBOX_L_XAXIS = 0; // xbox axes are 0 to 5 static final int XBOX_L_YAXIS = 1; static final int XBOX_L_TRIGGER = 2; static final int XBOX_R_TRIGGER = 3; static final int XBOX_R_XAXIS = 4; static final int XBOX_R_YAXIS = 5; static final int LAUNCHPAD_BTN_1_LEFT = 1; // launchpad switches are 1 to n static final int LAUNCHPAD_BTN_2_CENTER = 2; static final int LAUNCHPAD_BTN_3_RIGHT = 3; static final int LAUNCHPAD_XAXIS_LEFT = 0; // launchpad pots are 0 to n static final int LAUNCHPAD_YAXIS_CENTER = 1; static final int LAUNCHPAD_ZAXIS_RIGHT = 2; static final boolean CODE_TRACE = true; // true to enable code-tracing outputs to riolog // ----- enum states 'n such: public enum AutoStart { AUTO_LEFT_START, AUTO_CENTER_START, AUTO_RIGHT_START } public enum AutoState { AUTO_CENTER_STRAIGHT, AUTO_TURN_INITIAL_STRAIGHT, AUTO_TURN_TURNING, AUTO_TURN_FINAL_STRAIGHT, AUTO_STOPPED } // ----- declare class fields (global): // (FRC specifies these member variables start with m_) // (these get properly set in RobotInit() or later, but init here to keep compiler happy) double m_inchesPerCount = 0.0; // inches per count of encoder double m_encoder_kP = 0.0; // proportional term for encoder control of straight driving double m_autonomousStSpeed = 0.0; // straight drive speed for auto double m_autoMaxCorrection = 0.0; // max correction to speed for auto double m_autonomousTurnSpeed = 0.0; // turn drive speed for auto double m_autoCenterStraight = 0.0; // straight-only segment of center-autonomous double m_autoInitalStraight = 0.0; // initial straight segment of turning-autonomous double m_autoTurnArc = 0.0; // turn arc length for rotation to 60 degrees double m_autoFinalStraight = 0.0; // final straight segment of turning-autonomous double m_encoderDistance = 0.0; // distance travelled (average of both encoders) boolean m_teensyRioAvail = false; // true if teensyRio is available on I2C port byte[] m_i2cRx1 = new byte[1]; // 1-byte buf byte[] m_i2cRx8 = new byte[8]; // 8-byte buf // ----- declare/create class objects (global): DigitalOutput led0 = new DigitalOutput(0); // digital output on port 0 (active-low) Joystick xbox0 = new Joystick(0); // xbox controller on usb port 0 Joystick launchpad = new Joystick(1); // TI launchpad controller on usb port 1 Joystick xbox2 = new Joystick(2); // second xbox controller on usb port 2 // kMXP port not working -- last minute hack to add cable to onboard port: I2C teensyRioI2c = new I2C(Port.kOnboard, TEENSYRIO_ADDR); // teensyRio which connects to lidar pods Compressor compress = new Compressor(0); // compressor DoubleSolenoid shifterSolenoid = new DoubleSolenoid(0,1); // shifter VictorSP m_frontLeft; VictorSP m_frontRight; VictorSP m_rearLeft; VictorSP m_rearRight; DifferentialDrive m_drive; Encoder leftEncoder; Encoder rightEncoder; // ----- optional usb camera: UsbCamera camserv; /** * ******************************************************************************* * Constructor. * ******************************************************************************* */ public Robot() { // nothing needed here -- do it all in robotInit(). } /** * ******************************************************************************* * Called once. General initialization code. * ******************************************************************************* */ public void robotInit() { // ----- using two left drive motors on PWM ports 8, 9: m_frontLeft = new VictorSP(8); m_rearLeft = new VictorSP(9); SpeedControllerGroup m_left = new SpeedControllerGroup(m_frontLeft, m_rearLeft); // invert defines which end of the bot is the front (this is the same for comp-bot and proto-bot): m_left.setInverted(true); // ----- using two right drive motors on PWM ports 6, 7: m_frontRight = new VictorSP(6); m_rearRight = new VictorSP(7); SpeedControllerGroup m_right = new SpeedControllerGroup(m_frontRight, m_rearRight); // invert defines which end of the bot is the front (this is the same for comp-bot and proto-bot): m_right.setInverted(true); // ----- and using the new diff drive: m_drive = new DifferentialDrive(m_left, m_right); // ----- using quadrature encoders on DIO ports 8, 9, 6, 7: // Encoder(channelA, channelB, reverseDirection, encodingType) // (define direction later based on bot design) leftEncoder = new Encoder(8, 9, false, Encoder.EncodingType.k4X); rightEncoder = new Encoder(6, 7, false, Encoder.EncodingType.k4X); // ----- climb motor uses talon SRX at CAN address 5: CANTalon climbMotor = new CANTalon(5); // ----- encoder setup: leftEncoder.setMinRate(10); // below this rate will report stopped rightEncoder.setMinRate(10); leftEncoder.setSamplesToAverage(7); // for calculating rate rightEncoder.setSamplesToAverage(7); // ----- bot-specific inits: if (COMPETITION_BOT) { m_inchesPerCount = INCHES_PER_COUNT; leftEncoder.setDistancePerPulse(m_inchesPerCount); rightEncoder.setDistancePerPulse(m_inchesPerCount); // opposite sides so make them both read positive for forward (inverted from proto since hanging off 3:1 gearbox shaft) leftEncoder.setReverseDirection(true); rightEncoder.setReverseDirection(false); m_encoder_kP = ENCODER_KP; m_autonomousStSpeed = AUTO_ST_SPEED; m_autoMaxCorrection = m_autonomousStSpeed * AUTO_MAX_CORRECT; m_autonomousTurnSpeed = AUTO_TURN_SPEED; // straight-only segment of center-autonomous (rear wall to gear wall, less bot length): m_autoCenterStraight = INCHES_TO_CENTER_GEAR - BOT_LENGTH; // initial straight segment of turning-autonomous (rear wall to center of robot, less half bot length): m_autoInitalStraight = INCHES_TO_TURN - (BOT_LENGTH / 2.0); // turn arc length for rotation to 60 degrees = (AXLE_TRACK * pi) (60/360) m_autoTurnArc = AXLE_TRACK * 0.5236; // final straight segment of turning-autonomous (after the turn, gear wall to center of robot, less half bot length): m_autoFinalStraight = INCHES_TURN_TO_GEAR - (BOT_LENGTH / 2.0); } // proto-bot: else { m_inchesPerCount = INCHES_PER_COUNT_PROTO; leftEncoder.setDistancePerPulse(m_inchesPerCount); rightEncoder.setDistancePerPulse(m_inchesPerCount); // opposite sides so make them both read positive for forward (encoder mounted on motor shaft): leftEncoder.setReverseDirection(false); rightEncoder.setReverseDirection(true); m_encoder_kP = ENCODER_KP_PROTO; m_autonomousStSpeed = AUTO_ST_SPEED_PROTO; m_autoMaxCorrection = m_autonomousStSpeed * AUTO_MAX_CORRECT_PROTO; m_autonomousTurnSpeed = AUTO_TURN_SPEED_PROTO; // straight-only segment of center-autonomous (rear wall to gear wall, less bot length): m_autoCenterStraight = INCHES_TO_CENTER_GEAR - BOT_LENGTH_PROTO; // initial straight segment of turning-autonomous (rear wall to center of robot, less half bot length): m_autoInitalStraight = INCHES_TO_TURN - (BOT_LENGTH_PROTO / 2.0); // turn arc length for rotation to 60 degrees = (AXLE_TRACK * pi) (60/360) m_autoTurnArc = AXLE_TRACK_PROTO * 0.5236; // final straight segment of turning-autonomous (after the turn, gear wall to center of robot, less half bot length): m_autoFinalStraight = INCHES_TURN_TO_GEAR - (BOT_LENGTH_PROTO / 2.0); } // ----- climb motor: climbMotor.changeControlMode(TalonControlMode.PercentVbus); // standard throttle climbMotor.set(0.0); // call this immediately after changeControlMode climbMotor.reverseOutput(false); // true to invert direction climbMotor.setFeedbackDevice(FeedbackDevice.CtreMagEncoder_Relative); climbMotor.reverseSensor(true); // set to ensure sensor in-phase with motor // ----- misc init: led0.set(true); // active-low (off) SmartDashboard.putString( "DB/String 0", "" ); // clear any old mssgs SmartDashboard.putString( "DB/String 1", "" ); SmartDashboard.putString( "DB/String 2", "" ); SmartDashboard.putString( "DB/String 3", "" ); SmartDashboard.putString( "DB/String 4", "" ); SmartDashboard.putString( "DB/String 5", "" ); SmartDashboard.putString( "DB/String 6", "" ); SmartDashboard.putString( "DB/String 7", "" ); SmartDashboard.putString( "DB/String 8", "" ); SmartDashboard.putString( "DB/String 9", "" ); SmartDashboard.putBoolean( "DB/LED 0", false ); SmartDashboard.putBoolean( "DB/LED 1", false ); SmartDashboard.putBoolean( "DB/LED 2", false ); SmartDashboard.putBoolean( "DB/LED 3", false ); // ----- init TeensyRIO, if available: teensyRioI2c.write(CMD_NEXT_GET, GET_TEENSYID); // get TeensyRIO-ID next teensyRioI2c.readOnly(m_i2cRx1, 1); if (TEENSYRIO_ID == m_i2cRx1[0]) { m_teensyRioAvail = true; } else { m_teensyRioAvail = false; } // ----- init camera (cam0): if (USE_USB_CAMERA) { camserv = CameraServer.getInstance().startAutomaticCapture(0); } // ----- boot message: // use System.out to output to Riolog (in eclipse: window/show-view/other/general/riolog): if (CODE_TRACE) { System.out.printf("\n========== roboRIO boot ==========\n"); System.out.printf("m_encoder_kP: %.3f\n", m_encoder_kP); System.out.printf("m_autonomousStSpeed: %.2f ", m_autonomousStSpeed); System.out.printf("m_autonomousTurnSpeed: %.2f\n", m_autonomousTurnSpeed); System.out.printf("m_autoCenterStraight: %.2f\n", m_autoCenterStraight); System.out.printf("m_autoInitalStraight: %.2f ", m_autoInitalStraight); System.out.printf("m_autoTurnArc: %.2f ", m_autoTurnArc); System.out.printf("m_autoFinalStraight: %.2f\n", m_autoFinalStraight); } if (CODE_TRACE) { System.out.printf("robotInit\n"); } SmartDashboard.putString( "DB/String 0", ("robotInit") ); } /** * ******************************************************************************* * Called when robot is disabled by driver station or field control system. * Re-init anything necessary when changing between autonomous and teleop modes. * ******************************************************************************* */ public void disabled() { // recommended by ctre to fix possible roborio race condition at power-up: //// climbMotor.changeControlMode(TalonControlMode.PercentVbus); //// climbMotor.set(0.0); SmartDashboard.putString( "DB/String 0", "" ); // clear any old mssgs SmartDashboard.putString( "DB/String 1", "" ); SmartDashboard.putString( "DB/String 2", "" ); SmartDashboard.putString( "DB/String 3", "" ); SmartDashboard.putString( "DB/String 4", "" ); SmartDashboard.putString( "DB/String 5", "" ); SmartDashboard.putString( "DB/String 6", "" ); SmartDashboard.putString( "DB/String 7", "" ); SmartDashboard.putString( "DB/String 8", "" ); SmartDashboard.putString( "DB/String 9", "" ); SmartDashboard.putBoolean("DB/LED 0", false); SmartDashboard.putBoolean("DB/LED 1", false); SmartDashboard.putBoolean("DB/LED 2", false); SmartDashboard.putBoolean("DB/LED 3", false); if (CODE_TRACE) { System.out.printf("\nDisabled mode\n"); } SmartDashboard.putString( "DB/String 0", ("Disabled mode") ); } /** * ******************************************************************************* * Called when autonomous mode selected by driver station or field control system. * Dashboard string 0 is Mode * Dashboard string 1 is Start-Position * ******************************************************************************* */ public void autonomous() { AutoStart autonomousStart; AutoState autonomousState; boolean turningLeft = false; if (CODE_TRACE) { System.out.printf("Autonomous mode\n"); } SmartDashboard.putString( "DB/String 0", ("Autonomous mode") ); // ----- switches set which start: one of three positions: AUTO_LEFT_START, AUTO_CENTER_START, AUTO_RIGHT_START if (launchpad.getRawButton(LAUNCHPAD_BTN_1_LEFT)) { autonomousStart = AutoStart.AUTO_LEFT_START; // init to left starting position } else if (launchpad.getRawButton(LAUNCHPAD_BTN_3_RIGHT)) { autonomousStart = AutoStart.AUTO_RIGHT_START; // init to right starting position } else { autonomousStart = AutoStart.AUTO_CENTER_START; // init to center starting position } ////////////////////////////////////////////////////////////// // NOTE: since center start is the default, we don't need to use the blue switch, // and since the blue side of the field needs a 2" longer drive, // we are using the center switch to define blue side of field. if (launchpad.getRawButton(LAUNCHPAD_BTN_2_CENTER)) { m_autoCenterStraight += 2.0; m_autoInitalStraight += 2.0; } ////////////////////////////////////////////////////////////// // ----- set up based on start position (left means left when looking onto field): switch (autonomousStart) { case AUTO_CENTER_START: default: autonomousState = AutoState.AUTO_CENTER_STRAIGHT; SmartDashboard.putString( "DB/String 1", ("Center Start") ); if (CODE_TRACE) { System.out.printf("Center Start\n"); } break; case AUTO_LEFT_START: autonomousState = AutoState.AUTO_TURN_INITIAL_STRAIGHT; turningLeft = false; SmartDashboard.putString( "DB/String 1", ("Left Start") ); if (CODE_TRACE) { System.out.printf("Left Start\n"); } ///////////////////////////////////////// cut this out after selecting values and hard-coding finals: /* // ----- pot-1 (-1.0 to +1.0) (and dashboard string 5) for INCHES_TO_TURN: m_autoInitalStraight = ( (launchpad.getRawAxis(LAUNCHPAD_XAXIS_LEFT) + 1.0) * 10.0) + 100.0; // adjust to 90.0 to 110.0 SmartDashboard.putString( "DB/String 5", ("P1: Inch-Turn: " + String.valueOf( String.format("%.1f", m_autoInitalStraight) ) ) ); m_autoInitalStraight -= (BOT_LENGTH / 2.0); // // ----- pot-2 (-1.0 to +1.0) (and dashboard string 6) for auto turn arc: m_autoTurnArc = (launchpad.getRawAxis(LAUNCHPAD_YAXIS_CENTER) + 1.0) * 10.0; // adjust to 0.0 to 20.0 SmartDashboard.putString( "DB/String 6", ("P2: TurnArc: " + String.valueOf( String.format("%.2f", m_autoTurnArc) ) ) ); // // ----- pot-3 (-1.0 to +1.0) (and dashboard string 7) for INCHES_TURN_TO_GEAR: m_autoFinalStraight = ( (launchpad.getRawAxis(LAUNCHPAD_ZAXIS_RIGHT) + 1.0) * 10.0) + 45.0; // adjust to 35.0 to 55.0 SmartDashboard.putString( "DB/String 7", ("P3: Inch-Gear: " + String.valueOf( String.format("%.1f", m_autoFinalStraight) ) ) ); m_autoFinalStraight -= (BOT_LENGTH / 2.0); */ ///////////////////////////////////////// break; case AUTO_RIGHT_START: autonomousState = AutoState.AUTO_TURN_INITIAL_STRAIGHT; turningLeft = true; SmartDashboard.putString( "DB/String 1", ("Right Start") ); if (CODE_TRACE) { System.out.printf("Right Start\n"); } ///////////////////////////////////////// cut this out after selecting values and hard-coding finals: /* // ----- pot-1 (-1.0 to +1.0) (and dashboard string 5) for INCHES_TO_TURN: m_autoInitalStraight = ( (launchpad.getRawAxis(LAUNCHPAD_XAXIS_LEFT) + 1.0) * 10.0) + 100.0; // adjust to 90.0 to 110.0 SmartDashboard.putString( "DB/String 5", ("P1: Inch-Turn: " + String.valueOf( String.format("%.1f", m_autoInitalStraight) ) ) ); m_autoInitalStraight -= (BOT_LENGTH / 2.0); // // ----- pot-2 (-1.0 to +1.0) (and dashboard string 6) for auto turn arc: m_autoTurnArc = (launchpad.getRawAxis(LAUNCHPAD_YAXIS_CENTER) + 1.0) * 10.0; // adjust to 0.0 to 20.0 SmartDashboard.putString( "DB/String 6", ("P2: TurnArc: " + String.valueOf( String.format("%.2f", m_autoTurnArc) ) ) ); // // ----- pot-3 (-1.0 to +1.0) (and dashboard string 7) for INCHES_TURN_TO_GEAR: m_autoFinalStraight = ( (launchpad.getRawAxis(LAUNCHPAD_ZAXIS_RIGHT) + 1.0) * 10.0) + 35.0; // adjust to 35.0 to 55.0 SmartDashboard.putString( "DB/String 7", ("P3: Inch-Gear: " + String.valueOf( String.format("%.1f", m_autoFinalStraight) ) ) ); m_autoFinalStraight -= (BOT_LENGTH / 2.0); */ ///////////////////////////////////////// break; } m_drive.setSafetyEnabled(false); // disable motor timeout leftEncoder.reset(); // set count... to zero rightEncoder.reset(); // set count... to zero m_encoderDistance = 0.0; // distance traveled (average of both encoders) // autonomous loop: while (isEnabled()) { // ========== state machine: switch (autonomousState) { case AUTO_CENTER_STRAIGHT: default: SmartDashboard.putString( "DB/String 3", ("Center") ); if (CODE_TRACE) { System.out.printf("c"); } // this will stop robot either with a max distance or a lidar threshold: if ( driveStraight(m_autoCenterStraight, true) ) { // if we go far enough... m_drive.tankDrive(0.0, 0.0); // ...stop autonomousState = AutoState.AUTO_STOPPED; // next state is stopped SmartDashboard.putString( "DB/String 3", ("Stopped") ); if (CODE_TRACE) { System.out.printf("\nStopped\n"); } } // else keep driving straight: break; case AUTO_TURN_INITIAL_STRAIGHT: SmartDashboard.putString( "DB/String 3", ("T-initial") ); if (CODE_TRACE) { System.out.printf("i"); } // this will stop robot with a max distance (ignoring lidar threshold): if ( driveStraight(m_autoInitalStraight, false) ) { // if we go far enough... m_drive.tankDrive(0.0, 0.0); // ...stop autonomousState = AutoState.AUTO_TURN_TURNING; // next state is turning leftEncoder.reset(); // reset encoders to 0 rightEncoder.reset(); Timer.delay(0.1); // pause before next segment } // else keep driving straight: break; case AUTO_TURN_TURNING: SmartDashboard.putString( "DB/String 3", ("T-turning") ); if (CODE_TRACE) { System.out.printf("t"); } if (true == turningLeft) { m_drive.tankDrive(m_autonomousTurnSpeed, -m_autonomousTurnSpeed); } else { m_drive.tankDrive(-m_autonomousTurnSpeed, m_autonomousTurnSpeed); } // ----- stop if we go far enough: m_encoderDistance = ( Math.abs(leftEncoder.getDistance()) + Math.abs(rightEncoder.getDistance()) ) / 2.0; // average SmartDashboard.putString( "DB/String 4", ("distance: " + String.valueOf( String.format("%.1f", m_encoderDistance ) ) ) ); if ( m_encoderDistance > m_autoTurnArc ) { // if we go far enough... m_drive.tankDrive(0.0, 0.0); // ...stop autonomousState = AutoState.AUTO_TURN_FINAL_STRAIGHT; // next state is final straight leftEncoder.reset(); // reset encoders to 0 rightEncoder.reset(); Timer.delay(0.1); // pause before next segment } // else keep turning: break; case AUTO_TURN_FINAL_STRAIGHT: SmartDashboard.putString( "DB/String 3", ("final") ); if (CODE_TRACE) { System.out.printf("f"); } // this will stop robot either with a max distance or a lidar threshold: if ( driveStraight(m_autoFinalStraight, true) ) { // if we go far enough... m_drive.tankDrive(0.0, 0.0); // ...stop autonomousState = AutoState.AUTO_STOPPED; // next state is stopped SmartDashboard.putString( "DB/String 3", ("Stopped") ); if (CODE_TRACE) { System.out.printf("\nStopped\n"); } } // else keep driving straight: break; case AUTO_STOPPED: // (will stay here until disabled) break; } // (end of switch) // ----- loop delay: Timer.delay(0.01); } } /** * ******************************************************************************* * Called when teleop mode selected by driver station or field control system. * Runs the motors with 2-stick arcade. * ******************************************************************************* */ public void operatorControl() { if (CODE_TRACE) { System.out.printf("Teleop mode\n"); } SmartDashboard.putString( "DB/String 0", ("Teleop mode") ); boolean b = false; double smot = 0.0; double sjoy, tjoy, tmot; boolean lowGear = false; boolean shiftTrigger = false; double leftJoyCal = xbox0.getRawAxis(XBOX_L_YAXIS); // better not be touching joysticks at the start of teleop double rightJoyCal = xbox0.getRawAxis(XBOX_R_YAXIS); // since we get the error values here double climbUp, climbDown; SmartDashboard.putBoolean("DB/LED 0", false); SmartDashboard.putBoolean("DB/LED 1", false); SmartDashboard.putBoolean("DB/LED 2", false); SmartDashboard.putBoolean("DB/LED 3", false); m_drive.setSafetyEnabled(false); // disable motor timeout /////////////////////////// climbMotor.setPosition(0); // zero climb encoder (if we use it) SmartDashboard.putString( "DB/String 8", "High Gear"); // high gear is the "no-air" shifter default // ========== main teleop loop: while (isOperatorControl() && isEnabled()) { // ===== driving (2-stick arcade with profiles and deadband): // // --- get the left/right joystick Y axes (calibrate with errors read at init): sjoy = xbox0.getRawAxis(XBOX_L_YAXIS) - leftJoyCal; // fwd/rev speed tjoy = xbox0.getRawAxis(XBOX_R_XAXIS) - rightJoyCal; // left/right turning // // --- enforce a small deadband: if (Math.abs(sjoy) < JOYSTICK_DEADBAND) sjoy = 0.0; if (Math.abs(tjoy) < JOYSTICK_DEADBAND) tjoy = 0.0; // // --- soften the ranges: if (!COMPETITION_BOT) { sjoy *= 0.5; } // speed: hard-code softening for high-gear proto // tjoy *= TURN_TWEAK; // turn // // --- linear-to-cubic profile for the speed joystick: ////smot = JOYSTICK_SHAPE * (Math.pow(sjoy, 3)) + (1 - JOYSTICK_SHAPE) * sjoy; smot = sjoy; // // --- linear is fine for the turn joystick: tmot = tjoy; // --- drive the motors: m_drive.arcadeDrive(smot, tmot); // ===== two lidars are on the front -- provide led indications at far/close thresholds: checkLidars(); // don't need return distance, just leds // ===== xbox controller right trigger for toggling shifting // (using analog range axis as switch -- need hysteresis, else oscillation at threshold): if (xbox0.getRawAxis(XBOX_R_TRIGGER) > 0.5) { if (!shiftTrigger) { lowGear = !lowGear; } if (lowGear) { SmartDashboard.putString( "DB/String 8", "Low Gear"); shifterSolenoid.set(DoubleSolenoid.Value.kForward); } else { SmartDashboard.putString( "DB/String 8", "High Gear"); shifterSolenoid.set(DoubleSolenoid.Value.kReverse); } shiftTrigger = true; } else { shiftTrigger = false; } // ===== second xbox controller left/right triggers for climb up/down: // // get triggers and limit: climbUp = xbox2.getRawAxis(XBOX_R_TRIGGER) * 0.7; climbDown = xbox2.getRawAxis(XBOX_L_TRIGGER) * 0.4; // // up (right-trigger): if (climbUp > 0.05) { //// climbMotor.set(climbUp); // positive for up } // down (left-trigger): else if (climbDown > 0.05) { //// climbMotor.set(-climbDown); // negative for down } // motor off when neither pressed: else { //// climbMotor.set(0.0); } // ==== tidy up end of loop: b = !b; if (b) led0.set(false); // active-low -- light on else led0.set(true); // active-low -- light off Timer.delay(0.01); // loop delay } } /** * ******************************************************************************* * Called when test mode selected by driver station. * Enable to turn on compressor to charge tanks before match. * ******************************************************************************* */ public void test() { if (CODE_TRACE) { System.out.printf("Test mode\n"); } SmartDashboard.putString( "DB/String 0", ("Test mode") ); while (isEnabled()) { SmartDashboard.putString( "DB/String 1", "Enabling Compressor"); compress.setClosedLoopControl(true); if (compress.getPressureSwitchValue()) { SmartDashboard.putString( "DB/String 4", ("PressureSwitch on" )); } else { SmartDashboard.putString( "DB/String 4", ("PressureSwitch off" )); } SmartDashboard.putString( "DB/String 9", ("Current Draw: " + String.valueOf( compress.getCompressorCurrent() ) ) ); Timer.delay(0.01); } } /** * ******************************************************************************* * Drive straight specified number of inches then stop, with optional lidar stop * as well. Call this in loop until it returns true to stop. * * @param distance in inches, and true for lidar stop also (false to ignore lidar) * (also some globals) * @return true is it is time to stop, else false to continue looping * (SmartDashboard string 4 is used) * ******************************************************************************* */ public boolean driveStraight(double distanceInches, boolean useLidar) { int leftCount = 0; int rightCount = 0; int countDelta; double speedCorrection; ////int lidarDistance; // encoders must both count positive for forward: leftCount = leftEncoder.get(); rightCount = rightEncoder.get(); // positive delta means we need to turn left: countDelta = leftCount - rightCount; // ----- drive straight forward: // speedCorrection = (double)(countDelta) * m_encoder_kP; if (speedCorrection > m_autoMaxCorrection) { speedCorrection = m_autoMaxCorrection; } // limit pos if (speedCorrection < -m_autoMaxCorrection) { speedCorrection = -m_autoMaxCorrection; } // limit neg // // tankDrive(leftvalue, rightvalue) -- 0 is stopped, -1.0 is max forward, 1.0 is max reverse: m_drive.tankDrive( -(m_autonomousStSpeed - speedCorrection), -(m_autonomousStSpeed + speedCorrection) ); // ----- two lidars are on the front -- stop if we get up to wall: //// (some false trips stopping us early; comment-out for now -- need to look at later) ////if (useLidar) { //// lidarDistance = checkLidars(); //// if (lidarDistance < THRESH_TOUCHING_CM) { //// return true; // time to stop //// } ////} // ----- stop if we go far enough: m_encoderDistance = ( leftEncoder.getDistance() + rightEncoder.getDistance() ) / 2.0; // average SmartDashboard.putString( "DB/String 4", ("encDistance: " + String.valueOf( String.format("%.1f", m_encoderDistance ) ) ) ); if ( Math.abs(m_encoderDistance) > distanceInches ) { // don't care about direction of encoders return true; // time to stop } return false; // not yet time to stop } /** * ******************************************************************************* * Check two lidars on the front of bot, provide led indications at far/near * thresholds, and return best-guess of distance in cm (1 to 120). Call this in loop. * * @param void (also uses some globals) * @return distance in cm (1 to 120), or 120 cm if measurement is questionable * (SmartDashboard string 9 is used as well as all four leds) * ******************************************************************************* */ public int checkLidars() { int lidarDistance = 0; if (m_teensyRioAvail) { // get pod data: // (0xFF is bogus data flag (which is -1 in 2's-comp byte), and ignore 0, so > 0 is valid) teensyRioI2c.readOnly(m_i2cRx8, 8); // is left value valid? if (m_i2cRx8[LEFT_LIDAR] > 0) { // is right value valid? if (m_i2cRx8[RIGHT_LIDAR] > 0) { // if both values are valid, average the two lidars: lidarDistance = ( m_i2cRx8[LEFT_LIDAR] + m_i2cRx8[RIGHT_LIDAR] ) / 2; } // left good, but right is bogus: else { // use valid left value: lidarDistance = m_i2cRx8[LEFT_LIDAR]; } } // left is bogus: else { // is right value valid? if (m_i2cRx8[RIGHT_LIDAR] > 0) { // use valid right value: lidarDistance = m_i2cRx8[RIGHT_LIDAR]; } else { // (else, ignore since both values are bogus) lidarDistance = 120; } } SmartDashboard.putString( "DB/String 9", ("lidar cm: " + String.valueOf( lidarDistance ) ) ); // if (lidarDistance < THRESH_NEAR_CM) { // four leds to indicate "pretty close" SmartDashboard.putBoolean("DB/LED 0", true); SmartDashboard.putBoolean("DB/LED 1", true); SmartDashboard.putBoolean("DB/LED 2", true); SmartDashboard.putBoolean("DB/LED 3", true); } else if (lidarDistance < THRESH_FAR_CM) { // two leds to indicate "kinda close" SmartDashboard.putBoolean("DB/LED 0", true); SmartDashboard.putBoolean("DB/LED 1", true); SmartDashboard.putBoolean("DB/LED 2", false); SmartDashboard.putBoolean("DB/LED 3", false); } else { SmartDashboard.putBoolean("DB/LED 0", false); // no leds to indicate "nowhere close" SmartDashboard.putBoolean("DB/LED 1", false); SmartDashboard.putBoolean("DB/LED 2", false); SmartDashboard.putBoolean("DB/LED 3", false); } return lidarDistance; } else { SmartDashboard.putString( "DB/String 9", ("teensyRio not avail") ); return 120; } } }