/** * *************************************************************************** * * Robot.java -- project 2018-west-competition-6 * * @author Alec Smith, Team 698 Microbots, Hamilton High School, Chandler AZ. * @version 1.0 * * Competition Robot: * 37.75" long by 34.50" wide (including bumpers). * Axle track (distance between left and right wheel centerlines) is 24.56" * Wheelbase (distance between front and rear axles) is 20.07" with two center axles (four axles total). * The center two axles are dropped 1/16" (0.0625). * Has 4.00" wheels and 400CPR encoders (one wheel revolution is one encoder revolution). * Inches-per-count: 4.00 * pi / 400 = 0.0314" per count theoretical. * Proto bot: * Same as above except: * Has 4.00" wheels as well, but 360CPR encoders (one wheel revolution is one encoder revolution). * Inches-per-count: 4.00 * pi / 360 = 0.0349" per count theoretical. * * Main Xbox controller on usb port 0: * Left joystick forward-reverse (Y-axis) is speed (2-stick arcade) * Right joystick left-right (X-axis)is turning (2-stick arcade) * Button A is to lift arm to Switch * Button B is to drop arm down * Button X is unused * Button Y is to lift arm to Scale * Left Trigger (variable) is Intake Out-Front * Right Trigger (variable) is Intake In or Out-Rear * Left Bumper (on/off) is test of lift arm down (comment out when done testing) * Right Bumper (on/off) is test of lift arm up (comment out when done testing) * * Secondary Xbox controller on usb port 2: * Right Trigger (variable) is Winch * button B is to shut everything down in case of problem * * TI Launchpad (pcb inside wooden operator console) on usb port 1: * Provides three switches and three potentiometers that robot code can read for competition or debug use. * We are using two outer switches to select left/center/right start position on the field. * * * *************************************************************************** */ package org.usfirst.frc.team698.robot; import edu.wpi.first.wpilibj.SampleRobot; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.DigitalOutput; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.I2C; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.PowerDistributionPanel; import edu.wpi.first.wpilibj.SpeedControllerGroup; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.VictorSP; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.I2C.Port; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * ******************************************************************************* * Robot class run automatically by JVM * ******************************************************************************* */ @SuppressWarnings("deprecation") // Note: for 2018, SampleRobot has been deprecated but still functions. // Next year re-write this for TimedRobot or other class. public class Robot extends SampleRobot { // ========== hard-coded class constants (global): // static final boolean COMPETITION_BOT = true; // true for comp-bot which has some things different than proto-bot // ----- bot dimensions: static final double BOT_LENGTH = 37.75; // including bumpers static final double BOT_WIDTH = 34.50; // including bumpers static final double AXLE_TRACK = 24.56; // tire center left to right static final double WHEELBASE = 20.07; // front to rear axle center // ----- field dimensions: static final double DISTANCE_TO_SWITCH_CENTER = 160.0; // actual is 168.0 -- rear wall to centerline of switch plate on our side static final double DISTANCE_TO_SWITCH_FROM_SIDE = 38.35; // drive path from side position to switch plate //// static final double DISTANCE_TO_SWITCH_REAR_PATH = 235.25; // rear wall to drive path behind the switch //// static final double DISTANCE_BEHIND_SWITCH_HORIZ = 169.15; // drive path behind switch to opposite plate centerline //// static final double DISTANCE_BEHIND_SWITCH_TO_CUBE = 26.25; // drive path to touch cube on floor behind switch plate static final double DISTANCE_CENTER_FWD_TO_PLATE = 140.0; // actual is 140.0 -- center forward to right switch plate static final double DISTANCE_LIL_BIT_FWD = 55.00; // actual is 55.0 -- part way forward to switch plate static final double DISTANCE_REST_OF_THE_WAY_FWD = 75.00; // actual is 85.0 -- rest of way forward to switch plate static final double DISTANCE_CENTER_TO_LEFT_PLATE = 108.8; // center start to left switch plate centerline static final double DISTANCE_LR_TO_OPPOSITE_PLATE = 169.15; // left or right start to opposite switch plate centerline static final double DISTANCE_A_TAD_BACK = 45.00; // a bit backwards static final double DISTANCE_A_TAD_FWD = 25.00; // a bit forwards static final double SWITCH_WIDTH = 36.0; static final double SWITCH_DEPTH = 48.0; static final double INCHES_TO_AUTOLINE = 120.0; // 10 feet from rear wall to edge of auto-line // ----- joystick basic settings: static final double JOYSTICK_DEADBAND = 0.15; // centered-stick deadband static final boolean JOY_USE_CUBIC_SPEED = false; // true for cubic speed profile, false for linear static final double JOY_CUBIC_SPEED_SHAPE = 0.00; // 0.0 to 1.0 -- if cubic speed profile is used static final double MAX_MOTOR_INCREMENT = 0.10; // for acceleration limiting // ----- test mode: either compressor-charging, autonomous calibrate, tuning modes, etc: // set only one (or none) of these true: static final boolean TEST_AUTO_CALIBRATE_MODE = false; // true to manually push robot and see encoder measurements static final boolean TEST_AUTO_ST_DRIVE_TUNE_MODE = false; // true to tune params for straight drive static final boolean TEST_AUTO_TURN_TUNE_MODE = false; // true to tune params for turning 90 degrees static final boolean TEST_INTAKE_MODE = false; // true to test intake gizmo static final boolean TEST_LIFT_ARM_SWITCH_MODE = false; // true to test lift arm to switch static final boolean TEST_LIFT_ARM_SCALE_MODE = false; // true to test lift arm to scale static final boolean TEST_LIFT_ARM_SENSOR_MODE = false; // true to test lift arm sensor (string pot) static final boolean TEST_LIDAR_MODE = false; // true to test lidar // static final double TEST_AUTO_ST_DRIVE_INCHES = 38.35; // test distance static final boolean TEST_AUTO_DRIVE_TRACING = true; // true to enable tracing of tuning (to console) // ----- competition-bot-specific: static final double TELEOP_LINEAR_SPEED_MAX = 1.0; // 0.0 to 1.0 -- if linear speed profile is used static final double TELEOP_LINEAR_TURN_MAX = 1.0; // 0.0 to 1.0 -- linear turn profile static final double ENCODER_INCHES_PER_COUNT = 0.0322; // theoretical: 0.0314 static final double AUTO_ST_MAX_SPEED = 0.75; // 1.0 is full speed static final double AUTO_ST_MIN_SPEED = 0.37; // below a certain point motors won't move static final double AUTO_BRAKE_SPEED = 1.0; // in opposite direction, of course static final double AUTO_BEGIN_MED_SPEED = 0.5; // % distance to begin driving at med speed static final double AUTO_BEGIN_MIN_SPEED = 0.6; // % distance to begin driving at min speed static final double AUTO_BEGIN_BRAKING = 0.65; // % distance to begin braking static final double AUTO_DRIVE_KO = 40.0; // tuning -- offset (steering) term static final double AUTO_TURN_SPEED = 0.75; // 1.0 is full speed static final double AUTO_TURN_HACK = 1.05; // 1.0 in a perfect world // static final boolean AUTO_DRIVE_SWITCHED_TEN_FT_ONLY = true; // true to just drive straight 120 inches over auto line IF blue switch on static final boolean AUTO_USE_PRE_DELAY = false; // true for pre-auto delay static final double AUTO_PRE_DELAY_TIME = 5.0; // seconds of delay before auto run // // ----- proto-bot-specific: static final double TELEOP_LINEAR_SPEED_MAX_PROTO = 0.75; // 0.0 to 1.0 -- if linear speed profile is used static final double TELEOP_LINEAR_TURN_MAX_PROTO = 0.75; // 0.0 to 1.0 -- linear turn profile static final double ENCODER_INCHES_PER_COUNT_PROTO = 0.0360; // theoretical: 0.0349 static final double AUTO_ST_MAX_SPEED_PROTO = 0.75; // 1.0 is full speed static final double AUTO_ST_MIN_SPEED_PROTO = 0.40; // below a certain point motors won't move static final double AUTO_BRAKE_SPEED_PROTO = 0.50; // in opposite direction, of course static final double AUTO_BEGIN_MED_SPEED_PROTO = 0.50; // % distance to begin driving at med speed static final double AUTO_BEGIN_MIN_SPEED_PROTO = 0.70; // % distance to begin driving at min speed static final double AUTO_BEGIN_BRAKING_PROTO = 0.85; // % distance to begin braking static final double AUTO_DRIVE_KO_PROTO = 6.0; // tuning -- offset (steering) term static final double AUTO_TURN_SPEED_PROTO = 0.75; // 1.0 is full speed static final double AUTO_TURN_HACK_PROTO = 0.72; // 1.0 in a perfect world // ----- config lift arm: static final boolean LIFT_ENABLED = true; // true to enable lift static final double LIFT_SWITCH_STAGE1_SPEED_UP = 0.75; // speed up to switch stage1 (lowermost segment) static final double LIFT_SWITCH_STAGE2_SPEED_UP = 0.50; // speed up to switch stage2 (topmost segment) static final double LIFT_SWITCH_SPEED_HOLDING_UP = 0.15; // speed to hold at switch // static final double LIFT_SCALE_STAGE1_SPEED_UP = 0.85; // speed up to scale stage1 (lowermost segment) static final double LIFT_SCALE_STAGE2_SPEED_UP = 0.70; // speed up to scale stage2 (topmost segment) static final double LIFT_SCALE_STAGE3_SPEED_UP = 0.50; // speed up to scale stage2 (topmost segment) static final double LIFT_SCALE_SPEED_HOLDING_UP = 0.15; // speed to hold at switch // static final double LIFT_STAGE1_SPEED_DOWN = 0.30; // speed down to stage1 (topmost segment) static final double LIFT_STAGE2_SPEED_DOWN = 0.20; // speed down to stage2 static final double LIFT_STAGE3_SPEED_DOWN = 0.15; // speed down to stage3 static final double LIFT_STAGE4_SPEED_DOWN = 0.10; // speed down to ground (lowermost segment) static final double LIFT_SPEED_HOLDING_DOWN = 0.10; // speed to hold at ground // // string potentiometer reading -- measured (inches from floor to bottom of box, vs encoder readings): // 0" 240 // 6" 270 // 8" 320 // 12" 545 // 18" 715 // 24" 925 // 30" 995 // 36" 1235 // 42" 1455 // 48" 1660 // 54" 1770 // 60" 2025 // 66" 2200 // 72" 2390 // 78" 2500 NEVER GO HIGHER THAN THIS // static final int LIFT_ARM_HEIGHT_12_INCHES = 545; // 12" // static final int LIFT_ARM_HEIGHT_SWITCH_STAGE1 = 600; // 14" static final int LIFT_ARM_HEIGHT_SWITCH_STAGE2 = 1000; // 22" // static final int LIFT_ARM_HEIGHT_SCALE_STAGE1 = 1235; // 36" static final int LIFT_ARM_HEIGHT_SCALE_STAGE2 = 1770; // 54" static final int LIFT_ARM_HEIGHT_SCALE_STAGE3 = 2500; // 66" // static final int LIFT_ARM_HEIGHT_DOWN_STAGE1 = 1455; // 42" static final int LIFT_ARM_HEIGHT_DOWN_STAGE2 = 925; // 24" static final int LIFT_ARM_HEIGHT_DOWN_STAGE3 = 545; // 12" static final int LIFT_ARM_HEIGHT_FLOOR = 255; // 3" // ----- config other mechanisms: static final double INTAKE_IN_MAX_SPEED = 1.0; // 1.0 max static final double INTAKE_OUT_MAX_SPEED = 1.0; // 1.0 max static final double WINCH_MAX_SPEED = 1.0; // 1.0 max static final double MOTOR_OFF = 0.0; // (don't change) // ----- PWM ports and PDP current sense channels: static final int FRONT_LEFT_MOTOR = 6; // pwm port static final int FRONT_LEFT_MOTOR_SENSE = 12; // pdp sense channel (40A breaker) static final int REAR_LEFT_MOTOR = 7; // pwm port static final int REAR_LEFT_MOTOR_SENSE = 13; // pdp sense channel (40A breaker) static final int FRONT_RIGHT_MOTOR = 8; // pwm port static final int FRONT_RIGHT_MOTOR_SENSE = 3; // pdp sense channel (40A breaker) static final int REAR_RIGHT_MOTOR = 9; // pwm port static final int REAR_RIGHT_MOTOR_SENSE = 2; // pdp sense channel (40A breaker) static final int LIFT_ARM_MOTOR = 4; // pwm port static final int LIFT_ARM_MOTOR_SENSE = 14; // pdp sense channel (40A breaker) static final int WINCH_MOTOR = 5; // pwm port static final int WINCH_MOTOR_SENSE = 1; // pdp sense channel (40A breaker) // // now-unused FORMER intake wired with 14 AWG and needs 30A max breaker: ////static final int INTAKE_LEFT_MOTOR = 2; // pwm port ////static final int INTAKE_LEFT_MOTOR_SENSE = 11; // pdp sense channel (30A breaker) ////static final int INTAKE_RIGHT_MOTOR = 3; // pwm port ////static final int INTAKE_RIGHT_MOTOR_SENSE = 4; // pdp sense channel (30A breaker) // intake (was outtake) wired with 18 AWG and needs 20A max breaker: static final int INTAKE_LEFT_MOTOR = 0; // pwm port static final int INTAKE_LEFT_MOTOR_SENSE = 9; // pdp sense channel (20A breaker) static final int INTAKE_RIGHT_MOTOR = 1; // pwm port static final int INTAKE_RIGHT_MOTOR_SENSE = 10; // pdp sense channel (20A breaker) // ----- DIO ports: static final int LEFT_ENCODER_CHA = 8; static final int LEFT_ENCODER_CHB = 9; static final int RIGHT_ENCODER_CHA = 6; static final int RIGHT_ENCODER_CHB = 7; // ----- xbox controller defines: 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; // ----- ti launchpad (external switches etc) defines: 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; // ----- lidar: 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 LIDAR_HISTORY_BUFF_SIZE = 5; // lidar history buf size (for averaging) static final boolean CODE_TRACE = true; // true to enable code-tracing outputs to console // ----- enum states: public enum AutoStartPosition { AUTO_RIGHT_START, AUTO_CENTER_START, AUTO_LEFT_START } public enum AutoTurn { AUTO_TURN_90_RIGHT, AUTO_TURN_90_LEFT } public enum DriveStraightState { PROFILED, BRAKING, CRAWLING } public enum MainState { IDLE, LIFT_SWITCH_STAGE1_UP, LIFT_SWITCH_STAGE2_UP, LIFT_SCALE_STAGE1_UP, LIFT_SCALE_STAGE2_UP, LIFT_SCALE_STAGE3_UP, LIFT_STAGE1_DOWN, LIFT_STAGE2_DOWN, LIFT_STAGE3_DOWN, LIFT_STAGE4_DOWN, LIFT_ARM_HOLDING_UP, LIFT_ARM_TIMEOUT_UP, LIFT_ARM_HOLDING_DOWN, LIFT_ARM_TIMEOUT_DOWN, SHUTDOWN } // ----- 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_loopTime = 0.01; // use a 10ms loop rate everywhere double m_elapsedTime = 0.0; // for timeouts MainState m_currentState = MainState.IDLE; // good stuff happens in the state machine // // for teleop: double m_teleopSpeedMax = 0.0; double m_teleopTurnMax = 0.0; // // for autonomous: double m_autoInchesPerCount = 0.0; // inches per count of encoder double m_autoDrive_kO = 0.0; // left/right symmetry term for encoder control of driving double m_autoStraightMaxSpeed = 0.0; // straight drive max speed for auto double m_autoStraightMinSpeed = 0.0; // straight drive min speed for auto double m_autoStraightBrakeSpeed = 0.0; // straight drive min speed for auto double m_autoBeginMedSpeed = 0.0; // % distance to begin driving at med speed double m_autoBeginMinSpeed = 0.0; // % distance to begin driving at min speed double m_autoBeginBraking = 0.0; // % distance to begin braking double m_autoTurnSpeed = 0.0; // turn drive speed for auto double m_autoTurnHack = 0.0; // turn hack for auto // // for lidar: 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): VictorSP frontLeftMot; VictorSP frontRightMot; VictorSP rearLeftMot; VictorSP rearRightMot; VictorSP intakeLeftMot; VictorSP intakeRightMot; VictorSP liftArmMot; VictorSP winchMot; DifferentialDrive diffDrive; PowerDistributionPanel pdp; Encoder leftEncoder; Encoder rightEncoder; XboxController xbox0 = new XboxController(0); // xbox controller on usb port 0 Joystick launchpad = new Joystick(1); // TI launchpad controller on usb port 1 XboxController xbox2 = new XboxController(2); // second xbox controller on usb port 2 AnalogInput analog0 = new AnalogInput(0); // analog input on port 0 (lift arm) DigitalOutput led0 = new DigitalOutput(0); // digital output on port 0 (active-low) I2C teensyRioI2c = new I2C(Port.kMXP, TEENSYRIO_ADDR); // teensyRio which connects to lidar pods /** * ******************************************************************************* * Constructor. * ******************************************************************************* */ public Robot() { // nothing needed here -- do it all in robotInit() } /** * ******************************************************************************* * Called once. General initialization code. * ******************************************************************************* */ public void robotInit() { // ----- boot message: // use System.out to output to Riolog (in eclipse: window/show-view/other/general/riolog): SmartDashboard.putString( "DB/String 0", ("robotInit") ); if (CODE_TRACE) { System.out.printf("\n========== roboRIO boot ==========\n"); } // ========== drivetrain: // // ----- using two left drive motors: frontLeftMot = new VictorSP(FRONT_LEFT_MOTOR); rearLeftMot = new VictorSP(REAR_LEFT_MOTOR); SpeedControllerGroup leftMotors = new SpeedControllerGroup(frontLeftMot, rearLeftMot); // ----- using two right drive motors: frontRightMot = new VictorSP(FRONT_RIGHT_MOTOR); rearRightMot = new VictorSP(REAR_RIGHT_MOTOR); SpeedControllerGroup rightMotors = new SpeedControllerGroup(frontRightMot, rearRightMot); // ----- and using the new differential drive: diffDrive = new DifferentialDrive(leftMotors, rightMotors); // ----- using quadrature encoders on DIO ports: // Encoder(channelA, channelB, reverseDirection, encodingType) // (define direction later based on bot design) leftEncoder = new Encoder(LEFT_ENCODER_CHA, LEFT_ENCODER_CHB, false, Encoder.EncodingType.k4X); rightEncoder = new Encoder(RIGHT_ENCODER_CHA, RIGHT_ENCODER_CHB, false, Encoder.EncodingType.k4X); // ----- encoder setup: leftEncoder.setMinRate(10); // below this rate will report stopped rightEncoder.setMinRate(10); leftEncoder.setSamplesToAverage(7); // for calculating rate rightEncoder.setSamplesToAverage(7); // ========== mechanisms: intakeLeftMot = new VictorSP (INTAKE_LEFT_MOTOR); intakeRightMot = new VictorSP (INTAKE_RIGHT_MOTOR); // liftArmMot = new VictorSP (LIFT_ARM_MOTOR); // winchMot = new VictorSP (WINCH_MOTOR); // ========== bot-specific inits: if (COMPETITION_BOT) { // to monitor motor current draw (not avail in proto bot which uses ancient pdp): pdp = new PowerDistributionPanel(); // // invert, or not, to define which end of the bot is the front: leftMotors.setInverted(true); rightMotors.setInverted(true); liftArmMot.setInverted(true); // // encoders are on opposite sides so make them both read positive for forward: leftEncoder.setReverseDirection(true); rightEncoder.setReverseDirection(false); m_autoInchesPerCount = ENCODER_INCHES_PER_COUNT; leftEncoder.setDistancePerPulse(m_autoInchesPerCount); rightEncoder.setDistancePerPulse(m_autoInchesPerCount); m_autoDrive_kO = AUTO_DRIVE_KO; m_autoStraightMaxSpeed = AUTO_ST_MAX_SPEED; m_autoStraightMinSpeed = AUTO_ST_MIN_SPEED; m_autoStraightBrakeSpeed = AUTO_BRAKE_SPEED; m_autoBeginMedSpeed = AUTO_BEGIN_MED_SPEED; m_autoBeginMinSpeed = AUTO_BEGIN_MIN_SPEED; m_autoBeginBraking = AUTO_BEGIN_BRAKING; m_autoTurnSpeed = AUTO_TURN_SPEED; m_autoTurnHack = AUTO_TURN_HACK; // m_teleopSpeedMax = TELEOP_LINEAR_SPEED_MAX; m_teleopTurnMax = TELEOP_LINEAR_TURN_MAX; } // proto-bot: else { // invert, or not, to define which end of the bot is the front: leftMotors.setInverted(false); rightMotors.setInverted(false); // // encoders are on opposite sides so make them both read positive for forward: leftEncoder.setReverseDirection(false); rightEncoder.setReverseDirection(true); m_autoInchesPerCount = ENCODER_INCHES_PER_COUNT_PROTO; leftEncoder.setDistancePerPulse(m_autoInchesPerCount); rightEncoder.setDistancePerPulse(m_autoInchesPerCount); m_autoDrive_kO = AUTO_DRIVE_KO_PROTO; m_autoStraightMaxSpeed = AUTO_ST_MAX_SPEED_PROTO; m_autoStraightMinSpeed = AUTO_ST_MIN_SPEED_PROTO; m_autoStraightBrakeSpeed = AUTO_BRAKE_SPEED_PROTO; m_autoBeginMedSpeed = AUTO_BEGIN_MED_SPEED_PROTO; m_autoBeginMinSpeed = AUTO_BEGIN_MIN_SPEED_PROTO; m_autoBeginBraking = AUTO_BEGIN_BRAKING_PROTO; m_autoTurnSpeed = AUTO_TURN_SPEED_PROTO; m_autoTurnHack = AUTO_TURN_HACK_PROTO; // m_teleopSpeedMax = TELEOP_LINEAR_SPEED_MAX_PROTO; m_teleopTurnMax = TELEOP_LINEAR_TURN_MAX_PROTO; } // ========== 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; } // ========== misc init: m_currentState = MainState.IDLE; // reset state machine led0.set(true); // active-low (off) clear_DS_displays(); // clear any old mssgs on basic tab // ----- configure analog inputs: AnalogInput.setGlobalSampleRate(4000); } /** * ******************************************************************************* * Called when robot is disabled by driver station or field control system. * Re-init anything necessary when changing between autonomous and teleop modes. * Dashboard string 0 is title * Dashboard string 5 is competition/proto-bot indicator * ******************************************************************************* */ public void disabled() { SmartDashboard.putString( "DB/String 0", ("Start Disabled mode") ); if (CODE_TRACE) { System.out.printf("\nDisabled mode\n"); } // reset state machine: m_currentState = MainState.IDLE; // stop all motors (just in case): diffDrive.tankDrive(0.0, 0.0); intakeLeftMot.set(0.0); intakeRightMot.set(0.0); liftArmMot.set(0.0); winchMot.set(0.0); // drop lift arm gracefully, if needed: initiateLiftArmDownFromAnyHeight(); // clear any old mssgs on basic tab: clear_DS_displays(); // display this so you don't screw up and deploy wrong code: if (COMPETITION_BOT) { SmartDashboard.putString( "DB/String 5", ("Competition Bot") ); } else { SmartDashboard.putString( "DB/String 5", ("Proto Bot") ); } SmartDashboard.putString( "DB/String 0", ("End Disabled mode") ); } /** * ******************************************************************************* * Called when autonomous mode selected by driver station or field control system. * Dashboard string 0 is title * Dashboard string 1 is start position info * Dashboard string 2 is field info * Dashboard string 3 is delay note * * in driveStraight(): * Dashboard string 6 is drive note * Dashboard string 7 is leftError * Dashboard string 8 is rightError * Dashboard string 9 is encoder distance * * in turnNinetyDegreesLeft(): * Dashboard string 6 is l/r turning note * Dashboard string 8 is autoTurnArc distance * Dashboard string 9 is encoder distance * ******************************************************************************* */ public void autonomous() { String scoringSides; char ownSwitchPos; char scalePos; char opponentSwitchPos; AutoStartPosition autonomousStartPosition; boolean autoTenFootOnly = false; SmartDashboard.putString( "DB/String 0", ("Autonomous mode") ); if (CODE_TRACE) { System.out.printf("Autonomous mode\n"); } // ========== get switches on operator console: // switches set which start: one of three positions: AUTO_LEFT_START, AUTO_CENTER_START, AUTO_RIGHT_START if (launchpad.getRawButton(LAUNCHPAD_BTN_1_LEFT)) { autonomousStartPosition = AutoStartPosition.AUTO_LEFT_START; // init to left starting position SmartDashboard.putString( "DB/String 1", ("Left Start") ); if (CODE_TRACE) { System.out.printf("Left Start\n"); } } else if (launchpad.getRawButton(LAUNCHPAD_BTN_3_RIGHT)) { autonomousStartPosition = AutoStartPosition.AUTO_RIGHT_START; // init to right starting position SmartDashboard.putString( "DB/String 1", ("Right Start") ); if (CODE_TRACE) { System.out.printf("Right Start\n"); } } else { autonomousStartPosition = AutoStartPosition.AUTO_CENTER_START; // init to center starting position SmartDashboard.putString( "DB/String 1", ("Center Start") ); if (CODE_TRACE) { System.out.printf("Center Start\n"); } } // if (launchpad.getRawButton(LAUNCHPAD_BTN_2_CENTER)) { if (AUTO_DRIVE_SWITCHED_TEN_FT_ONLY) { autoTenFootOnly = true; // only a 10 foot auto run, IF blue switch on } } // ========== get game-specific instructions: scoringSides = DriverStation.getInstance().getGameSpecificMessage(); ownSwitchPos = scoringSides.charAt(0); scalePos = scoringSides.charAt(1); // unused opponentSwitchPos = scoringSides.charAt(2); // unused SmartDashboard.putString( "DB/String 2", ("Field info: " + ownSwitchPos + ", " + scalePos + ", " + opponentSwitchPos) ); if (CODE_TRACE) { System.out.printf("Field info: %c, %c, %c\n", ownSwitchPos, scalePos, opponentSwitchPos); } // ========== init things: diffDrive.setSafetyEnabled(false); // disable motor timeout m_currentState = MainState.IDLE; // reset state machine // ========== if pre-autonomous delay selected (for coordination with other team): if (AUTO_USE_PRE_DELAY) { SmartDashboard.putString( "DB/String 3", ("Delay...") ); Timer.delay(AUTO_PRE_DELAY_TIME); SmartDashboard.putString( "DB/String 3", ("") ); } // ========== just simple straight 120" drive (half of robot past auto line): if (autoTenFootOnly) { SmartDashboard.putString( "DB/String 0", ("Auto 10ft only") ); driveStraightInches( INCHES_TO_AUTOLINE - (BOT_LENGTH / 2.0) ); return; } // ========== or normal autonomous runs: else { switch (autonomousStartPosition) { case AUTO_CENTER_START: default: // ----- going for right switch plate directly in front of us: if (ownSwitchPos == 'R') { // drive fwd to switch plate: driveStraightInches( DISTANCE_CENTER_FWD_TO_PLATE - BOT_LENGTH ); // lift arm to switch height, eject cube out front, and lower arm: liftToSwitchEjectAndLower(); return; } // ----- else going for left switch plate to our left: else if (ownSwitchPos == 'L') { // drive fwd a 'lil bit: driveStraightInches( DISTANCE_LIL_BIT_FWD - (BOT_LENGTH / 2.0) ); // rotate 90 degrees to the left: turnInPlace(AutoTurn.AUTO_TURN_90_LEFT); // drive fwd to line up with switch centerline: driveStraightInches( DISTANCE_CENTER_TO_LEFT_PLATE ); // rotate 90 degrees to the right: turnInPlace(AutoTurn.AUTO_TURN_90_RIGHT); // drive fwd to switch plate: driveStraightInches( DISTANCE_REST_OF_THE_WAY_FWD - (BOT_LENGTH / 2.0) ); // lift arm to switch height, eject cube out front, and lower arm: liftToSwitchEjectAndLower(); return; } break; case AUTO_LEFT_START: // ----- going for left switch plate in front of us: if (ownSwitchPos == 'L') { // drive fwd to line up with horizontal centerline of left switch plate: driveStraightInches( DISTANCE_TO_SWITCH_CENTER - (BOT_LENGTH / 2.0) ); // rotate 90 degrees to the right: turnInPlace(AutoTurn.AUTO_TURN_90_RIGHT); // drive fwd to switch plate: driveStraightInches( DISTANCE_TO_SWITCH_FROM_SIDE - (BOT_LENGTH / 2.0) ); // lift arm to switch height, eject cube out front, and lower arm: liftToSwitchEjectAndLower(); // drive back away from switch plate: driveStraightInches( -DISTANCE_A_TAD_BACK - (BOT_LENGTH / 2.0) ); // rotate 90 degrees to the left: turnInPlace(AutoTurn.AUTO_TURN_90_LEFT); // drive fwd to switch plate: driveStraightInches( DISTANCE_A_TAD_FWD - (BOT_LENGTH / 2.0) ); return; } // ----- else going for right switch plate on opposite side: else if (ownSwitchPos == 'R') { SmartDashboard.putString( "DB/String 0", ("Auto 10ft only") ); driveStraightInches( INCHES_TO_AUTOLINE - (BOT_LENGTH / 2.0) ); // delay to let center alliance bot get out of the way: //// Timer.delay(2.0); // drive fwd a 'lil bit: //// driveStraightInches( DISTANCE_LIL_BIT_FWD - (BOT_LENGTH / 2.0) ); // rotate 90 degrees to the right: //// turnInPlace(AutoTurn.AUTO_TURN_90_RIGHT); // drive fwd to line up with switch centerline: //// driveStraightInches( DISTANCE_LR_TO_OPPOSITE_PLATE ); // rotate 90 degrees to the left: //// turnInPlace(AutoTurn.AUTO_TURN_90_LEFT); // drive fwd to switch plate: //// driveStraightInches( DISTANCE_REST_OF_THE_WAY_FWD - (BOT_LENGTH / 2.0) ); // lift arm to switch height, eject cube out front, and lower arm: //// liftToSwitchEjectAndLower(); return; } break; case AUTO_RIGHT_START: // ----- going for right switch plate in front of us: if (ownSwitchPos == 'R') { // drive fwd to line up with horizontal centerline of right switch plate: driveStraightInches( DISTANCE_TO_SWITCH_CENTER - (BOT_LENGTH / 2.0) ); // rotate 90 degrees to the left: turnInPlace(AutoTurn.AUTO_TURN_90_LEFT); // drive fwd to switch plate: driveStraightInches( DISTANCE_TO_SWITCH_FROM_SIDE - (BOT_LENGTH / 2.0) ); // lift arm to switch height, eject cube out front, and lower arm: liftToSwitchEjectAndLower(); // drive back away from switch plate: driveStraightInches( -DISTANCE_A_TAD_BACK - (BOT_LENGTH / 2.0) ); // rotate 90 degrees to the right: turnInPlace(AutoTurn.AUTO_TURN_90_RIGHT); // drive fwd to switch plate: driveStraightInches( DISTANCE_A_TAD_FWD - (BOT_LENGTH / 2.0) ); return; } // ----- else going for left switch plate on opposite side: else if (ownSwitchPos == 'L') { SmartDashboard.putString( "DB/String 0", ("Auto 10ft only") ); driveStraightInches( INCHES_TO_AUTOLINE - (BOT_LENGTH / 2.0) ); // delay to let center alliance bot get out of the way: //// Timer.delay(2.0); // drive fwd a 'lil bit: //// driveStraightInches( DISTANCE_LIL_BIT_FWD - (BOT_LENGTH / 2.0) ); // rotate 90 degrees to the left: //// turnInPlace(AutoTurn.AUTO_TURN_90_LEFT); // drive fwd to line up with switch centerline: //// driveStraightInches( DISTANCE_LR_TO_OPPOSITE_PLATE ); // rotate 90 degrees to the right: //// turnInPlace(AutoTurn.AUTO_TURN_90_RIGHT); // drive fwd to switch plate: //// driveStraightInches( DISTANCE_REST_OF_THE_WAY_FWD - (BOT_LENGTH / 2.0) ); // lift arm to switch height, eject cube out front, and lower arm: //// liftToSwitchEjectAndLower(); return; } break; } // (end of switch) } // (end of else normal autonomous) } /** * ******************************************************************************* * Called when teleop mode selected by driver station or field control system. * Runs the motors with 2-stick arcade. * Dashboard string 0 is title * Dashboard string 1 is state * Dashboard string 2 is diagnostic * Dashboard string 3 is lift arm amps (lift arm methods) * Dashboard string 4 is encoder reading (lift arm methods) * Dashboard string 6 is state machine diag * ******************************************************************************* */ 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 smot_prev = 0.0; double sjoy; double tjoy; double tmot; double tmot_prev = 0.0; double winchSpeed = 0.0; double inSpeed = 0.0; double outSpeed = 0.0; // ===== init things: diffDrive.setSafetyEnabled(false); // disable motor timeout m_currentState = MainState.IDLE; // reset state machine // ========== main teleop loop: while (isOperatorControl() && isEnabled()) { // clear fields we use here: SmartDashboard.putString( "DB/String 2", ("") ); // ===== driving (2-stick arcade with adjustable profiles and deadband): // // --- get the left/right joystick Y axes (calibrate with errors read at init???): sjoy = xbox0.getRawAxis(XBOX_L_YAXIS); // fwd/rev speed tjoy = xbox0.getRawAxis(XBOX_R_XAXIS); // left/right turning // // --- enforce a small stick deadband: if (Math.abs(sjoy) < JOYSTICK_DEADBAND) sjoy = 0.0; if (Math.abs(tjoy) < JOYSTICK_DEADBAND) tjoy = 0.0; // if (JOY_USE_CUBIC_SPEED) { // --- linear-to-cubic profile for the speed joystick: smot = JOY_CUBIC_SPEED_SHAPE * (Math.pow(sjoy, 3)) + (1.0 - JOY_CUBIC_SPEED_SHAPE) * sjoy; } else { // linear for the speed joystick: smot = m_teleopSpeedMax * sjoy; } // // --- limit forward/backward acceleration in positive dir: if (smot > 0.0) { // only limit acceleration on large positive-going increments (don't limit deceleration): if ( (smot > smot_prev) && ((smot - smot_prev) > MAX_MOTOR_INCREMENT) ) { smot = smot_prev + MAX_MOTOR_INCREMENT; } } // and also limit forward/backward acceleration in negative dir: else { // only limit acceleration on large negative-going increments (don't limit deceleration): if ( (smot < smot_prev) && ((smot_prev - smot) > MAX_MOTOR_INCREMENT) ) { smot = smot_prev - MAX_MOTOR_INCREMENT; } } // save for next loop: smot_prev = smot; // // --- linear is fine for the turn joystick (negative to invert stick direction): tmot = -m_teleopTurnMax * tjoy; // // --- limit left/right acceleration in positive dir: if (tmot > 0.0) { // only limit acceleration on large positive-going increments (don't limit deceleration): if ( (tmot > tmot_prev) && ((tmot - tmot_prev) > MAX_MOTOR_INCREMENT) ) { tmot = tmot_prev + MAX_MOTOR_INCREMENT; } } // and also limit left/right acceleration in negative dir: else { // only limit acceleration on large negative-going increments (don't limit deceleration): if ( (tmot < tmot_prev) && ((tmot_prev - tmot) > MAX_MOTOR_INCREMENT) ) { tmot = tmot_prev - MAX_MOTOR_INCREMENT; } } // save for next loop: tmot_prev = tmot; // ----- whew, finally, drive the motors already: diffDrive.arcadeDrive(smot, tmot); // ===== xbox controller button A is to lift arm to Switch: if (xbox0.getRawButton(XBOX_BTN_A)) { initiateLiftArmToSwitch(); } // ===== xbox controller button B is to drop arm down: if (xbox0.getRawButton(XBOX_BTN_B)) { initiateLiftArmDownFromAnyHeight(); } // ===== xbox controller button X is unused: ////if (xbox0.getRawButton(XBOX_BTN_X)) { ////} // ===== xbox controller button Y is to lift arm to Scale: if (xbox0.getRawButton(XBOX_BTN_Y)) { initiateLiftArmToScale(); } // ===== xbox controller right trigger is Intake In or Out-Rear, // and left trigger is Intake Out-Front: inSpeed = xbox0.getRawAxis(XBOX_R_TRIGGER) * INTAKE_IN_MAX_SPEED; outSpeed = xbox0.getRawAxis(XBOX_L_TRIGGER) * INTAKE_OUT_MAX_SPEED; // // --- direction is in (right trigger): if (inSpeed > 0.08) { // are we at floor sucking in the cube? if (analog0.getValue() < LIFT_ARM_HEIGHT_12_INCHES) { // is cube sucked in? ///////////////////////////////////////////////// if (checkLidar() < 1) { intakeMotorsIn(MOTOR_OFF); SmartDashboard.putString( "DB/String 2", ("lidar limit reached") ); } // or not quite yet? else { intakeMotorsIn(inSpeed); SmartDashboard.putString( "DB/String 2", ("sucky-in speed: " + String.valueOf( String.format("%.2f", inSpeed ) ) ) ); } } // or we have lifted and are ejecting out the rear? else { intakeMotorsIn(inSpeed); SmartDashboard.putString( "DB/String 2", ("rear-eject speed: " + String.valueOf( String.format("%.2f", inSpeed ) ) ) ); } } // --- direction is out (left trigger): else if (outSpeed > 0.08) { // we are ejecting out the front: intakeMotorsOut(outSpeed); SmartDashboard.putString( "DB/String 2", ("front-eject speed: " + String.valueOf( String.format("%.2f", outSpeed ) ) ) ); } // motors off when not pressed: else { intakeMotorsIn(MOTOR_OFF); } // ===== Right Bumper (on/off) is unused: ////if (xbox0.getRawButton(XBOX_BUMPER_R)) { ////} // ===== Left Bumper (on/off) is unused: ////if (xbox0.getRawButton(XBOX_BUMPER_L)) { ////} // ===== SECONDARY xbox controller right trigger for winch: winchSpeed = xbox2.getRawAxis(XBOX_R_TRIGGER) * WINCH_MAX_SPEED; if (winchSpeed > 0.08) { // MUST BE NEGATIVE OR MOTOR JAMS AGAINST PAWL, BREAKING STUFF: winchMot.set(-winchSpeed); SmartDashboard.putString( "DB/String 2", ("winch speed: " + String.valueOf( String.format("%.2f", winchSpeed ) ) ) ); } // motor off when not pressed: else { winchMot.set(MOTOR_OFF); } // ===== SECONDARY xbox controller button B is to shut everything down in case of problem: if (xbox2.getRawButton(XBOX_BTN_B)) { m_currentState = MainState.SHUTDOWN; } // ===== always call state machine (general magic, timeouts...): stateMachine(); // ===== end of loop stuff: b = !b; // toggle led to measure loop time with scope if (b) { led0.set(false); } // active-low -- light on else { led0.set(true); } // active-low -- light off Timer.delay(m_loopTime); // loop delay } // (end of teleop loop) } /** * ******************************************************************************* * Called when test mode selected by driver station. * * Dashboard string 0 is title * * In auto-calibrate mode: * Dashboard string 1 is auto-calibrate/test note * Dashboard string 2 is leftInches * Dashboard string 3 is rightInches * Dashboard string 4 is averageInches * * Lift arm: * Dashboard string 3 is lift arm amps (lift arm methods) * Dashboard string 4 is encoder reading (lift arm methods) * ******************************************************************************* */ public void test() { boolean butt0, butt1, butt2, butt3; if (CODE_TRACE) { System.out.printf("Test mode\n"); } SmartDashboard.putString( "DB/String 0", ("Test mode") ); // reset state machine: m_currentState = MainState.IDLE; // ----- this only needed for auto calibrate: leftEncoder.reset(); rightEncoder.reset(); while (isEnabled()) { if (TEST_AUTO_CALIBRATE_MODE) { SmartDashboard.putString( "DB/String 1", "Auto-Calibrate Mode"); SmartDashboard.putString( "DB/String 2", ("leftInch: " + String.valueOf( String.format("%.1f", leftEncoder.getDistance() ) ) ) ); SmartDashboard.putString( "DB/String 3", ("rightInch: " + String.valueOf( String.format("%.1f", rightEncoder.getDistance() ) ) ) ); SmartDashboard.putString( "DB/String 4", ("avgInch: " + String.valueOf( String.format("%.1f", (leftEncoder.getDistance() + rightEncoder.getDistance() ) / 2.0 ) ) ) ); } else if (TEST_AUTO_ST_DRIVE_TUNE_MODE) { SmartDashboard.putString( "DB/String 1", ("drive inches: " + String.valueOf( String.format("%.1f", TEST_AUTO_ST_DRIVE_INCHES ) ) ) ); // drive straight and then stop (dashboard strings set here): driveStraightInches(TEST_AUTO_ST_DRIVE_INCHES); return; } else if (TEST_AUTO_TURN_TUNE_MODE) { SmartDashboard.putString( "DB/String 1", "turn 90 degrees:"); SmartDashboard.putString( "DB/String 2", "Butt 2 for left"); SmartDashboard.putString( "DB/String 3", "Butt 3 for right"); SmartDashboard.putBoolean("DB/Button 0", false); SmartDashboard.putBoolean("DB/Button 1", false); // button 2 ON selects left turn: butt2 = SmartDashboard.getBoolean("DB/Button 2", false); if (butt2) { // turn button off: SmartDashboard.putBoolean("DB/Button 2", false); // turn 90 degrees left, then stop (dashboard strings set here): turnInPlace(AutoTurn.AUTO_TURN_90_LEFT); } // button 3 ON selects right turn: butt3 = SmartDashboard.getBoolean("DB/Button 3", false); if (butt3) { SmartDashboard.putBoolean("DB/Button 3", false); // turn 90 degrees right, then stop (dashboard strings set here): turnInPlace(AutoTurn.AUTO_TURN_90_RIGHT); } } else if (TEST_INTAKE_MODE) { SmartDashboard.putString( "DB/String 1", "test intake wheels:"); SmartDashboard.putString( "DB/String 2", "B0: off"); SmartDashboard.putString( "DB/String 3", "B2:in B3:out"); // button 0 ON selects wheels off: butt0 = SmartDashboard.getBoolean("DB/Button 0", false); if (butt0) { SmartDashboard.putBoolean("DB/Button 0", false); intakeMotorsIn(MOTOR_OFF); } // button 2 ON selects wheels in butt2 = SmartDashboard.getBoolean("DB/Button 2", false); if (butt2) { SmartDashboard.putBoolean("DB/Button 2", false); intakeMotorsIn(INTAKE_IN_MAX_SPEED); } // button 3 ON selects wheels out: butt3 = SmartDashboard.getBoolean("DB/Button 3", false); if (butt3) { SmartDashboard.putBoolean("DB/Button 3", false); intakeMotorsOut(INTAKE_OUT_MAX_SPEED); } } else if (TEST_LIFT_ARM_SWITCH_MODE) { SmartDashboard.putString( "DB/String 1", "test lift arm to switch:"); SmartDashboard.putString( "DB/String 6", "switch B0:up B1:eject"); // button 0 ON selects lift up to switch: butt0 = SmartDashboard.getBoolean("DB/Button 0", false); if (butt0) { SmartDashboard.putBoolean("DB/Button 0", false); initiateLiftArmToSwitch(); } // button 1 ON selects eject out front: butt1 = SmartDashboard.getBoolean("DB/Button 1", false); if (butt1) { // turn button off: SmartDashboard.putBoolean("DB/Button 1", false); ejectOutFront(); } } else if (TEST_LIFT_ARM_SCALE_MODE) { SmartDashboard.putString( "DB/String 1", "test lift arm to scale:"); SmartDashboard.putString( "DB/String 6", "switch B0:up B1:eject"); // button 0 ON selects lift up to scale: butt0 = SmartDashboard.getBoolean("DB/Button 0", false); if (butt0) { SmartDashboard.putBoolean("DB/Button 0", false); initiateLiftArmToScale(); } // button 1 ON selects eject out front: butt1 = SmartDashboard.getBoolean("DB/Button 1", false); if (butt1) { // turn button off: SmartDashboard.putBoolean("DB/Button 1", false); ejectOutFront(); } } else if (TEST_LIFT_ARM_SENSOR_MODE) { SmartDashboard.putString( "DB/String 1", "lift arm sensor (counts):"); SmartDashboard.putString( "DB/String 2", String.valueOf(analog0.getValue()) ); } else if (TEST_LIDAR_MODE) { SmartDashboard.putString( "DB/String 1", "test lidar (cm):"); SmartDashboard.putString( "DB/String 2", String.valueOf(checkLidar()) ); } Timer.delay(m_loopTime); // loop delay } } /** * ******************************************************************************* * Clear text boxes and leds on driverstation Basic tab. * ******************************************************************************* */ public void clear_DS_displays() { SmartDashboard.putString( "DB/String 0", "" ); 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 ); SmartDashboard.putBoolean("DB/Button 0", false); SmartDashboard.putBoolean("DB/Button 1", false); SmartDashboard.putBoolean("DB/Button 2", false); SmartDashboard.putBoolean("DB/Button 3", false); } /** * ******************************************************************************* * Motion-profiled drive straight for distanceInches using encoders (blocking). * Tune terms for speeds and segment percentages, and steering offset KO (left/right equalization) * * Dashboard string 6 is drive note * Dashboard string 7 is leftError * Dashboard string 8 is rightError * Dashboard string 9 is distance * Dashboard led3 flashes while loop active * ******************************************************************************* */ public void driveStraightInches(double distanceInches) { double stallTime = 0.0; double leftError = 0.0; double rightError = 0.0; double leftErrorNorm = 0.0; double rightErrorNorm = 0.0; double deltaLRNorm = 0.0; double leftDrive = 0.0; double rightDrive = 0.0; double lrOffset = 0.0; int i = 0; boolean b = false; double encoderDistance = 0.0; double encoderPreviousDistance = 0.0; boolean dirRev = false; double autoBeginMedSpeed = 0.0; double autoBeginMinSpeed = 0.0; double autoBeginBraking = 0.0; DriveStraightState currentState; SmartDashboard.putString( "DB/String 6", ("driveInches: " + String.valueOf( String.format("%.1f", distanceInches ) ) ) ); if (TEST_AUTO_DRIVE_TRACING) { System.out.printf("----- driveStraightInches %.2f \n", distanceInches); } if (TEST_AUTO_DRIVE_TRACING) { System.out.printf("distance, Lerr, Rerr, LRoffset, Ldrive, Rdrive)\n"); } // ----- set encoder count etc. to zero: leftEncoder.reset(); rightEncoder.reset(); // ----- misc init: currentState = DriveStraightState.PROFILED; // profiled driving is first stallTime = 0.0; // for detecting motor stall if (distanceInches < 0) { dirRev = true; } // which way are we going? else { dirRev = false; } // ----- hack to adjust distance thresholds for shorter distances: if (distanceInches < 50) { autoBeginMedSpeed = m_autoBeginMedSpeed * 0.40; autoBeginMinSpeed = m_autoBeginMinSpeed * 0.40; autoBeginBraking = m_autoBeginBraking * 0.40; } else { autoBeginMedSpeed = m_autoBeginMedSpeed; autoBeginMinSpeed = m_autoBeginMinSpeed; autoBeginBraking = m_autoBeginBraking; } // ========== loop until drive complete (position tolerance exit, or 200 ms of motor stall): while (stallTime < 0.2) { // exit on disable so we don't block that critical function: if (!isEnabled()) { diffDrive.tankDrive(MOTOR_OFF, MOTOR_OFF); return; } // toggle led3 every ten times through loop so we know loop is running: if (10 == i++) { i = 0; b = !b; } if (b) SmartDashboard.putBoolean("DB/LED 3", false); else SmartDashboard.putBoolean("DB/LED 3", true); // average distance traveled: encoderDistance = ( leftEncoder.getDistance() + rightEncoder.getDistance() ) / 2.0; // average SmartDashboard.putString( "DB/String 9", ("encDistance: " + String.valueOf( String.format("%.1f", encoderDistance ) ) ) ); if (TEST_AUTO_DRIVE_TRACING) { System.out.printf("\n%.1f, ", encoderDistance); } // calc current distance errors: // (encoders must both count positive for forward, which is set in init) rightError = distanceInches - rightEncoder.getDistance(); leftError = distanceInches - leftEncoder.getDistance(); SmartDashboard.putString( "DB/String 7", ("leftError: " + String.valueOf( String.format("%.1f", leftError) ) ) ); SmartDashboard.putString( "DB/String 8", ("rightError: " + String.valueOf( String.format("%.1f", rightError) ) ) ); if (TEST_AUTO_DRIVE_TRACING) { System.out.printf("%.1f, %.1f, ", leftError, rightError); } // normalize current distance errors and calc normalized LR delta: leftErrorNorm = leftError / Math.abs( distanceInches ); rightErrorNorm = rightError / Math.abs( distanceInches ); deltaLRNorm = leftErrorNorm - rightErrorNorm; ////if (TEST_AUTO_DRIVE_TRACING) { System.out.printf("%.3f, %.3f, %.4f, ", leftErrorNorm, rightErrorNorm, deltaLRNorm); } // calc left-right offset term: lrOffset = m_autoDrive_kO * deltaLRNorm; if (TEST_AUTO_DRIVE_TRACING) { System.out.printf("%.4f, ", lrOffset); } switch (currentState) { case PROFILED: default: // max speed segment: if ((encoderDistance / distanceInches) < autoBeginMedSpeed) { leftDrive = m_autoStraightMaxSpeed; rightDrive = m_autoStraightMaxSpeed; if (dirRev) { leftDrive = -leftDrive; rightDrive = -rightDrive; } } // medium speed segment: else if ((encoderDistance / distanceInches) < autoBeginMinSpeed) { leftDrive = (m_autoStraightMaxSpeed + m_autoStraightMinSpeed) / 2.0; rightDrive = (m_autoStraightMaxSpeed + m_autoStraightMinSpeed) / 2.0; if (dirRev) { leftDrive = -leftDrive; rightDrive = -rightDrive; } } // min speed segment: else if ((encoderDistance / distanceInches) < autoBeginBraking) { leftDrive = m_autoStraightMinSpeed; rightDrive = m_autoStraightMinSpeed; if (dirRev) { leftDrive = -leftDrive; rightDrive = -rightDrive; } } // we have reached our braking point (pun intended): else { currentState = DriveStraightState.BRAKING; if (TEST_AUTO_DRIVE_TRACING) { System.out.printf(" (braking next) "); } } break; case BRAKING: leftDrive = -m_autoStraightBrakeSpeed; rightDrive = -m_autoStraightBrakeSpeed; if (dirRev) { leftDrive = -leftDrive; rightDrive = -rightDrive; } // brake until delta less than 0.4" per 10ms loop time: if ( Math.abs(encoderDistance - encoderPreviousDistance) < 0.4) { currentState = DriveStraightState.CRAWLING; if (TEST_AUTO_DRIVE_TRACING) { System.out.printf(" (crawl next) "); } } break; case CRAWLING: leftDrive = m_autoStraightMinSpeed; rightDrive = m_autoStraightMinSpeed; if (dirRev) { leftDrive = -leftDrive; rightDrive = -rightDrive; } // ----- exit? if (dirRev) { if ( encoderDistance < distanceInches ) { //stop motors: diffDrive.tankDrive(MOTOR_OFF, MOTOR_OFF); // ensure led off (was toggling): SmartDashboard.putBoolean( "DB/LED 3", false ); if (TEST_AUTO_DRIVE_TRACING) { System.out.printf("\n--- tolerance exit (rev) at %.1f inches\n", encoderDistance); } return; } } else { if ( encoderDistance > distanceInches ) { //stop motors: diffDrive.tankDrive(MOTOR_OFF, MOTOR_OFF); // ensure led off (was toggling): SmartDashboard.putBoolean( "DB/LED 3", false ); if (TEST_AUTO_DRIVE_TRACING) { System.out.printf("\n--- tolerance exit (fwd) at %.1f inches\n", encoderDistance); } return; } } break; } // add L/R offset term to steer straight: leftDrive = leftDrive + lrOffset; rightDrive = rightDrive - lrOffset; if (TEST_AUTO_DRIVE_TRACING) { System.out.printf("%.2f, %.2f", leftDrive, rightDrive); } // tankDrive(leftvalue, rightvalue) -- 0 is stopped, -1.0 is max forward, 1.0 is max reverse: diffDrive.tankDrive(-leftDrive, -rightDrive); // are we sitting stalled since motor drive is too low to move? if (encoderDistance == encoderPreviousDistance) { stallTime += m_loopTime; } encoderPreviousDistance = encoderDistance; } // stall exit -- stop motors: diffDrive.tankDrive(MOTOR_OFF, MOTOR_OFF); // ensure led off (was toggling): SmartDashboard.putBoolean( "DB/LED 3", false ); if (TEST_AUTO_DRIVE_TRACING) { System.out.printf("\n--- stall exit at %.1f inches\n", encoderDistance); } } /** * ******************************************************************************* * Turn robot in place (blocking). Only implemented 90 degrees, either left or right * * Dashboard string 6 is l/r turning note * Dashboard string 8 is autoTurnArc distance * Dashboard string 9 is encoder distance * ******************************************************************************* */ public void turnInPlace(AutoTurn whichTurn) { double autoTurnArc = 0.0; double encoderDistance = 0.0; //turn arc length for rotation to 90 degrees = (hack * AXLE_TRACK * pi) (90/360) // (hacked since it does not work as theory says it should -- sheesh) autoTurnArc = m_autoTurnHack * AXLE_TRACK * 0.7854; SmartDashboard.putString( "DB/String 8", ("autoTurnArc: " + String.valueOf( String.format("%.2f", autoTurnArc ) ) ) ); leftEncoder.reset(); // reset encoders to 0 rightEncoder.reset(); SmartDashboard.putString( "DB/String 7", ""); // clear if (AutoTurn.AUTO_TURN_90_LEFT == whichTurn) { SmartDashboard.putString( "DB/String 6", "turning 90 left"); if (TEST_AUTO_DRIVE_TRACING) { System.out.printf("\n----- turnInPlace left: %.4f", autoTurnArc); } diffDrive.tankDrive(m_autoTurnSpeed, -m_autoTurnSpeed); } else { SmartDashboard.putString( "DB/String 6", "turning 90 right"); if (TEST_AUTO_DRIVE_TRACING) { System.out.printf("\n----- turnInPlace right: %.4f", autoTurnArc); } diffDrive.tankDrive(-m_autoTurnSpeed, m_autoTurnSpeed); } do { // exit on disable so we don't block that critical function: if (!isEnabled()) { diffDrive.tankDrive(MOTOR_OFF, MOTOR_OFF); return; } encoderDistance = ( Math.abs(leftEncoder.getDistance()) + Math.abs(rightEncoder.getDistance()) ) / 2.0; // average SmartDashboard.putString( "DB/String 9", ("distance: " + String.valueOf( String.format("%.2f", encoderDistance ) ) ) ); if (TEST_AUTO_DRIVE_TRACING) { System.out.printf("\ndistance: %.4f", encoderDistance); } } while (encoderDistance < autoTurnArc); // stop motors: diffDrive.tankDrive(MOTOR_OFF, MOTOR_OFF); } /** * ******************************************************************************* * Limit double parameter from min to max * ******************************************************************************* */ public double limit(double min, double max, double param) { if (param < min) { return min; } else if (param > max) { return max; } else { return param; } } /** * ******************************************************************************* * Intake motors In * ******************************************************************************* */ public void intakeMotorsIn(double speed) { intakeLeftMot.set(speed); intakeRightMot.set(-speed); } /** * ******************************************************************************* * Intake motors Out * ******************************************************************************* */ public void intakeMotorsOut(double speed) { intakeLeftMot.set(-speed); intakeRightMot.set(speed); } /** * ******************************************************************************* * Eject out front (blocking) * ******************************************************************************* */ public void ejectOutFront() { intakeMotorsOut(INTAKE_OUT_MAX_SPEED); Timer.delay(2.0); intakeMotorsOut(MOTOR_OFF); } /** * ******************************************************************************* * lift arm to switch height, eject cube out front, and lower arm (blocking) * ******************************************************************************* */ public void liftToSwitchEjectAndLower() { if (LIFT_ENABLED) { // start lifting arm: initiateLiftArmToSwitch(); // wait until arm lifted: while ( m_currentState != MainState.LIFT_ARM_HOLDING_UP ) { // exit on disable so we don't block that critical function: if (!isEnabled()) { return; } stateMachine(); Timer.delay(m_loopTime); } // eject cube: ejectOutFront(); // start lowering arm: initiateLiftArmDownFromAnyHeight(); // wait until arm lowered: while (m_currentState != MainState.IDLE) { // exit on disable so we don't block that critical function: if (!isEnabled()) { return; } stateMachine(); Timer.delay(m_loopTime); } } } /** * ******************************************************************************* * Lift arm to switch (using analog string potentiometer reading) * * Dashboard string 3 is lift arm amps * Dashboard string 4 is encoder reading ******************************************************************************* */ public void initiateLiftArmToSwitch() { if (LIFT_ENABLED) { m_currentState = MainState.LIFT_SWITCH_STAGE1_UP; } } /** * ******************************************************************************* * Lift arm to scale (using analog string potentiometer reading) * * Dashboard string 3 is lift arm amps * Dashboard string 4 is encoder reading * ******************************************************************************* */ public void initiateLiftArmToScale() { if (LIFT_ENABLED) { m_currentState = MainState.LIFT_SCALE_STAGE1_UP; } } /** * ******************************************************************************* * Lift arm down from any height * * Dashboard string 3 is lift arm amps * Dashboard string 4 is encoder reading * ******************************************************************************* */ public void initiateLiftArmDownFromAnyHeight() { if (LIFT_ENABLED) { m_currentState = MainState.LIFT_STAGE1_DOWN; } } /** * ******************************************************************************* * Check lidar on the front of bot 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 checkLidar() { int lidarRaw = 0; int lidarDistance = 0; int[] lidarHistoryBuffer = new int[LIDAR_HISTORY_BUFF_SIZE]; int bufIndex = 0; int i; 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); lidarRaw = (int)(m_i2cRx8[0]); // is lidar value valid (trap bogus data)? if (lidarRaw < 1) { lidarRaw = 120; } // power-up season tweak: limit far distance (so it does not skew average too much) and add to buffer: if (lidarRaw > 50) { lidarRaw = 50; } // lidarHistoryBuffer[bufIndex++] = lidarRaw; // wrap circular buffer as needed: if (LIDAR_HISTORY_BUFF_SIZE == bufIndex) { bufIndex = 0; } // average buffer: for (i=0; i LIFT_ARM_HEIGHT_DOWN_STAGE1) { liftArmMot.set(-LIFT_STAGE1_SPEED_DOWN); // neg for down liftMotorAmps = pdp.getCurrent(LIFT_ARM_MOTOR_SENSE); SmartDashboard.putString( "DB/String 3", ("Intake amps: " + String.valueOf( String.format("%.1f", liftMotorAmps ) ) ) ); SmartDashboard.putString( "DB/String 4", ("encReading: " + String.valueOf( encReading ) ) ); } else { m_currentState = MainState.LIFT_STAGE2_DOWN; } break; case LIFT_STAGE2_DOWN: SmartDashboard.putString( "DB/String 1", ("LIFT_STAGE2_DOWN") ); encReading = analog0.getValue(); if (encReading > LIFT_ARM_HEIGHT_DOWN_STAGE2) { liftArmMot.set(-LIFT_STAGE2_SPEED_DOWN); // neg for down liftMotorAmps = pdp.getCurrent(LIFT_ARM_MOTOR_SENSE); SmartDashboard.putString( "DB/String 3", ("Intake amps: " + String.valueOf( String.format("%.1f", liftMotorAmps ) ) ) ); SmartDashboard.putString( "DB/String 4", ("encReading: " + String.valueOf( encReading ) ) ); } else { m_currentState = MainState.LIFT_STAGE3_DOWN; } break; case LIFT_STAGE3_DOWN: SmartDashboard.putString( "DB/String 1", ("LIFT_STAGE3_DOWN") ); encReading = analog0.getValue(); if (encReading > LIFT_ARM_HEIGHT_DOWN_STAGE3) { liftArmMot.set(-LIFT_STAGE3_SPEED_DOWN); // neg for down liftMotorAmps = pdp.getCurrent(LIFT_ARM_MOTOR_SENSE); SmartDashboard.putString( "DB/String 3", ("Intake amps: " + String.valueOf( String.format("%.1f", liftMotorAmps ) ) ) ); SmartDashboard.putString( "DB/String 4", ("encReading: " + String.valueOf( encReading ) ) ); } else { m_currentState = MainState.LIFT_STAGE4_DOWN; } break; case LIFT_STAGE4_DOWN: SmartDashboard.putString( "DB/String 1", ("LIFT_STAGE4_DOWN") ); encReading = analog0.getValue(); if (encReading > LIFT_ARM_HEIGHT_FLOOR) { liftArmMot.set(-LIFT_STAGE4_SPEED_DOWN); // neg for down liftMotorAmps = pdp.getCurrent(LIFT_ARM_MOTOR_SENSE); SmartDashboard.putString( "DB/String 3", ("Intake amps: " + String.valueOf( String.format("%.1f", liftMotorAmps ) ) ) ); SmartDashboard.putString( "DB/String 4", ("encReading: " + String.valueOf( encReading ) ) ); } else { liftArmMot.set(-LIFT_SPEED_HOLDING_DOWN); // neg for down m_elapsedTime = 0.0; m_currentState = MainState.LIFT_ARM_HOLDING_DOWN; } break; case LIFT_ARM_HOLDING_UP: SmartDashboard.putString( "DB/String 1", ("LIFT_ARM_HOLDING_UP") ); // timeout if it takes too long: m_elapsedTime += m_loopTime; SmartDashboard.putString( "DB/String 2", ("elapsed time: " + String.valueOf( String.format("%.1f", m_elapsedTime ) ) ) ); if (m_elapsedTime > 10.0) { m_currentState = MainState.LIFT_ARM_TIMEOUT_UP; } break; case LIFT_ARM_TIMEOUT_UP: SmartDashboard.putString( "DB/String 1", ("LIFT_ARM_TIMEOUT_UP") ); intakeMotorsIn(MOTOR_OFF); initiateLiftArmDownFromAnyHeight(); break; case LIFT_ARM_HOLDING_DOWN: SmartDashboard.putString( "DB/String 1", ("LIFT_ARM_HOLDING_DOWN") ); // timeout if it takes too long: m_elapsedTime += m_loopTime; SmartDashboard.putString( "DB/String 2", ("elapsed time: " + String.valueOf( String.format("%.1f", m_elapsedTime ) ) ) ); if (m_elapsedTime > 10.0) { m_currentState = MainState.LIFT_ARM_TIMEOUT_DOWN; } break; case LIFT_ARM_TIMEOUT_DOWN: SmartDashboard.putString( "DB/String 1", ("LIFT_ARM_TIMEOUT_DOWN") ); intakeMotorsIn(MOTOR_OFF); liftArmMot.set(MOTOR_OFF); m_currentState = MainState.IDLE; break; case SHUTDOWN: SmartDashboard.putString( "DB/String 1", ("SHUTDOWN") ); intakeMotorsIn(MOTOR_OFF); initiateLiftArmDownFromAnyHeight(); break; } // (end of switch) } } // (end of Robot)