Examples with HardwareMap com.qualcomm.robotcore.hardware.HardwareMap used on opensource projects

Example 6 with HardwareMap

use of com.qualcomm.robotcore.hardware.HardwareMap in project robotcode by OutoftheBoxFTC.

the class HardwareFactory method mapLynxUsbDevice.

private void mapLynxUsbDevice(HardwareMap map, DeviceManager deviceMgr, LynxUsbDeviceConfiguration lynxUsbDeviceConfiguration) throws RobotCoreException, InterruptedException {
    if (!lynxUsbDeviceConfiguration.isEnabled()) {
        return;
    }
    // Make a new LynxUsbDevice
    LynxUsbDevice lynxUsbDevice = (LynxUsbDevice) deviceMgr.createLynxUsbDevice(lynxUsbDeviceConfiguration.getSerialNumber(), lynxUsbDeviceConfiguration.getName());
    try {
        // If the system made up this device, let the live device know that too
        if (lynxUsbDeviceConfiguration.isSystemSynthetic()) {
            lynxUsbDevice.setSystemSynthetic(true);
        }
        // Are we the first USB-attached (as opposed to embedded) LynxUsbDevice?
        boolean isFirstLynxUsbDevice = !LynxConstants.isEmbeddedSerialNumber(lynxUsbDeviceConfiguration.getSerialNumber());
        for (LynxUsbDevice usbDevice : map.getAll(LynxUsbDevice.class)) {
            if (!LynxConstants.isEmbeddedSerialNumber(usbDevice.getSerialNumber())) {
                isFirstLynxUsbDevice = false;
                break;
            }
        }
        // Make all the modules first, since we need to ping the parent before pinging anyone else,
        // and so we need to figure out who that is.
        // 
        List<LynxModule> potentialModules = new ArrayList<LynxModule>();
        Map<Integer, String> moduleNames = new HashMap<Integer, String>();
        final int parentModuleAddress = lynxUsbDeviceConfiguration.getParentModuleAddress();
        for (DeviceConfiguration moduleConfiguration : lynxUsbDeviceConfiguration.getModules()) {
            int moduleAddress = moduleConfiguration.getPort();
            moduleNames.put(moduleAddress, moduleConfiguration.getName());
            // 
            LynxModule module = (LynxModule) deviceMgr.createLynxModule(lynxUsbDevice, moduleAddress, parentModuleAddress == moduleAddress, moduleConfiguration.getName());
            potentialModules.add(module);
            // If the system made up this device, let the live device know that too
            if (((LynxModuleConfiguration) moduleConfiguration).isSystemSynthetic()) {
                module.setSystemSynthetic(true);
            }
        }
        // Attach all the LynxModules to that LynxUsbDevice, parents first, so that parents get pinged first. Note that if some
        // modules aren't actually there, or are there but are wedged, these may throw exceptions.
        Map<Integer, LynxModule> connectedModules = new HashMap<Integer, LynxModule>();
        for (LynxModule module : potentialModules) {
            if (module.isParent()) {
                // nb: there should be only one parent
                connectModule(lynxUsbDevice, module, moduleNames, connectedModules, isFirstLynxUsbDevice && connectedModules.isEmpty());
            }
        }
        for (LynxModule module : potentialModules) {
            if (!module.isParent()) {
                connectModule(lynxUsbDevice, module, moduleNames, connectedModules, false);
            }
        }
        // Finish adding all the pieces to the modules we successfully talked to
        mapLynxModuleComponents(map, deviceMgr, lynxUsbDeviceConfiguration, lynxUsbDevice, connectedModules);
        // For the things that worked, remember the module names in the hwmap
        for (Map.Entry<Integer, LynxModule> pair : connectedModules.entrySet()) {
            int moduleAddress = pair.getKey();
            map.put(moduleNames.get(moduleAddress), pair.getValue());
        }
        // Remember the LynxUsbDevice too, so we we'll only enable one to charge the RC battery
        map.put(lynxUsbDeviceConfiguration.getName(), lynxUsbDevice);
    } catch (LynxNackException e) {
        throw e.wrap();
    } catch (RobotCoreException | RuntimeException e) {
        lynxUsbDevice.close();
        map.remove(lynxUsbDeviceConfiguration.getName(), lynxUsbDevice);
        throw e;
    }
}