示例#1
0
void runMotorBoardPC(bool singleMode) {
    singleModeActivated = singleMode;
    setBoardName(MOTOR_BOARD_PC_NAME);
    moveConsole(HALF_SCREEN_WIDTH, 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, MOTOR_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, MOTOR_BOARD_PC_TIMER_LENGTH);

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

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

    if (!singleModeActivated) {
        initI2cBus(&motorI2cBus, I2C_BUS_TYPE_SLAVE, I2C_BUS_PORT_1);
        initI2cBusConnection(&motorI2cBusConnection, &motorI2cBus, MOTOR_BOARD_PC_I2C_ADDRESS);
        openSlaveI2cStreamLink(&i2cSlaveStreamLink,
            &i2cSlaveInputBuffer,
            (char(*)[]) &i2cSlaveInputBufferArray,
            MOTOR_BOARD_PC_IN_BUFFER_LENGTH,
            &i2cSlaveOutputBuffer,
            (char(*)[]) &i2cSlaveOutputBufferArray,
            MOTOR_BOARD_PC_OUT_BUFFER_LENGTH,
            &motorI2cBusConnection
        );

        // I2C Debug
        initI2CDebugBuffers(&i2cSlaveDebugInputBuffer,
            (char(*)[]) &i2cSlaveDebugInputBufferArray,
            MOTOR_BOARD_PC_I2C_DEBUG_SLAVE_IN_BUFFER_LENGTH,
            &i2cSlaveDebugOutputBuffer,
            (char(*)[]) &i2cSlaveDebugOutputBufferArray,
            MOTOR_BOARD_PC_I2C_DEBUG_SLAVE_OUT_BUFFER_LENGTH);
    }

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

    // Battery
    initBattery(&battery, getBatteryVoltage);

    // Clock
    initSoftClock(&clock);

    // Devices
    initDeviceList((Device(*)[]) &deviceListArray, MOTOR_BOARD_PC_DEVICE_LIST_LENGTH);
    addLocalDevice(getTestDeviceInterface(), getTestDeviceDescriptor());
    addLocalDevice(getI2cSlaveDebugDeviceInterface(), getI2cSlaveDebugDeviceDescriptor(&motorI2cBusConnection));
    addLocalDevice(getSystemDeviceInterface(), getSystemDeviceDescriptor());
    addLocalDevice(getMotorDeviceInterface(), getMotorDeviceDescriptor());

    addLocalDevice(getEepromDeviceInterface(), getEepromDeviceDescriptor(&eeprom));
    addLocalDevice(getBatteryDeviceInterface(), getBatteryDeviceDescriptor(&battery));
    addLocalDevice(getClockDeviceInterface(), getClockDeviceDescriptor(&clock));
    addLocalDevice(getRobotKinematicsDeviceInterface(), getRobotKinematicsDeviceDescriptor(&eeprom));

    addLocalDevice(getPIDDeviceInterface(), getPIDDeviceDescriptor(&eeprom, true));
    addLocalDevice(getMotorDeviceInterface(), getMotorDeviceDescriptor());
    addLocalDevice(getCodersDeviceInterface(), getCodersDeviceDescriptor());
    addLocalDevice(getTrajectoryDeviceInterface(), getTrajectoryDeviceDescriptor());
    addLocalDevice(getMotionDeviceInterface(), getMotionDeviceDescriptor(&eeprom, true));
    addLocalDevice(getMotionSimulationDeviceInterface(), getMotionSimulationDeviceDescriptor());

    initDevices();

    startTimerList();

    setDebugI2cEnabled(false);
    while (1) {
        motorBoardWaitForInstruction();
    }
}
示例#2
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
}