Ejemplo n.º 1
0
void hd44780_initLcd(void) {
    currentRow = 0;
    currentColumn = 0;
    busyFlag();

    // PORT de Commande et de DAta en Sortie
    TRIS_RS_LCD = 0;
    TRIS_RW_LCD = 0;
    TRIS_E_LCD = 0;
    TRIS_D7_LCD = 0;
    TRIS_D6_LCD = 0;
    TRIS_D5_LCD = 0;
    TRIS_D4_LCD = 0;

    delaymSec(5);
    hd44780_sendIniComLcd(0, 0, 1, 1); //fonction set : 8bits

    delaymSec(5);
    hd44780_sendIniComLcd(0, 0, 1, 1); //fonction set : 8bits

    delaymSec(5);
    hd44780_sendIniComLcd(0, 0, 1, 1); //fonction set : 8bits

    delaymSec(5);
    hd44780_sendIniComLcd(0, 0, 1, 0); //fonction set : 4bits
    hd44780_sendComLcd(0b00101000); //fonction set : 4 bits  2lignes  5x8dots
    hd44780_sendComLcd(0b00001000); //display off  curs off  blinkin cursor off
    hd44780_sendComLcd(0b00001111); //display on  curs on  blinkin cursor on
    hd44780_sendComLcd(0b00000001); //display clear
    hd44780_sendComLcd(0b00000110); //increment   accompagnies display shift
    hd44780_setCursorAtHome();
}
void readNextDistance(I2cBusConnection* i2cBusConnection) {
    if (!readNextDistanceFlag) {
        return;
    }
    readNextDistanceFlag = false;

    unsigned int distance = getSRF02DistanceEndRanging(i2cBusConnection, SONAR_INDEX);
    delaymSec(1);
    startSRF02Ranging(i2cBusConnection, SONAR_INDEX);
    distances[(unsigned int) distanceIndex] = distance;

    /*
    appendDec(getDebugOutputStreamLogger(), distance);
    append(getDebugOutputStreamLogger(), '.');
     */

    // Manage index of history
    distanceIndex++;
    if (distanceIndex > ROBOT_SONAR_DETECTOR_DEVICE_HISTORY_SIZE) {
        distanceIndex = 0;
    }

    if (checkObstacle()) {
        // for the main LOOP
        obstacle = true;
    }
}
Ejemplo n.º 3
0
unsigned char takeCornIfNotBlack(int plierIndex) {
    downSensor(plierIndex);
    preparePlier(plierIndex);
    delaymSec(DOWN_SENSOR_TIME_MS);
    unsigned detected = isMetalDetected(plierIndex);
    upSensor(plierIndex);
    if (!detected) {
//        setTomatoeEnabled(1);
        forwardSimpleMMAndWait(DISTANCE_SENSOR_PLIER);
//        setTomatoeEnabled(0);
        if (isRobotMustStop()) {
            return CORN_BLACK_NOT_TAKEN;
        }

        takeAndLoadCorn(plierIndex);

//        setTomatoeEnabled(1);

        return CORN_WHITE_TAKEN;
    }
    else {
        upPlier(plierIndex);
        notifyReached();
        return CORN_BLACK_NOT_TAKEN;
    }
}
void deviceAirConditionningBoardHandleRawData(char header, InputStream* inputStream, OutputStream* outputStream) {
    if (header == COMMAND_AIR_CONDITIONNING) {
        int powerState = readHex2(inputStream);

        if (powerState != 0x00) {
            pwmServoAll(SERVO_SPEED, SERVO_VALUE_TOUCH);
            delaymSec(1000);
            pwmServoAll(SERVO_SPEED, SERVO_VALUE_STAND_BY);
        } else {
            pwmServoAll(SERVO_SPEED, SERVO_VALUE_TOUCH);
            delaymSec(1000);
            pwmServoAll(SERVO_SPEED, SERVO_VALUE_STAND_BY);
        }

        appendAck(outputStream);
        append(outputStream, COMMAND_AIR_CONDITIONNING);
    }
}
Ejemplo n.º 5
0
BOOL armUp(int color, int index) {
	BOOL result;
	if (color) {
		result = armDriver2012Up(index);
	}
	else {
		result = armDriver2012Up(index);
	}
	delaymSec(500);
	setReadyForNextMotion(TRUE);
	return result;
}
void deviceRobotSonarDetectorInit() {
    // Timer for detector
    addTimer(ROBOT_SONAR_DETECTOR_TIMER_INDEX,
            TIME_DIVIDER_16_HERTZ,
            deviceRobotSonarDetectorCallbackFunc, "ROBOT SONAR DETECTOR TIMER");
    // useSonar = isConfigSet(CONFIG_USE_LASER_MASK);
    clearHistory();
    startSRF02Ranging(robotSonarDetectorI2cBusConnection, SONAR_INDEX);
    delaymSec(65);
    getSRF02DistanceEndRanging(robotSonarDetectorI2cBusConnection, SONAR_INDEX);
    obstacle = false;
    readNextDistanceFlag = false;
}
Ejemplo n.º 7
0
void devicePwmMotorHandleRawData(char commandHeader, InputStream* inputStream, OutputStream* outputStream) {
    if (commandHeader == COMMAND_MOVE_MOTOR) {
        signed int left = readSignedHex2(inputStream);
        signed int right = readSignedHex2(inputStream);
        ackCommand(outputStream, MOTOR_DEVICE_HEADER, COMMAND_MOVE_MOTOR);
        setMotorSpeeds(left * 2, right * 2);
    }
    else if (commandHeader == COMMAND_READ_MOTOR_VALUE) {
        ackCommand(outputStream, MOTOR_DEVICE_HEADER, COMMAND_READ_MOTOR_VALUE);
        signed int left = getLeftSpeed();
        signed int right = getRightSpeed();
        appendHex2(outputStream, left / 2);
        appendHex2(outputStream, right / 2);
    }
    else if (commandHeader == COMMAND_STOP_MOTOR) {
        ackCommand(outputStream, MOTOR_DEVICE_HEADER, COMMAND_STOP_MOTOR);

        stopMotors();
    }
    else if (commandHeader == COMMAND_TEST_MOTOR) {
        ackCommand(outputStream, MOTOR_DEVICE_HEADER, COMMAND_TEST_MOTOR);

        appendString(getAlwaysOutputStreamLogger(), "Left Forward\n");
        // Left forward
        setMotorSpeeds(50, 0);
        delaymSec(2000);

        appendString(getAlwaysOutputStreamLogger(), "Right Forward\n");
        // Right forward
        setMotorSpeeds(0, 50);
        delaymSec(2000);

        appendString(getAlwaysOutputStreamLogger(), "Left Backward\n");
        // Left backward
        setMotorSpeeds(-50, 0);
        delaymSec(2000);

        appendString(getAlwaysOutputStreamLogger(), "Right Forward\n");
        // Right backward
        setMotorSpeeds(0, -50);
        delaymSec(2000);

        appendString(getAlwaysOutputStreamLogger(), "Both Forward\n");
        // Both forward
        setMotorSpeeds(50, 50);
        delaymSec(2000);

        appendString(getAlwaysOutputStreamLogger(), "Both Backward\n");
        // Both backward
        setMotorSpeeds(-50, -50);
        delaymSec(2000);

        stopMotors();
    }
}
Ejemplo n.º 8
0
void homologation5_Totem(int color) {
    unsigned int index = getMotionInstructionIndex();

    switch (index) {
        case 1:
            armDriver2012Down(ARM_LEFT);
            armDriver2012Down(ARM_RIGHT);
			delaymSec(1000);
			setReadyForNextMotion(TRUE);
            break;
		case 2:
			motionDriverForward(500.0f);
			break;
        case 3:
			armDriver2012Up(ARM_LEFT);
            armDriver2012Up(ARM_RIGHT);
			break;

	}
}
Ejemplo n.º 9
0
unsigned int getSRF02Distance(unsigned char sonarIndex) {
    startSRF02Ranging(sonarIndex);
    delaymSec(100);
    unsigned int result = getSRF02DistanceEndRanging(sonarIndex);
    return result;
}
Ejemplo n.º 10
0
int main(void) {
    setBoardName("TITAN ELECTRONICAL MAIN BOARD 32bits V-JJ_7");

    i2cMasterInitialize();
    
    //setRobotMustStop(false);
    // Open the serial Link for debug
    openSerialLink(&debugSerialStreamLink,
            &debugInputBuffer,
            &debugInputBufferArray,
            MAIN_BOARD_DEBUG_INPUT_BUFFER_LENGTH,
            &debugOutputBuffer,
            &debugOutputBufferArray,
            MAIN_BOARD_DEBUG_OUTPUT_BUFFER_LENGTH,
            &debugOutputStream,
            SERIAL_PORT_DEBUG,
            DEFAULT_SERIAL_SPEED);

    // Open the serial Link for the PC
    openSerialLink(&pcSerialStreamLink,
            &pcInputBuffer,
            &pcInputBufferArray,
            MAIN_BOARD_PC_INPUT_BUFFER_LENGTH,
            &pcOutputBuffer,
            &pcOutputBufferArray,
            MAIN_BOARD_PC_OUTPUT_BUFFER_LENGTH,
            &pcOutputStream,
            SERIAL_PORT_PC,
            DEFAULT_SERIAL_SPEED);

    // LCD (LCD03 via Serial interface)
    initLCDOutputStream(&lcdOutputStream);

    initTimerList(&timerListArray, MAIN_BOARD_TIMER_LENGTH);

    // Init the logs
    initLog(DEBUG);
    addLogHandler(&debugSerialLogHandler, "UART", &debugOutputStream, DEBUG);
    addLogHandler(&lcdLogHandler, "LCD", &lcdOutputStream, ERROR);

    appendString(getAlwaysOutputStreamLogger(), getBoardName());
    println(getAlwaysOutputStreamLogger());

    initDevicesDescriptor();
    initDriversDescriptor();

    i2cMasterInitialize();
    // Initialize EEPROM and RTC
    initClockPCF8563(&globalClock);
    init24C512Eeprom(&eeprom_);






    initRobotConfigPic32(&robotConfig);



    initStartMatchDetector32(&startMatchDetector);


    // Initializes the opponent robot
    // initOpponentRobot();

    /*
    // Creates a composite to notify both PC and Debug
    initCompositeOutputStream(&compositePcAndDebugOutputStream);
    addOutputStream(&compositePcAndDebugOutputStream, &debugOutputStream);
    addOutputStream(&compositePcAndDebugOutputStream, &pcOutputStream);

    // Creates a composite to redirect to driverRequest and Debug
    initCompositeOutputStream(&compositeDriverAndDebugOutputStream);
    addOutputStream(&compositeDriverAndDebugOutputStream, &debugOutputStream);
    addOutputStream(&compositeDriverAndDebugOutputStream, getDriverRequestOutputStream());
    */

    appendString(&debugOutputStream, "DEBUG\n");
    
    // Start interruptions
    //startTimerList();  //////RALENTI FORTEMENT LE PIC!!! PLANTE I2C !!!!!!!!

    // Configure data dispatcher
    //addLocalDriverDataDispatcher();

    // Stream for motorBoard
    /*
    addI2CDriverDataDispatcher(&motorI2cDispatcher,
            "MOTOR_BOARD_DISPATCHER",
            &motorBoardInputBuffer,
            &motorBoardInputBufferArray,
            MAIN_BOARD_LINK_TO_MOTOR_BOARD_BUFFER_LENGTH,
            &motorBoardOutputStream,
            &motorBoardInputStream,
            MOTOR_BOARD_I2C_ADDRESS);
    */
    /*
    // Stream for Mechanical Board 2
    addI2CDriverDataDispatcher(&mechanical2I2cDispatcher,
            "MECHANICAL_BOARD_2_DISPATCHER",
            &mechanical2BoardInputBuffer,
            &mechanical2BoardInputBufferArray,
            MAIN_BOARD_LINK_TO_MECA_BOARD_2_BUFFER_LENGTH,
            &mechanical2BoardOutputStream,
            &mechanical2BoardInputStream,
            MECHANICAL_BOARD_2_I2C_ADDRESS);
    */

 /*   // Stream for Air Conditioning
    addI2CDriverDataDispatcher(&airConditioningI2cDispatcher,
            "AIR_CONDITIONING_DISPATCHER",
            &airConditioningBoardInputBuffer,
            &airConditioningBoardInputBufferArray,
            MAIN_BOARD_LINK_TO_AIR_CONDITIONING_BOARD_BUFFER_LENGTH,
            &airConditioningBoardOutputStream,
            &airConditioningBoardInputStream,
            AIR_CONDITIONING_BOARD_I2C_ADDRESS);
*/
    // I2C Debug
    initI2CDebugBuffers(&i2cMasterDebugInputBuffer,
                        &i2cMasterDebugInputBufferArray,
                        MAIN_BOARD_I2C_DEBUG_MASTER_IN_BUFFER_LENGTH,
                        &i2cMasterDebugOutputBuffer,
                        &i2cMasterDebugOutputBufferArray,
                        MAIN_BOARD_I2C_DEBUG_MASTER_OUT_BUFFER_LENGTH);


    appendStringConfig(&lcdOutputStream);

    //pingDriverDataDispatcherList(getDebugOutputStreamLogger());

    // Inform PC waiting
    showWaitingStart(&debugOutputStream);



    // wait other board initialization
    delaymSec(1500);

    // 2012 VALUE
    unsigned int configValue = getConfigValue();
    unsigned int homologationIndex = configValue & CONFIG_STRATEGY_MASK;
    unsigned int color = configValue & CONFIG_COLOR_BLUE_MASK;

    appendString(getAlwaysOutputStreamLogger(), "Homologation:");
    appendCRLF(getAlwaysOutputStreamLogger());
    appendDec(getAlwaysOutputStreamLogger(), homologationIndex);
    
    appendString(getAlwaysOutputStreamLogger(), "Config:");
    appendHex4(getAlwaysOutputStreamLogger(), configValue);
    appendCRLF(getAlwaysOutputStreamLogger());
    useBalise = configValue & CONFIG_USE_BALISE_MASK;
    useInfrared = configValue & CONFIG_USE_SONAR_MASK;

    if (color != 0) {
        appendString(getAlwaysOutputStreamLogger(), "COLOR:VIOLET\n");
    }
    else {
        appendString(getAlwaysOutputStreamLogger(), "COLOR:RED\n");
    }    

    // TODO 2012 SV motionDriverMaintainPosition();

    // wait for start
 //   setInitialPosition(color);

    // notify game strategy
 //   sendStrategyConfiguration(configValue);

    loopUntilStart(&waitForInstruction);

    // Enable the sonar Status only after start
    //setSonarStatus(configValue & CONFIG_USE_SONAR_MASK);

    // mark that match begins
    markStartMatch();

    // write begin of match
    showStarted(&pcOutputStream);

    if (homologationIndex == 0) {
        // MATCH

        while (1) {
            waitForInstruction();
            //CLOCK Read


            if (isEnd()) {
                break;
            }
        }
    } else {
        setReadyForNextMotion(true);

        while (1) {
            
            portableStartI2C(i2cBus);
            WaitI2C(i2cBus);
            portableMasterWriteI2C(FREE_ADDRESS_2);//0x54
            WaitI2C(i2cBus);
            portableMasterWriteI2C('H');
            WaitI2C(i2cBus);
            portableMasterWriteI2C('E');
            WaitI2C(i2cBus);
            portableMasterWriteI2C('L');
            WaitI2C(i2cBus);
            portableMasterWriteI2C('L');
            WaitI2C(i2cBus);
            portableMasterWriteI2C('O');

            portableStopI2C(i2cBus);
            WaitI2C(i2cBus);
            int data1,data2,data3;
            while(1){
                waitForInstruction();
                //globalClock.readClock(&globalClock);
                //printClock(&debugOutputStream,&globalClock);
                //appendCR(&debugOutputStream);

                int data = 0;
                portableMasterWaitSendI2C(i2cBus);

                portableStartI2C(i2cBus);
                IdleI2C1();
                portableMasterWriteI2C(FREE_ADDRESS_2 + 1);//0x54
                WaitI2C(i2cBus);
                
                data = portableMasterReadI2C(i2cBus);
                portableAckI2C(i2cBus);
                IdleI2C1();

                data1 = portableMasterReadI2C(i2cBus);
                portableAckI2C(i2cBus);
                IdleI2C1();

                data2= portableMasterReadI2C(i2cBus);
                portableAckI2C(i2cBus);
                IdleI2C1();


                data3 = portableMasterReadI2C(i2cBus);
                portableNackI2C(i2cBus);
                IdleI2C1();
                append(&lcdOutputStream,data);
                append(&lcdOutputStream,data1);
                append(&lcdOutputStream,data2);
                append(&lcdOutputStream,data3);

                portableStopI2C(i2cBus);
                IdleI2C1();
                
                delaymSec(500);
            }

















            homologation(homologationIndex, color);

            // We stop if we are in homologation mode
            if (isRobotMustStop()) {
                motionDriverStop();
                break;
            }

            // ultimate tentative to restart the robot if it is blocked
            forceRobotNextStepIfNecessary();
        }
    }

    showEnd(&lcdOutputStream);

    while (1) {
        // Avoid reboot even at end
    }
}
Ejemplo n.º 11
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
}
Ejemplo n.º 12
0
void deviceSystemHandleRawData(char header, InputStream* inputStream, OutputStream* outputStream) {
    if (header == COMMAND_PING) {
        // data
        ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_PING);
        
        // Read and write in output the pingIndex (to control that it's the right which does the response)
        unsigned char pingIndex = readHex2(inputStream);
        appendHex2(outputStream, pingIndex);
    }
    // Last Error
    else if (header == COMMAND_GET_LAST_ERROR) {
        ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_GET_LAST_ERROR);
        unsigned int lastError = getLastError();
        appendHex4(outputStream, lastError);
    }
    else if (header == COMMAND_CLEAR_LAST_ERROR) {
        ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_CLEAR_LAST_ERROR);
        clearLastError();
    }
    // Device list
    else if (header == COMMAND_DEVICE_LIST) {
        ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_DEVICE_LIST);
        printDeviceList(getInfoOutputStreamLogger());
    // Usage
    } else if (header == COMMAND_USAGE) {
        ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_USAGE);
        printDeviceListUsage(getInfoOutputStreamLogger(), false);
    } else if (header == COMMAND_USAGE_PROBLEM) {
        ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_USAGE_PROBLEM);
        printDeviceListUsage(getInfoOutputStreamLogger(), true);
    }    
    else if (header == COMMAND_USAGE_SPECIFIC_DEVICE) {
         ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_USAGE_SPECIFIC_DEVICE);
         char deviceHeader = readBinaryChar(inputStream);
         int size = getDeviceCount();
         int i;
         for (i = 0; i < size; i++) {
             Device* device = getDevice(i);
             if (deviceHeader == device->deviceInterface->deviceHeader) {
                 println(getInfoOutputStreamLogger());
                 printDeviceUsage(getInfoOutputStreamLogger(), device, false);
                 return;
             }
         }
         appendString(getErrorOutputStreamLogger(), "Device Not Found ! ");
    }
	else if (header == COMMAND_CLS) {
		ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_CLS);
#ifdef PC_COMPILER
		system("cls");
#else
		appendString(outputStream, "Unsupported Operation");
#endif // PC_COMPILER
	}
	else if (header == COMMAND_RESET) {
		ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_RESET);
		#ifdef PC_COMPILER
			appendString(outputStream, "Unsupported Operation");
		#else
			// goto 0;
		#endif // PC_COMPILER
	}
    // Notifications
    else if (header == COMMAND_NOTIFICATION) {
        ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_NOTIFICATION);
        printDeviceListNotification(getInfoOutputStreamLogger(), false);
    } else if (header == COMMAND_WAIT) {
        appendAck(outputStream);
        int mSec = readHex4(inputStream);
        delaymSec(mSec);
        append(outputStream, SYSTEM_DEVICE_HEADER);
        append(outputStream, COMMAND_WAIT);
    } else if (header == COMMAND_BOARD_NAME) {
        appendString(getInfoOutputStreamLogger(), getBoardName());
        println(getInfoOutputStreamLogger());
        ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_BOARD_NAME);
    }
}
Ejemplo n.º 13
0
unsigned int getSRF02Distance(I2cBusConnection* i2cBusConnection, unsigned char sonarIndex) {
    startSRF02Ranging(i2cBusConnection, sonarIndex);
    delaymSec(100);
    unsigned int result = getSRF02DistanceEndRanging(i2cBusConnection, sonarIndex);
    return result;
}