Пример #1
0
void mainBoardCommonAddDevices(RobotConfig* robotConfig) {
    // SYSTEM & DEBUG
    addLocalDevice(getSystemDeviceInterface(), getSystemDeviceDescriptor());
    addLocalDevice(getLogDeviceInterface(), getLogDeviceDescriptor());
    addLocalDevice(getSerialDebugDeviceInterface(), getSerialDebugDeviceDescriptor());
    addLocalDevice(getTimerDeviceInterface(), getTimerDeviceDescriptor());
    addLocalDevice(getDataDispatcherDeviceInterface(), getDataDispatcherDeviceDescriptor());
    addLocalDevice(getI2cCommonDebugDeviceInterface(), getI2cCommonDebugDeviceDescriptor());
    addLocalDevice(getI2cMasterDebugDeviceInterface(), getI2cMasterDebugDeviceDescriptor());
    
    // COMMON DEVICES
    addLocalDevice(getServoDeviceInterface(), getServoDeviceDescriptor(&servoList));
    addLocalDevice(getEepromDeviceInterface(), getEepromDeviceDescriptor(&eeprom));
    addLocalDevice(getClockDeviceInterface(), getClockDeviceDescriptor(&clock));
    addLocalDevice(getTemperatureSensorDeviceInterface(), getTemperatureSensorDeviceDescriptor(&temperature));
    // addLocalDevice(getCurrentSensorDeviceInterface(), getCurrentSensorDeviceDescriptor(&current));
    addLocalDevice(getADCDeviceInterface(), getADCDeviceDescriptor());
    
    // ACCELEROMETER
    // addLocalDevice(getAccelerometerDeviceInterface(), getAccelerometerDeviceDescriptor(&accelerometer));

    // ROBOT CONFIG
    addLocalDevice(getRobotConfigDeviceInterface(), getRobotConfigDeviceDescriptor(robotConfig));
}
void runMechanicalBoard1PC(bool singleMode) {
    singleModeActivated = singleMode;
    setBoardName(MECHANICAL_BOARD_1_PC_NAME);
	setConsoleSizeAndBuffer(CONSOLE_CHARS_WIDTH, CONSOLE_CHARS_HEIGHT, CONSOLE_BUFFER_WIDTH, CONSOLE_BUFFER_HEIGHT);
    moveConsole(HALF_SCREEN_WIDTH, CONSOLE_HEIGHT / 2, HALF_SCREEN_WIDTH, CONSOLE_HEIGHT / 2);

    // We use http://patorjk.com/software/taag/#p=display&v=2&f=Jerusalem&t=MECA%20%20%201%20%20%20PC
    // with Font : Jerusalem
    printf(" __  __ _____  ____    _        _     ____   ____\r\n");
    printf("|  \\/  | ____ / ___|  / \\      / |   |  _ \\ / ___|\r\n");
    printf("| |\\/| |  _|| |      / _ \\     | |   | |_) | |\r\n");
    printf("| |  | | |__| |___  / ___ \\    | |   |  __/| |___\r\n");
    printf("|_|  |_| _____\\____/ _ / \\_\\   |_|   |_|    \\____ |\r\n");

    initLogs(LOG_LEVEL_DEBUG, (LogHandler(*)[]) &logHandlerListArray, MECHANICAL_BOARD_1_PC_LOG_HANDLER_LIST_LENGTH, LOG_HANDLER_CATEGORY_ALL_MASK);
    initConsoleInputStream(&consoleInputStream);
    initConsoleOutputStream(&consoleOutputStream);
    addConsoleLogHandler(LOG_LEVEL_DEBUG, LOG_HANDLER_CATEGORY_ALL_MASK);

	initSerialLinkList((SerialLink(*)[]) &serialLinkListArray, MECHANICAL_BOARD_1_PC_SERIAL_LINK_LIST_LENGTH);

    initTimerList((Timer(*)[]) &timerListArray, MECHANICAL_BOARD_1_PC_TIMER_LENGTH);

    initBuffer(&consoleInputBuffer, (char(*)[]) &consoleInputBufferArray, MECHANICAL_BOARD_1_PC_CONSOLE_INPUT_BUFFER_LENGTH, "inputConsoleBuffer", "IN");
    initBuffer(&consoleOutputBuffer, (char(*)[]) &consoleOutputBufferArray, MECHANICAL_BOARD_1_PC_CONSOLE_OUTPUT_BUFFER_LENGTH, "outputConsoleBuffer", "IN");

    // Dispatchers
    initDriverDataDispatcherList((DriverDataDispatcher(*)[]) &driverDataDispatcherListArray, MECHANICAL_BOARD_1_PC_DATA_DISPATCHER_LIST_LENGTH);
    addLocalDriverDataDispatcher();

    if (!singleModeActivated) {
		// I2c
		initI2cBusList((I2cBus(*)[]) &i2cBusListArray, MECHANICAL_BOARD_1_PC_I2C_BUS_LIST_LENGTH);
		mechanical1I2cBus = addI2cBus(I2C_BUS_TYPE_SLAVE, I2C_BUS_PORT_1);

        initI2cBusConnectionList((I2cBusConnection(*)[]) &i2cBusConnectionListArray, MECHANICAL_BOARD_1_PC_I2C_BUS_CONNECTION_LIST_LENGTH);
        mechanical1I2cBusConnection = addI2cBusConnection(mechanical1I2cBus, MECHANICAL_BOARD_1_I2C_ADDRESS, false);
        mechanical1I2cBusConnection->i2cBus = mechanical1I2cBus;
        mechanical1I2cBusConnectionPc.i2cPipeMasterName = MAIN_BOARD_TO_MECA1_BOARD_PC_PIPE_I2C_MASTER_NAME;
        mechanical1I2cBusConnectionPc.i2cPipeSlaveName = MECHANICAL_BOARD_1_PC_PIPE_I2C_SLAVE_NAME;
        mechanical1I2cBusConnection->object = &mechanical1I2cBusConnectionPc;

		openSlaveI2cStreamLink(&i2cSlaveStreamLink,
            &i2cSlaveInputBuffer,
            (char(*)[]) &i2cSlaveInputBufferArray,
            MECHANICAL_BOARD_1_PC_IN_BUFFER_LENGTH,
            &i2cSlaveOutputBuffer,
            (char(*)[]) &i2cSlaveOutputBufferArray,
            MECHANICAL_BOARD_1_PC_OUT_BUFFER_LENGTH,
            mechanical1I2cBusConnection
        );
        // I2C Debug
        initI2CDebugBuffers(&i2cSlaveDebugInputBuffer,
            (char(*)[]) &i2cSlaveDebugInputBufferArray,
            MECHANICAL_BOARD_1_PC_I2C_DEBUG_SLAVE_IN_BUFFER_LENGTH,
            &i2cSlaveDebugOutputBuffer,
            (char(*)[]) &i2cSlaveDebugOutputBufferArray,
            MECHANICAL_BOARD_1_PC_I2C_DEBUG_SLAVE_OUT_BUFFER_LENGTH);
	}

    // Eeprom
    initEepromPc(&eeprom, "MECHANICAL_BOARD_1_PC_EEPROM");

    // Battery
    initBattery(&battery, getBatteryVoltage);
	
    // Clock
    initSoftClock(&clock);

    // HBridge Fake Motor MD22
    initDualHBridgeMotorPc(&md22);

    // 2018
    //  IO Expander
    initIOExpanderList(&ioExpanderList, (IOExpander(*)[]) &ioExpanderArray, MECHANICAL_BOARD_1_PC_IO_EXPANDER_LIST_LENGTH);
    initIOExpanderPc(getIOExpanderByIndex(&ioExpanderList, MECHANICAL_BOARD_1_PC_IO_EXPANDER_LAUNCHER_INDEX), &ioExpanderValue0);
    IOExpander* launcherIoExpander = getIOExpanderByIndex(&ioExpanderList, MECHANICAL_BOARD_1_PC_IO_EXPANDER_LAUNCHER_INDEX);

    // TODO : Add tof !
    // initLauncher2018(&launcher2018, launcherIoExpander, &relay, &md22, NULL);
 
    // Relay
    initRelayPc(&relay, &relayValue);

    // Devices
    initDeviceList((Device(*)[]) &deviceListArray, MECHANICAL_BOARD_1_PC_DEVICE_LIST_LENGTH);
    addLocalDevice(getTestDeviceInterface(), getTestDeviceDescriptor());
    addLocalDevice(getTimerDeviceInterface(), getTimerDeviceDescriptor());
	addLocalDevice(getI2cCommonDebugDeviceInterface(), getI2cCommonDebugDeviceDescriptor());
    addLocalDevice(getI2cSlaveDebugDeviceInterface(), getI2cSlaveDebugDeviceDescriptor(mechanical1I2cBusConnection));
    addLocalDevice(getSystemDeviceInterface(), getSystemDeviceDescriptor());
	addLocalDevice(getLogDeviceInterface(), getLogDeviceDescriptor());

    addLocalDevice(getEepromDeviceInterface(), getEepromDeviceDescriptor(&eeprom));
    addLocalDevice(getClockDeviceInterface(), getClockDeviceDescriptor(&clock));

    // 2018
    addLocalDevice(getIOExpanderDeviceInterface(), getIOExpanderDeviceDescriptor(&ioExpanderList));
    addLocalDevice(getRelayDeviceInterface(), getRelayDeviceDescriptor(&relay));
    addLocalDevice(getMD22DeviceInterface(), getMD22DeviceDescriptor(&md22));

    initDevices();

    startTimerList(true);

    setDebugI2cEnabled(false);
    while (1) {
        mechanicalBoard1PcWaitForInstruction();
    }
}
Пример #3
0
void runMainBoardPC(bool connectToRobotManagerMode) {
    connectToRobotManager = connectToRobotManagerMode;
    setBoardName(MAIN_BOARD_PC_NAME);
    moveConsole(0, 0, HALF_SCREEN_WIDTH, CONSOLE_HEIGHT);

    // We use http://patorjk.com/software/taag/#p=testall&v=2&f=Acrobatic&t=MOTOR%20BOARD%20PC
    // with Font : Jerusalem
    printf(" __  __    _    ___ _   _    ____   ___    _    ____  ____     ____   ____ \r\n");
    printf("|  \\/  |  / \\  |_ _| \\ | |  | __ ) / _ \\  / \\  |  _ \\|  _ \\   |  _ \\ / ___|\r\n");
    printf("| |\\/| | / _ \\  | ||  \\| |  |  _ \\| | | |/ _ \\ | |_) | | | |  | |_) | |    \r\n");
    printf("| |  | |/ ___ \\ | || |\\  |  | |_) | |_| / ___ \\|  _ <| |_| |  |  __/| |___ \r\n");
    printf("|_|  |_/_/   \\_|___|_| \\_|  |____/ \\___/_/   \\_|_| \\_|____/   |_|    \\____|\r\n");
    printf("\r\n");


    initLogs(DEBUG, (LogHandler(*)[]) &logHandlerListArray, MAIN_BOARD_PC_LOG_HANDLER_LIST_LENGTH, LOG_HANDLER_CATEGORY_ALL_MASK);
    initConsoleInputStream(&consoleInputStream);
    initConsoleOutputStream(&consoleOutputStream);
    addConsoleLogHandler(DEBUG, LOG_HANDLER_CATEGORY_ALL_MASK);
    appendStringCRLF(getDebugOutputStreamLogger(), getBoardName());


    initTimerList((Timer(*)[]) &timerListArray, MAIN_BOARD_PC_TIMER_LENGTH);

    initBuffer(&consoleInputBuffer, (char(*)[]) &consoleInputBufferArray, MAIN_BOARD_PC_CONSOLE_INPUT_BUFFER_LENGTH, "inputConsoleBuffer", "IN");
    initBuffer(&consoleOutputBuffer, (char(*)[]) &consoleOutputBufferArray, MAIN_BOARD_PC_CONSOLE_OUTPUT_BUFFER_LENGTH, "outputConsoleBuffer", "IN");

    // Dispatchers
    initDriverDataDispatcherList((DriverDataDispatcher(*)[]) &driverDataDispatcherListArray, MAIN_BOARD_PC_DATA_DISPATCHER_LIST_LENGTH);
    addLocalDriverDataDispatcher();

    initI2cBus(&motorBoardI2cBus, I2C_BUS_TYPE_MASTER, I2C_BUS_PORT_1);
    initI2cBusConnection(&motorBoardI2cBusConnection, &motorBoardI2cBus, MOTOR_BOARD_PC_I2C_ADDRESS);

    addI2CDriverDataDispatcher("MOTOR_BOARD_DISPATCHER",
        &motorBoardInputBuffer,
        (char(*)[]) &motorBoardInputBufferArray,
        MAIN_BOARD_PC_DATA_MOTOR_BOARD_DISPATCHER_BUFFER_LENGTH,
        &motorBoardOutputStream,
        &motorBoardInputStream,
        &motorBoardI2cBusConnection
    );

    if (connectToRobotManager) {
        // Open the serial Link between RobotManager (C# Project) and the MainBoardPc
        openSerialLink(&robotManagerSerialStreamLink,
            &robotManagerInputBuffer,
            (char(*)[]) &robotManagerInputBufferArray,
            ROBOT_MANAGER_INPUT_BUFFER_LENGTH,
            &robotManagerOutputBuffer,
            (char(*)[]) &robotManagerOutputBufferArray,
            ROBOT_MANAGER_OUTPUT_BUFFER_LENGTH,
            &robotManagerOutputStream,
            SERIAL_PORT_ROBOT_MANAGER,
            0);
    }

    // CONFIG
    initRobotConfigPc(&robotConfig);

    // EEPROM
    initEepromPc(&eeprom, "TODO");
    initEepromFile(&eeprom);

    // Clock
    initPcClock(&clock);

    // Temperature
    initTemperaturePc(&temperature);

    // I2C Debug
    initI2CDebugBuffers(&i2cMasterDebugInputBuffer,
        (char(*)[]) &i2cMasterDebugInputBufferArray,
        MAIN_BOARD_PC_I2C_DEBUG_MASTER_IN_BUFFER_LENGTH,
        &i2cMasterDebugOutputBuffer,
        (char(*)[]) &i2cMasterDebugOutputBufferArray,
        MAIN_BOARD_PC_I2C_DEBUG_MASTER_OUT_BUFFER_LENGTH);

    // Init the drivers
    initDrivers(&driverRequestBuffer, (char(*)[]) &driverRequestBufferArray, MAIN_BOARD_PC_REQUEST_DRIVER_BUFFER_LENGTH,
        &driverResponseBuffer, (char(*)[]) &driverResponseBufferArray, MAIN_BOARD_PC_RESPONSE_DRIVER_BUFFER_LENGTH);

    // Get test driver for debug purpose
    addDriver(testDriverGetDescriptor(), TRANSMIT_LOCAL);

    // Devices
    initDeviceList((Device(*)[]) &deviceListArray, MAIN_BOARD_PC_DEVICE_LIST_LENGTH);
    // addLocalDevice(getTestDeviceInterface(), getTestDeviceDescriptor());

    testDevice = addI2cRemoteDevice(getTestDeviceInterface(), MOTOR_BOARD_PC_I2C_ADDRESS);
    testDevice->deviceHandleNotification = mainBoardDeviceHandleNotification;

    // LOCAL BOARD
    addLocalDevice(getADCDeviceInterface(), getADCDeviceDescriptor());
    addLocalDevice(getStrategyDeviceInterface(), getStrategyDeviceDescriptor());
    addLocalDevice(getSystemDeviceInterface(), getSystemDeviceDescriptor());
    addLocalDevice(getRobotConfigDeviceInterface(), getRobotConfigDeviceDescriptor(&robotConfig));
    addLocalDevice(getI2cMasterDebugDeviceInterface(), getI2cMasterDebugDeviceDescriptor());
    addLocalDevice(getDataDispatcherDeviceInterface(), getDataDispatcherDeviceDescriptor());
    addLocalDevice(getServoDeviceInterface(), getServoDeviceDescriptor());
    addLocalDevice(getTimerDeviceInterface(), getTimerDeviceDescriptor());
    addLocalDevice(getClockDeviceInterface(), getClockDeviceDescriptor(&clock));
    // addLocalDevice(getFileDeviceInterface(), getFileDeviceDescriptor());
    addLocalDevice(getEepromDeviceInterface(), getEepromDeviceDescriptor(&eeprom));
    addLocalDevice(getLogDeviceInterface(), getLogDeviceDescriptor());
    addLocalDevice(getLCDDeviceInterface(), getLCDDeviceDescriptor());
    addLocalDevice(getTemperatureSensorDeviceInterface(), getTemperatureSensorDeviceDescriptor(&temperature));
    addLocalDevice(getNavigationDeviceInterface(), getNavigationDeviceDescriptor());

    initStartMatch(&startMatch, isMatchStartedPc, mainBoardPcWaitForInstruction, &eeprom);
    addLocalDevice(getStartMatchDeviceInterface(), getStartMatchDeviceDescriptor(&startMatch));
 
    // MOTOR BOARD
    addI2cRemoteDevice(getBatteryDeviceInterface(), MOTOR_BOARD_PC_I2C_ADDRESS);
    // addI2cRemoteDevice(getEepromDeviceInterface(), MOTOR_BOARD_PC_I2C_ADDRESS);
    addI2cRemoteDevice(getPIDDeviceInterface(), MOTOR_BOARD_PC_I2C_ADDRESS);
    addI2cRemoteDevice(getMotorDeviceInterface(), MOTOR_BOARD_PC_I2C_ADDRESS);
    addI2cRemoteDevice(getCodersDeviceInterface(), MOTOR_BOARD_PC_I2C_ADDRESS);
    addI2cRemoteDevice(getTrajectoryDeviceInterface(), MOTOR_BOARD_PC_I2C_ADDRESS);
    addI2cRemoteDevice(getMotionDeviceInterface(), MOTOR_BOARD_PC_I2C_ADDRESS);
    addI2cRemoteDevice(getRobotKinematicsDeviceInterface(), MOTOR_BOARD_PC_I2C_ADDRESS);
    addI2cRemoteDevice(getMotionSimulationDeviceInterface(), MOTOR_BOARD_PC_I2C_ADDRESS);

    initDevices();

    delaymSec(100);

    setDebugI2cEnabled(false);

    // Ping
    if (!pingDriverDataDispatcherList()) {
        printf("PING PROBLEM !");
    }

    // Set Clock for Motor Board !

    // Read Clock
    ClockData* clockData = clock.readClock(&clock);
    // TODO : Change Dispatcher Index ...
    writeDateRemoteClockData(clockData, 0x01);
    writeHourRemoteClockData(clockData, 0x01);

    // testDriverIntensive(100);

    startTimerList();

    while (1) {
        mainBoardPcWaitForInstruction(&startMatch);
    }

    // TODO : ShowEnd
}
Пример #4
0
/**
 * @private
 */
void initMainBoardDevicesDescriptor() {
    initDeviceList(&deviceListArray, MAIN_BOARD_DEVICE_LENGTH);

    // SYSTEM & DEBUG
    addLocalDevice(getSystemDeviceInterface(), getSystemDeviceDescriptor());
    addLocalDevice(getLogDeviceInterface(), getLogDeviceDescriptor());
    addLocalDevice(getSerialDebugDeviceInterface(), getSerialDebugDeviceDescriptor());
    addLocalDevice(getTimerDeviceInterface(), getTimerDeviceDescriptor());
    addLocalDevice(getTest2DeviceInterface(), getTest2DeviceDescriptor());
    addLocalDevice(getDataDispatcherDeviceInterface(), getDataDispatcherDeviceDescriptor());
    addLocalDevice(getI2cMasterDebugDeviceInterface(), getI2cMasterDebugDeviceDescriptor());

    // LOCAL
    addLocalDevice(getLCDDeviceInterface(), getLCDDeviceDescriptor());
    addLocalDevice(getRobotConfigDeviceInterface(), getRobotConfigDeviceDescriptor(&robotConfig));

    initStartMatch(&startMatch, isMatchStarted32, mainBoardWaitForInstruction, &eeprom);
    addLocalDevice(getStartMatchDeviceInterface(), getStartMatchDeviceDescriptor(&startMatch));
    addLocalDevice(getEndMatchDetectorDeviceInterface(), getEndMatchDetectorDeviceDescriptor());
    addLocalDevice(getEepromDeviceInterface(), getEepromDeviceDescriptor(&eeprom));
    addLocalDevice(getClockDeviceInterface(), getClockDeviceDescriptor(&clock));
    addLocalDevice(getADCDeviceInterface(), getADCDeviceDescriptor());
    addLocalDevice(getServoDeviceInterface(), getServoDeviceDescriptor());
    addLocalDevice(getLedDeviceInterface(), getLedDeviceDescriptor());

    addLocalDevice(getTestDeviceInterface(), getTestDeviceDescriptor());

    /*
    addLocalDevice(getSonarDeviceInterface(), getSonarDeviceDescriptor(&i2cBus));
    addLocalDevice(getRobotSonarDetectorDeviceInterface(), getRobotSonarDetectorDeviceDescriptor(&i2cBus));
    addLocalDevice(getTemperatureSensorDeviceInterface(), getTemperatureSensorDeviceDescriptor(&i2cBus));
    */
    // Mechanical Board 2->I2C
    // Device* armDevice = addI2cRemoteDevice(getArm2012DeviceInterface(), MECHANICAL_BOARD_2_I2C_ADDRESS);
    // Device* infraredDetectorDevice = addI2cRemoteDevice(getRobotInfraredDetectorDeviceInterface(), MECHANICAL_BOARD_2_I2C_ADDRESS);
    // addI2cRemoteDevice(getServoDeviceInterface(), MECHANICAL_BOARD_2_I2C_ADDRESS);
    // addUartRemoteDevice(getRobotInfraredDetectorDeviceInterface(), SERIAL_PORT_MECA2);

    // Motor Board->I2C
    /*
    addI2cRemoteDevice(getTestDeviceInterface(), MOTOR_BOARD_I2C_ADDRESS);
    addI2cRemoteDevice(getMotorDeviceInterface(), MOTOR_BOARD_I2C_ADDRESS);
    addI2cRemoteDevice(getCodersDeviceInterface(), MOTOR_BOARD_I2C_ADDRESS);
    addI2cRemoteDevice(getPIDDeviceInterface(), MOTOR_BOARD_I2C_ADDRESS);
    addI2cRemoteDevice(getTrajectoryDeviceInterface(), MOTOR_BOARD_I2C_ADDRESS);
    addI2cRemoteDevice(getMotionDeviceInterface(), MOTOR_BOARD_I2C_ADDRESS);
    */

    // MOTOR BOARD -> UART
    // addUartRemoteDevice(getTestDeviceInterface(), SERIAL_PORT_MOTOR);
    addUartRemoteDevice(getMotorDeviceInterface(), SERIAL_PORT_MOTOR);
    addUartRemoteDevice(getCodersDeviceInterface(), SERIAL_PORT_MOTOR);
    addUartRemoteDevice(getPIDDeviceInterface(), SERIAL_PORT_MOTOR);
    addUartRemoteDevice(getTrajectoryDeviceInterface(), SERIAL_PORT_MOTOR);
    addUartRemoteDevice(getMotionDeviceInterface(), SERIAL_PORT_MOTOR);
    addUartRemoteDevice(getRobotKinematicsDeviceInterface(), SERIAL_PORT_MOTOR);

    // Beacon Receiver Board->I2C
    // addI2cRemoteDevice(getBeaconReceiverDeviceInterface(), BEACON_RECEIVER_I2C_ADDRESS);

    // Strategy Board->I2C
    // addI2cRemoteDevice(getStrategyDeviceInterface(), STRATEGY_BOARD_I2C_ADDRESS);

    // Air Conditioning Board
    addI2cRemoteDevice(getAirConditioningDeviceInterface(), AIR_CONDITIONING_BOARD_I2C_ADDRESS);

    // Init the devices
    initDevices();  

    // Manage the callback notification
    // trajectoryDevice->deviceHandleCallbackRawData = &mainBoardCallbackRawData;
    // testDevice.deviceHandleCallbackRawData = &mainBoardCallbackRawData;
    // motionDevice->deviceHandleCallbackRawData = &mainBoardCallbackRawData;
    // infraredDetectorDevice->deviceHandleCallbackRawData = &mainBoardCallbackRawData;
}