diff --git a/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/DropPath.java b/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/DropPath.java index a9c66b8f016..ec2d9b50a95 100644 --- a/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/DropPath.java +++ b/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/DropPath.java @@ -1,12 +1,6 @@ package com.qualcomm.ftcrobotcontroller.opmodes; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.OpMode; -import com.qualcomm.robotcore.hardware.AnalogInput; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.LightSensor; -import com.qualcomm.robotcore.util.Range; -import lib.FourWheelDrive; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import lib.Robot; import lib.SensorState; import lib.TwoWheelDrive; @@ -17,12 +11,12 @@ public class DropPath extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { - Robot dave = new Robot(hardwareMap, telemetry, this); // makes Robot "dave" + Robot.init(hardwareMap, telemetry, this); // makes Robot "dave" TwoWheelDrive dave_train = new TwoWheelDrive(hardwareMap.dcMotor.get("leftdrive"), true, hardwareMap.dcMotor.get("rightdrive"), false, 10); - dave.registerDriveTrain(dave_train); + Robot.registerDrivetrain(dave_train); Robot.state = new SensorState(hardwareMap, 1, 0); Robot.state.registerSensor("mr", SensorState.SensorType.COLOR, false, 12); @@ -39,9 +33,9 @@ public void runOpMode() throws InterruptedException { while (Robot.state.gyroIsCalibrating("hero")); -// dave_train.move(0.2, "hero", this); + dave_train.turnWithGyro(90, "hero"); - dave.colorSweep(SensorState.ColorType.BLUE, 4, 10, "mrs", "mr", -0.3); + Robot.colorSweep(SensorState.ColorType.BLUE, 4, 10, -0.3, "mrs", "mr", "hero"); while (opModeIsActive()){ // telemetry.addData("running", Robot.state.getSensorReading("hero")); diff --git a/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/MeetTwoPath.java b/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/MeetTwoPath.java index 7ea896d59f1..c6654739ff5 100644 --- a/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/MeetTwoPath.java +++ b/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/MeetTwoPath.java @@ -17,7 +17,7 @@ public class MeetTwoPath extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { - Robot dave = new Robot(hardwareMap, telemetry, this); // makes Robot "dave" + Robot.init(hardwareMap, telemetry, this); // makes Robot "dave" Robot.state = new SensorState(hardwareMap, 1, 0); Robot.state.registerSensor("hero", SensorState.SensorType.GYRO, true, 12); @@ -33,7 +33,7 @@ public void runOpMode() throws InterruptedException { hardwareMap.dcMotor.get("rightRear"), false, 4); - dave.registerDriveTrain(dave_train); + Robot.registerDrivetrain(dave_train); waitForStart(); diff --git a/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NewDrivetrainTest.java b/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NewDrivetrainTest.java index ce1cab6b15b..fbd41223591 100644 --- a/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NewDrivetrainTest.java +++ b/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NewDrivetrainTest.java @@ -13,31 +13,15 @@ public class NewDrivetrainTest extends LinearOpMode { @Override public void runOpMode() throws InterruptedException{ - Robot dave = new Robot(hardwareMap, telemetry, this); // makes Robot "dave" - - DriveTrain dave_train = new TwoWheelDrive( - hardwareMap.dcMotor.get("leftdrive"), true, - hardwareMap.dcMotor.get("rightdrive"), false, 10); - - dave.registerDriveTrain(dave_train); + Robot.init(hardwareMap, telemetry, this); // makes Robot "dave" Robot.state = new SensorState(hardwareMap, 1, 0); - Robot.state.registerSensor("mr", SensorState.SensorType.COLOR, false, 12); - Robot.state.registerSensor("mrs", SensorState.SensorType.LIGHT, true, 12); - Robot.state.registerSensor("hero", SensorState.SensorType.GYRO, true, 12); - Robot.state.registerSensor("rearUltra", SensorState.SensorType.ULTRASONIC, true, 50); - Robot.state.registerSensor("frontUltra", SensorState.SensorType.ULTRASONIC, true, 50); - Robot.state.setUltrasonicPin("ultraToggle"); Thread state_thread = new Thread(Robot.state); state_thread.start(); waitForStart(); - while (Robot.state.gyroIsCalibrating("hero")); - - dave_train.turnWithGyro(45,"hero"); - while (opModeIsActive()){ try { Thread.sleep(10); diff --git a/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/SensorStateOpMode.java b/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/SensorStateOpMode.java index 96e38f00d69..ff619656588 100644 --- a/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/SensorStateOpMode.java +++ b/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/SensorStateOpMode.java @@ -14,7 +14,7 @@ public class SensorStateOpMode extends LinearOpMode { private Thread state_thread; public void runOpMode() throws InterruptedException{ - Robot dave = new Robot(hardwareMap, telemetry, this); + Robot.init(hardwareMap, telemetry, this); Robot.state = new SensorState(hardwareMap, 1, 0); Robot.state.registerSensor("mr", SensorState.SensorType.COLOR, true, 60); diff --git a/FtcRobotController/src/main/java/lib/DriveTrain.java b/FtcRobotController/src/main/java/lib/DriveTrain.java index b599bef5a6d..d1f4cad5d9b 100644 --- a/FtcRobotController/src/main/java/lib/DriveTrain.java +++ b/FtcRobotController/src/main/java/lib/DriveTrain.java @@ -1,9 +1,6 @@ package lib; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.util.ElapsedTime; -import com.qualcomm.robotcore.util.Range; /** * Created by luke on 10/7/15. diff --git a/FtcRobotController/src/main/java/lib/Robot.java b/FtcRobotController/src/main/java/lib/Robot.java index fc607280466..6146a6957a5 100644 --- a/FtcRobotController/src/main/java/lib/Robot.java +++ b/FtcRobotController/src/main/java/lib/Robot.java @@ -11,171 +11,173 @@ * Created by luke on 10/7/15. */ - public class Robot { public static SensorState state; // Hardware map pulls device Objects from the robot. // Drivetrain handles functions specific to our drive type (four-wheeld, two-wheel, treads, etc). - private HardwareMap hmap; - public DriveTrain drivetrain; + private static HardwareMap hmap; + public static DriveTrain drivetrain; + + // Telemetry lets any class anywhere print messages through Robot public static Telemetry tel; + // Store the objects corresponding to the devices of the robot (motors, sensors, servos) in hashmaps. - private HashMap motors; - private HashMap servos; - public UltraServoHelper ultraservohelper; + private static HashMap motors; + private static HashMap servos; + public static UltraServoHelper ultraservohelper; public static LinearOpMode waiter; - public Robot (HardwareMap hmap, Telemetry tel, LinearOpMode opm) { - this.hmap = hmap; - this.tel = tel; - this.servos = new HashMap(); - this.ultraservohelper = new UltraServoHelper(); - waiter = opm; - - // 100 millisecond delay between updates. -// state = new SensorState(hmap, 100); -// Thread state_thread = new Thread(state); -// state_thread.start(); - } + // Prevents instantiation + private Robot(){} - // If someone tries to get a device not registered in a hashmap. - public class HardwareNotRegisteredException extends Exception { - public HardwareNotRegisteredException() { super(); } - public HardwareNotRegisteredException(String cause) { super(cause); } - } - public void registerServo(String servoName) { - servos.put(servoName, hmap.servo.get(servoName)); + public static void init (HardwareMap hmap, Telemetry tel, LinearOpMode waiter) { + Robot.hmap = hmap; + Robot.tel = tel; + Robot.servos = new HashMap(); + Robot.ultraservohelper = new UltraServoHelper(); + Robot.waiter = waiter; } - public void setPosition(String name, int position) { - servos.get(name).setPosition(position/180); + // REGISTRATION FUNCTIONS + public static void registerDrivetrain(DriveTrain d){ + Robot.drivetrain = d; } - // REGISTRATION FUNCTIONS - // It makes more sense to have the opmode construct a drivetrain and pass it to Robot than to repeat - // constructors in Robot and the drivetrain classes. - public void registerDriveTrain(DriveTrain d){ - this.drivetrain = d; + public static void registerServo(String servoName) { + servos.put(servoName, hmap.servo.get(servoName)); } - public void registerUltrasonicServo(String sensorName, String servoName) { + public static void registerUltrasonicServo(String sensorName, String servoName) { if(servos.containsKey(servoName)) { ultraservohelper.registerServo(sensorName, servos.get(servoName)); } else{ - this.registerServo(servoName); + registerServo(servoName); ultraservohelper.registerServo(sensorName, servos.get(servoName)); } } - public void tillSense(String sensorName, int servoPosition, double power, int distance, int filterlength) { + // MOVEMENT FUNCTIONS + public static void setPosition(String servoName, int position) { + servos.get(servoName).setPosition(position / 180); + } + + public void tillSense(String sensorName, int servoPosition, double power, int distance, int filterlength, String gyro_name) { PID ultraPID = new PID(0.05, 0.005, 0, false,0.1); + ultraPID.setTarget(distance); ultraPID.setMinOutput(-1); ultraPID.setMaxOutput(1); + ultraservohelper.setPosition(sensorName, servoPosition); + try { Thread.sleep(400); } catch (InterruptedException e) { e.printStackTrace(); } - drivetrain.move(power,"hero",waiter); + + drivetrain.move(power, gyro_name, waiter); + while(!ultraPID.isAtTarget() && waiter.opModeIsActive()){ power = ultraPID.update(state.getAvgSensorData(sensorName)); drivetrain.move(power); Log.i("AvgUSDistance", "" + state.getAvgSensorData(sensorName)); + try{ Thread.sleep(10); } catch (InterruptedException e) { e.printStackTrace(); } } + drivetrain.move(0); } public void parallel(String sensorNameA, String sensorNameB, double power, double thresh, int filterlength) { double diff; - int count =0; - do{ - tel.addData("frontAvg", Robot.state.getAvgSensorData("frontUltra")); - tel.addData("rearAvg", Robot.state.getAvgSensorData("rearUltra")); - tel.addData("frontReading", Robot.state.getSensorReading("frontUltra")); - tel.addData("rearReading", Robot.state.getSensorReading("rearUltra")); + int count = 0; + do { diff = (state.getAvgSensorData(sensorNameA) - state.getAvgSensorData(sensorNameB)); - if(Math.signum(diff) == 1 && count==0){ + + if(Math.signum(diff) == 1 && count==0) { drivetrain.setLeftMotors(-power); drivetrain.setRightMotors(power); - } - else if(Math.signum(diff) == -1 && count==0) { + } else if(Math.signum(diff) == -1 && count==0) { drivetrain.setLeftMotors(power); drivetrain.setRightMotors(-power); } + + // If count isn't 0, then we're waiting to see if the robot slips at all, which means + // We need to keep the motors at 0. else{ drivetrain.setLeftMotors(0); drivetrain.setRightMotors(0); } + // Wait to see if the robot slips if(Math.abs(diff) < thresh){ count ++; - } - else{ + } else{ count = 0; } + try { Thread.sleep(5); } catch (InterruptedException e) { e.printStackTrace(); } - }while (count<15 && waiter.opModeIsActive()); + } while (count<15 && waiter.opModeIsActive()); drivetrain.move(0); } // tillSense for colors. If the first color we detect is the color argument (our teams color) - // Then we will hit that button. - // Otherwise, we go to the next light. - public void colorSweep(SensorState.ColorType color, double low_threshold, double high_threshold, String lightname, String colorname, double power) { + // Then we will hit that button. Otherwise, we go to the next light. + public static void colorSweep( SensorState.ColorType color, + double low_threshold, + double high_threshold, + double power, + String light_name, + String color_name, + String gyro_name) { - SensorState.ColorType stored_color = SensorState.ColorType.NONE; // First detected color - SensorState.ColorType dominant = state.getColorData(colorname); // Current dominant color detected - double average = 0.0; // Average of light values - double reading = 0.0; + SensorState.ColorType dominant = state.getColorData(color_name); // Current dominant color detected + double average; // Average of light values + double reading; // reading of light values -// drivetrain.move(power,"hero", waiter); + drivetrain.move(power,gyro_name, waiter); - average = state.getAvgSensorData(lightname); + average = state.getAvgSensorData(light_name); while (waiter.opModeIsActive()) { - reading = state.getAvgSensorData(lightname); - tel.addData("Reading", reading); - tel.addData("Average", average); + reading = state.getAvgSensorData(light_name); if (average + low_threshold <= reading && reading <= average + high_threshold){ -// break; + break; } + try{ Thread.sleep(1); - } catch (InterruptedException ex){} + } catch (InterruptedException ex){ + ex.printStackTrace(); + } } drivetrain.stopMove(); - + // Add code here to drop the climbers in the right bin if (dominant == color){ tel.addData("Color", "CORRECT"); -// drivetrain.move(0.0); -// ((TwoWheelDrive)drivetrain).moveDistance(0.18, 8); } // First color detected is wrong color, so hit other button, which must be the right button. else { tel.addData("Color", "WRONG"); -// drivetrain.move(0.0); -// ((TwoWheelDrive)drivetrain).moveDistance(-0.18, 8); } } } diff --git a/FtcRobotController/src/main/java/lib/SensorState.java b/FtcRobotController/src/main/java/lib/SensorState.java index d0085fcbaa9..97d414df7ca 100644 --- a/FtcRobotController/src/main/java/lib/SensorState.java +++ b/FtcRobotController/src/main/java/lib/SensorState.java @@ -113,9 +113,14 @@ public static ColorType[] toColor(int[] i){ } } - private HashMap maps; // HashMap of DeviceMappings from HardwareMap to grab sensor objects in registration. - private HashMap sensorContainers; // Stores SensorContainer objects (definition at bottom) - private HashMap types_inv; // Allows recovery of all sensors of a certain type. + // HashMap of DeviceMappings from HardwareMap to grab sensor objects in registration. + private HashMap maps; + + // Stores SensorContainer objects (definition at bottom) + private HashMap sensorContainers; + + // Allows recovery of all sensors of a certain type. + private HashMap types_inv; private HardwareMap hmap; private DigitalChannel usPin; @@ -177,8 +182,11 @@ public SensorState(HardwareMap hmap, int milli_interval, int nano_interval) { * @param data_length The number of sensor readings to store for the sensor */ public synchronized void registerSensor(String name, SensorType type, boolean update, int data_length){ - Object sensor_obj = maps.get(type).get(name); // Get underlying sensor object for the sensor - SensorContainer sen = new SensorContainer(sensor_obj, type, name, update, data_length); // Make a SensorContainer to wrap around the object + // Get underlying sensor object for the sensor + Object sensor_obj = maps.get(type).get(name); + + // Make a SensorContainer to wrap around the object + SensorContainer sen = new SensorContainer(sensor_obj, type, name, update, data_length); if (type == SensorType.GYRO) ((GyroSensor) sensor_obj).calibrate(); @@ -188,7 +196,7 @@ public synchronized void registerSensor(String name, SensorType type, boolean up } sensorContainers.put(name, sen); - updateTypes_Inv(sen); + updateTypesInv(sen); } /** @@ -200,6 +208,7 @@ public synchronized void registerSensor(String name, SensorType type, boolean up public void setUltrasonicPin(String pin_name){ if (!this.usPinWasSet) { this.usPin = hmap.digitalChannel.get(pin_name); + assert(this.usPin != null); this.usPinWasSet = true; } } @@ -209,7 +218,7 @@ public void setUltrasonicPin(String pin_name){ * * @param sen SensorContainer to add to the HashMap. This will be grouped into the array indexed by the sensor's type */ - private void updateTypes_Inv(SensorContainer sen){ + private void updateTypesInv(SensorContainer sen){ SensorContainer[] old_sensors = types_inv.get(sen.type); // Pull out the old array of sensors int old_length = old_sensors.length; @@ -232,10 +241,13 @@ private void updateTypes_Inv(SensorContainer sen){ ************************ */ + // All of these will throw errors if called with a name that hasn't been registered. + /** * Returns true if the given gyro is currently calibrating, and therefore can't give good values. */ public synchronized boolean gyroIsCalibrating(String gyro_name){ + assert(sensorContainers.keySet().contains(gyro_name)); return ((GyroSensor)sensorContainers.get(gyro_name).sensor).isCalibrating(); } @@ -243,10 +255,15 @@ public synchronized boolean gyroIsCalibrating(String gyro_name){ * Returns true if all the values of the filter array have been filled, allowing averaging. */ public synchronized boolean filterIsFilled(String name){ + assert(sensorContainers.keySet().contains(name)); return sensorContainers.get(name).filter.isFilled(); } + /** + * Change the length of the specified filter + */ public synchronized void changeFilterLength(String name, int fl){ + assert(sensorContainers.keySet().contains(name)); sensorContainers.get(name).filter.changeFilter_length(fl); } @@ -255,6 +272,7 @@ public synchronized void changeFilterLength(String name, int fl){ * Allows us to access by name only in the public functions. */ public synchronized ColorType getColorData(String name){ + assert(sensorContainers.keySet().contains(name)); return getDominantColor(sensorContainers.get(name)); } @@ -262,6 +280,7 @@ public synchronized ColorType getColorData(String name){ * Immediately return a value for the given sensor, without waiting for another run(). */ public synchronized double getSensorReading(String name){ + assert(sensorContainers.keySet().contains(name)); return getSensorReading(sensorContainers.get(name)); } @@ -270,6 +289,7 @@ public synchronized double getSensorReading(String name){ * cpu cycles. */ public synchronized void changeUpdateStatus(String name, boolean update){ + assert(sensorContainers.keySet().contains(name)); sensorContainers.get(name).update = update; } @@ -280,6 +300,7 @@ public synchronized void changeUpdateStatus(String name, boolean update){ * @return The SensorData object corresponding to the given sensor. */ public synchronized Filter getFilter(String name){ + assert(sensorContainers.keySet().contains(name)); return sensorContainers.get(name).filter.clone(); } @@ -287,6 +308,7 @@ public synchronized Filter getFilter(String name){ * Using the most recent chronological sensor data, average the last several readings */ public synchronized double getAvgSensorData(String name) { + assert(sensorContainers.keySet().contains(name)); return sensorContainers.get(name).filter.getAvg(); } @@ -294,6 +316,7 @@ public synchronized double getAvgSensorData(String name) { * Get a String[] array of all sensor names belonging to sensors of a certain type. */ public synchronized String[] getSensorsFromType(SensorType type){ + assert(types_inv.keySet().contains(type)); SensorContainer[] sens = types_inv.get(type); String[] ret = new String[sens.length]; @@ -368,7 +391,7 @@ private double getSensorReading(SensorContainer sen){ ex.printStackTrace(); } } else { - throw new RuntimeException(); + throw new RuntimeException("usPin not set in SensorState"); } case LIGHT: