Example #1
0
void i2cSlaveInitialize(I2cBusConnection* i2cBusConnection) {

    // Avoid more than one initialization
    if (i2cBusConnection->opened) {
        writeError(I2C_SLAVE_ALREADY_INITIALIZED);
        return;
    }
    i2cBusConnection->opened = true;
    
    appendString(getDebugOutputStreamLogger(), "I2C Slave Write Address=");
    appendHex2(getDebugOutputStreamLogger(), i2cBusConnection->i2cAddress);
    appendCRLF(getDebugOutputStreamLogger());

    if (i2cBusConnection == NULL) {
        
        // Enable the I2C module with clock stretching enabled
        OpenI2C1(I2C_ON | I2C_7BIT_ADD | I2C_STR_EN, BRG_VAL);
    
        // 7-bit I2C slave address must be initialised here.
        // we shift because i2c address is shift to the right
        // to manage read and write address
        I2C1ADD = i2cBusConnection->i2cAddress >> 1;
        I2C1MSK = 0;
    
        // Interruption on I2C Slave
        // -> Priority of I2C Slave interruption
        mI2C1SetIntPriority(I2C_INT_PRI_3 | I2C_INT_SLAVE);
        // -> Enable Interruption Flag => See the same code in interruption
        mI2C1SClearIntFlag();
    
        // Enable I2C (MACRO)
        EnableIntSI2C1;
    }
void i2cSlaveInitialize(I2cBusConnection* i2cBusConnection) {
    // Avoid more than one initialization
    if (i2cBusConnection->opened) {
        writeError(I2C_SLAVE_ALREADY_INITIALIZED);
        return;
    }
    i2cBusConnection->object = &i2cSlaveBusConnectionPc;

    appendString(getDebugOutputStreamLogger(), "I2C Slave Write Address=");
    appendHex2(getDebugOutputStreamLogger(), i2cBusConnection->i2cAddress);
    appendCRLF(getDebugOutputStreamLogger());

    i2cSlaveBusConnectionPc.masterToSlaveHandle = initClientPipe(L"\\\\.\\pipe\\mainBoardPipe");
    i2cSlaveBusConnectionPc.slaveToMasterHandle = initServerPipe(L"\\\\.\\pipe\\motorBoardPipe");
    i2cBusConnection->opened = true;

    // Thread : master => slave
    i2cSlaveBusConnectionPc.masterToSlaveThreadHandle = createStandardThread(
        masterToSlaveCallback,    // thread proc
        (LPVOID)i2cBusConnection,    // thread parameter 
        &(i2cSlaveBusConnectionPc.masterToSlaveThreadId));      // returns thread ID 

    // Thread : slave => master
    i2cSlaveBusConnectionPc.slaveToMasterThreadHandle = createStandardThread(
        slaveToMasterCallback,    // thread proc
        (LPVOID)i2cBusConnection,    // thread parameter 
        &(i2cSlaveBusConnectionPc.slaveToMasterThreadId));      // returns thread ID 
}
Example #3
0
void mainBoardDeviceHandleNotification(const Device* device, const char commandHeader, InputStream* inputStream) {
    appendString(getDebugOutputStreamLogger(), "Notification ! commandHeader=");
    append(getDebugOutputStreamLogger(), commandHeader);
    appendCRLF(getDebugOutputStreamLogger());
    if (commandHeader == NOTIFY_TEST) {
        unsigned char value = readHex2(inputStream);
        appendStringAndDec(getDebugOutputStreamLogger(), "value=", value);
    }
}
void mainBoardDeviceHandleMotionDeviceNotification(const Device* device, const unsigned char commandHeader, InputStream* notificationInputStream) {
    if (device->deviceInterface->deviceHeader == MOTION_DEVICE_HEADER) {
        if (
                commandHeader == NOTIFY_MOTION_STATUS_FAILED
                || commandHeader == NOTIFY_MOTION_STATUS_MOVING
                || commandHeader == NOTIFY_MOTION_STATUS_OBSTACLE
                || commandHeader == NOTIFY_MOTION_STATUS_REACHED
                || commandHeader == NOTIFY_MOTION_STATUS_BLOCKED
                || commandHeader == NOTIFY_MOTION_STATUS_SHOCKED) {
            updateNewPositionFromNotification(notificationInputStream);
            // FAKE DATA To Align with TrajectoryDevice
            checkIsSeparator(notificationInputStream);
            checkIsChar(notificationInputStream, 'F');
            
            gameStrategyContext->trajectoryType = TRAJECTORY_TYPE_NONE;
            
            appendStringCRLF(getDebugOutputStreamLogger(), "Motion Device Notification !");

        }
        else {
            writeError(NOTIFICATION_BAD_DEVICE_COMMAND_HANDLER_NOT_HANDLE);
            appendString(getAlwaysOutputStreamLogger(), "header");
            append(getAlwaysOutputStreamLogger(), commandHeader);
            println(getAlwaysOutputStreamLogger());
        }
    }
    else {
        writeError(NOTIFICATION_BAD_DEVICE);
    }
}
void notifyInfraredDetectorDetection(int type) {
    Buffer* buffer = getMechanicalBoard2I2CSlaveOutputBuffer();
    OutputStream* outputStream = getOutputStream(buffer);
    append(outputStream, NOTIFY_INFRARED_DETECTOR_DETECTION);
	appendHex2(outputStream, type);

	// Debug
	OutputStream* debugOutputStream = getDebugOutputStreamLogger();
	append(debugOutputStream, NOTIFY_INFRARED_DETECTOR_DETECTION);
	appendHex2(debugOutputStream, type);
}
int32_t VL53L0X_read_multi(uint8_t deviceAddress,  uint8_t index, uint8_t  *pdata, int32_t count) {
    I2cBusConnection* i2cBusConnection = getI2cBusConnectionBySlaveAddress(deviceAddress);
    I2cBus* i2cBus = i2cBusConnection->i2cBus;
    
    portableMasterWaitSendI2C(i2cBusConnection);
    
    portableMasterStartI2C(i2cBusConnection);
    WaitI2C(i2cBus);
    
    portableMasterWriteI2C(i2cBusConnection, deviceAddress);
    WaitI2C(i2cBus);
    
    // Write the "index" from which we want to read
    portableMasterWriteI2C(i2cBusConnection, index);
    WaitI2C(i2cBus);
    
    portableMasterStartI2C(i2cBusConnection);
    WaitI2C(i2cBus);
    // Enter in "read" mode
    portableMasterWriteI2C(i2cBusConnection, deviceAddress | 1);
    WaitI2C(i2cBus);

#ifdef VL53L0X_DEBUG
    OutputStream* debugOutputStream = getDebugOutputStreamLogger();
    appendString(debugOutputStream, "\tReading ");
    appendDec(debugOutputStream, count);
    appendString(debugOutputStream, " from addr 0x");
    appendHex2(debugOutputStream, deviceAddress);
    appendString(debugOutputStream, ": ");
#endif

  while (count--) {
    pdata[0] = portableMasterReadI2C(i2cBusConnection);
    // Ack
    if (count > 0) {
        portableMasterAckI2C(i2cBusConnection);
    }
    else {
        portableMasterNackI2C(i2cBusConnection);
    }
    WaitI2C(i2cBus);
#ifdef VL53L0X_DEBUG
    appendString(debugOutputStream, "0x ");
    appendHex2(debugOutputStream, pdata[0]);
    appendString(debugOutputStream, ", ");
#endif
    pdata++;
  }
#ifdef VL53L0X_DEBUG
     println(debugOutputStream);
#endif
     
  return VL53L0X_ERROR_NONE;
}
void i2cMasterInitialize(I2cBus* i2cBus) {
    OutputStream* outputStream = getDebugOutputStreamLogger();
    appendString(outputStream, "Initializing I2C ...");

    // Avoid more than one initialization
    if (i2cBus->initialized) {
        printI2cBus(outputStream, i2cBus);
        appendCRLF(outputStream);
        appendString(getDebugOutputStreamLogger(), "\n!!! ALREADY INITIALIZED !!!\n");
        return;
    }
    i2cBus->initialized = true;
    #define I2C_BRG     0xC6    // 100khz for PIC32
    // Configure I2C for 7 bit address mode
    #define I2C_CON     I2C_ON

    i2cBus->config = I2C_CON;

    if (i2cBus == NULL) {
        OpenI2C1(
                 // Configure I2C for 7 bit address mode.
                 I2C_CON,
                 // 100khz for PIC32.
                 I2C_BRG);
    }
    else {
        I2C_MODULE i2cModule = getI2C_MODULE(i2cBus->port);
        I2CConfigure(i2cModule, I2C_ON);
        I2CSetFrequency(i2cModule, GetPeripheralClock(), 100000L);
    }

    WaitI2C(i2cBus);
    
    // Indicates that it's ok !
    appendString(outputStream, "OK\n"); 
    printI2cBus(outputStream, i2cBus);
    appendCRLF(outputStream);
}
Example #8
0
bool absFloatTest(void) {
    signed long value = 40000.0;
    signed long absValue = fabsf(value);
    
    appendCRLF(getDebugOutputStreamLogger());
    appendString(getDebugOutputStreamLogger(), "absFloatTest\n");
    appendString(getDebugOutputStreamLogger(), "NORMAL=");
    appendDecf(getDebugOutputStreamLogger(), value);
    appendCRLF(getDebugOutputStreamLogger());

    appendString(getDebugOutputStreamLogger(), "RESULT=");
    appendDecf(getDebugOutputStreamLogger(), absValue);
    appendCRLF(getDebugOutputStreamLogger());

    return false;
}
void deviceStartMatchDetectorHandleRawData(unsigned char commandHeader, InputStream* inputStream, OutputStream* outputStream, OutputStream* notificationOutputStream) {
    if (commandHeader == COMMAND_MATCH_IS_STARTED) {
        StartMatch* startMatch = getStartMatchDetectorStartMatchObject();
        int value = isMatchStarted(startMatch);
        ackCommand(outputStream, START_MATCH_DEVICE_HEADER, COMMAND_MATCH_IS_STARTED);
        appendHex2(outputStream, value);
    }
    else if (commandHeader == COMMAND_START_MATCH_DEBUG) {
        StartMatch* startMatch = getStartMatchDetectorStartMatchObject();
        ackCommand(outputStream, START_MATCH_DEVICE_HEADER, COMMAND_START_MATCH_DEBUG);
        OutputStream* debugOutputStream = getDebugOutputStreamLogger();
        printStartMatchTable(debugOutputStream, startMatch);
    }
}
void deviceTrajectoryHandleRawData(char header,
        InputStream* inputStream,
        OutputStream* outputStream) {
    if (header == COMMAND_GET_ABSOLUTE_POSITION) {
        appendAck(outputStream);

        updateTrajectoryWithNoThreshold();
        append(outputStream, COMMAND_GET_ABSOLUTE_POSITION);
        notifyAbsolutePositionWithoutHeader(outputStream);

    }
    else if (header == COMMAND_DEBUG_GET_ABSOLUTE_POSITION) {
        appendAck(outputStream);

        updateTrajectoryWithNoThreshold();
        append(outputStream, COMMAND_DEBUG_GET_ABSOLUTE_POSITION);

        OutputStream* debugOutputStream = getOutputStreamLogger(ALWAYS);
        printPosition(debugOutputStream);
    }

    else if (header == COMMAND_SET_ABSOLUTE_POSITION) {
        long newX = readHex4(inputStream);
        inputStream->readChar(inputStream);
        long newY = readHex4(inputStream);
        inputStream->readChar(inputStream);
        long newAngle = readHex4(inputStream);

        appendAck(outputStream);

        OutputStream* debugOutputStream = getDebugOutputStreamLogger();

        appendStringAndDec(debugOutputStream, "newX=", newX);
        appendStringAndDec(debugOutputStream, ",newY=", newY);
        appendStringAndDec(debugOutputStream, ",newAngle=", newAngle);

        float fX = (float) newX;
        float fY = (float) newY;
        float fAngle = PI_DIVIDE_1800 * (float) newAngle;

        appendStringAndDecf(debugOutputStream, "fX=", fX);
        appendStringAndDecf(debugOutputStream, ",fY=", fY);
        appendStringAndDecf(debugOutputStream, ",fAngle=", fAngle);

        setPosition(fX, fY, fAngle);

        append(outputStream, COMMAND_SET_ABSOLUTE_POSITION);
    }
}
Example #11
0
bool absLimitTest(void) {
    signed long value1 = 50000;
    signed long limitValue = 40000;

    signed long result = limit(value1, limitValue);
    
    appendCRLF(getDebugOutputStreamLogger());
    appendString(getDebugOutputStreamLogger(), "absLimitTest\n");
    appendString(getDebugOutputStreamLogger(), "NORMAL=");
    appendDec(getDebugOutputStreamLogger(), limitValue);
    appendCRLF(getDebugOutputStreamLogger());

    appendString(getDebugOutputStreamLogger(), "RESULT=");
    appendDec(getDebugOutputStreamLogger(), result);
    appendCRLF(getDebugOutputStreamLogger());

    return false;
}
int32_t VL53L0X_write_multi(uint8_t deviceAddress, uint8_t index, uint8_t  *pdata, int32_t count) {
    I2cBusConnection* i2cBusConnection = getI2cBusConnectionBySlaveAddress(deviceAddress);
    I2cBus* i2cBus = i2cBusConnection->i2cBus;
    
    portableMasterWaitSendI2C(i2cBusConnection);
    // Wait till Start sequence is completed
    WaitI2C(i2cBus);
    
    portableMasterStartI2C(i2cBusConnection);
    WaitI2C(i2cBus);
    
    // I2C PICs adress use 8 bits and not 7 bits
    portableMasterWriteI2C(i2cBusConnection, deviceAddress);
    WaitI2C(i2cBus);
    
    portableMasterWriteI2C(i2cBusConnection, index);
    WaitI2C(i2cBus);
    
#ifdef VL53L0X_DEBUG
    OutputStream* debugOutputStream = getDebugOutputStreamLogger();
    appendString(debugOutputStream, "\tWriting ");
    appendDec(debugOutputStream, count);
    appendString(debugOutputStream, " to addr 0x");
    appendHex2(debugOutputStream, deviceAddress);
    appendString(debugOutputStream, ": ");
#endif
    
    while(count--) {
        portableMasterWriteI2C(i2cBusConnection, pdata[0]);
        WaitI2C(i2cBus);
#ifdef VL53L0X_DEBUG
    appendString(debugOutputStream, "0x ");
    appendHex2(debugOutputStream, pdata[0]);
    appendString(debugOutputStream, ", ");
#endif
        pdata++;
    }
#ifdef VL53L0X_DEBUG
    println(debugOutputStream);
#endif
    
    portableMasterStopI2C(i2cBusConnection);
    WaitI2C(i2cBus);
    
    return VL53L0X_ERROR_NONE;
}
Example #13
0
bool convertFloatTest2(void) {
    signed long value = -40000;
    float result = (float) value;
    
    appendCRLF(getDebugOutputStreamLogger());
    appendString(getDebugOutputStreamLogger(), "convertFloatTest2\n");
    appendString(getDebugOutputStreamLogger(), "NORMAL=-40000.0000");
    appendCRLF(getDebugOutputStreamLogger());

    appendString(getDebugOutputStreamLogger(), "RESULT=");
    appendDecf(getDebugOutputStreamLogger(), result);
    appendCRLF(getDebugOutputStreamLogger());

    return false;
}
void mainBoardCommonInitCommonDrivers(void) {
    // -> Eeproms
    appendString(getDebugOutputStreamLogger(), "EEPROM ...");
    eepromI2cBusConnection = addI2cBusConnection(i2cBus, ST24C512_ADDRESS_0, true);
    init24C512Eeprom(&eeprom, eepromI2cBusConnection);
    appendStringLN(getDebugOutputStreamLogger(), "OK");
    // -> Clock
    appendString(getDebugOutputStreamLogger(), "CLOCK ...");
    clockI2cBusConnection = addI2cBusConnection(i2cBus, PCF8563_WRITE_ADDRESS, true);
    initClockPCF8563(&clock, clockI2cBusConnection);
    appendStringLN(getDebugOutputStreamLogger(), "OK");
    // -> Temperature
    appendString(getDebugOutputStreamLogger(), "TEMPERATURE ...");
    temperatureI2cBusConnection = addI2cBusConnection(i2cBus, LM75A_ADDRESS, true);
    initTemperatureLM75A(&temperature, temperatureI2cBusConnection);
    appendStringLN(getDebugOutputStreamLogger(), "OK");
    /*
    //--> Current
    appendString(getDebugOutputStreamLogger(), "CURRENT...");
    currentI2CBusConnection = addI2cBusConnection(i2cBus, INA3221_ADDRESS_1, true);
    initCurrentINA3221(&current, currentI2CBusConnection);
    appendStringLN(getDebugOutputStreamLogger(), "OK");    
    */
    
    // -> Servo
    initServoList(&servoList, (Servo(*)[]) &servoListArray, MAIN_BOARD_SERVO_LIST_LENGTH);
    addServos_1_2_5(&servoList, "PWM 1", "PWM 2", "PWM 5");
    
    // -> Accelerometer
    /*
    appendString(getDebugOutputStreamLogger(), "ACCELEROMETER ...");
    I2cBusConnection* adxl345BusConnection = addI2cBusConnection(i2cBus, ADXL345_ALT_ADDRESS, true);
    initADXL345AsAccelerometer(&accelerometer, &accelerometerData, adxl345BusConnection);
    adxl345_setupInterruptOnSingleTapOnInt1(&accelerometer,
                                            8000,
                                            2,
                                            TAP_AXES_ALL_ENABLE,
                                            ADXL345_RATE_400HZ,
                                            ADXL345_RANGE_16G);
    appendStringLN(getDebugOutputStreamLogger(), "OK");
    */
    
    // Start interruptions
    startTimerList(true);
}
void i2cSlaveInitialize(I2cBusConnection* i2cBusConnection) {
    // Avoid more than one initialization
    if (i2cBusConnection->opened) {
        writeError(I2C_SLAVE_ALREADY_INITIALIZED);
        return;
    }
    i2cBusConnection->opened = true;

    I2CCONbits.STREN = 1;
    // I2CCONbits.GCEN = 1;
    I2CCONbits.DISSLW = 1;
    I2CCONbits.SCLREL = 1;

    // 7-bit I2C slave address must be initialised here.
    // we shift because i2c address is shift to the right
    // to manage read and write address
    I2CADD = i2cBusConnection->i2cAddress >> 1;

    // Interruption on I2C Slave
    // -> Priority of I2C Slave interruption
    IPC3bits.SI2CIP = 1;
    // -> Enable I2C Slave interruption
    IEC0bits.SI2CIE = 1;
    // -> Enable Interruption Flag => See the same code in interruption
    IFS0bits.SI2CIF = 0;

    // Enable I2C
    I2CCONbits.I2CEN = 1;

    appendString(getDebugOutputStreamLogger(), "I2C Slave CONF=");
    appendBinary16(getDebugOutputStreamLogger(), I2CCON, 4);
    appendCRLF(getDebugOutputStreamLogger());

    appendString(getDebugOutputStreamLogger(), "I2C Slave Write Address=");
    appendHex2(getDebugOutputStreamLogger(), i2cBusConnection->i2cAddress);
    appendCRLF(getDebugOutputStreamLogger());
}
void bSplineMotionUCompute(void) {
    PidMotion* pidMotion = getPidMotion();
    PidComputationValues* computationValues = &(pidMotion->computationValues);
    PidMotionDefinition* motionDefinition = &(pidMotion->currentMotionDefinition);

    BSplineCurve* curve = &(motionDefinition->curve);
    float pidTime = computationValues->pidTime;
    MotionInstruction* thetaInst = &(motionDefinition->inst[THETA]);
    float normalPosition = computeNormalPosition(thetaInst, pidTime);

    // Computes the time of the bSpline in [0.00, 1.00]
    float bSplineTime = computeBSplineTimeAtDistance(curve, normalPosition);

    Point normalPoint;

    // Computes the normal Point where the robot must be
    computeBSplinePoint(curve, bSplineTime, &normalPoint);
    // Convert normalPoint into mm space
    RobotKinematics* robotKinematics = getRobotKinematics();
    float wheelAverageLength = robotKinematics->wheelAverageLengthForOnePulse;

    normalPoint.x = normalPoint.x * wheelAverageLength;
    normalPoint.y = normalPoint.y * wheelAverageLength;

    Position* robotPosition = getPosition();
    Point robotPoint = robotPosition->pos;

    // GET PID
    unsigned pidIndex = getIndexOfPid(THETA, thetaInst->pidType);
    unsigned char rollingTestMode = getRollingTestMode();
    PidParameter* pidParameter = getPidParameter(pidIndex, rollingTestMode);

    // ALPHA
    PidMotionError* alphaMotionError = &(computationValues->errors[ALPHA]);    

    float normalAlpha = computeBSplineOrientationWithDerivative(curve, bSplineTime);
    float realAlpha = robotPosition->orientation;
    
    // backward management
    if (curve->backward) {
        realAlpha += PI;
    }

    float alphaError = (normalAlpha - realAlpha);
    // restriction to [-PI, PI]
    alphaError = mod2PI(alphaError);

    // Convert angleError into pulse equivalent
    float wheelsDistanceFromCenter = getWheelsDistanceFromCenter(robotKinematics);
    float alphaPulseError = (-wheelsDistanceFromCenter * alphaError) / wheelAverageLength;

    // THETA
    PidMotionError* thetaMotionError = &(computationValues->errors[THETA]);

    // thetaError must be in Pulse and not in MM
    float thetaError = distanceBetweenPoints(&robotPoint, &normalPoint) / wheelAverageLength;
    float thetaAngle = angleOfVector(&robotPoint, &normalPoint);
    if (curve->backward) {
        thetaAngle += PI;
    }
    float alphaAndThetaDiff = thetaAngle - normalAlpha;

    // restriction to [-PI, PI]
    alphaAndThetaDiff = mod2PI(alphaAndThetaDiff);

    float cosAlphaAndThetaDiff = cosf(alphaAndThetaDiff);
    float thetaErrorWithCos = thetaError * cosAlphaAndThetaDiff;
    
    float normalSpeed = computeNormalSpeed(thetaInst, pidTime);
    float thetaU = computePidCorrection(thetaMotionError, pidParameter, normalSpeed, thetaErrorWithCos);

    PidCurrentValues* thetaCurrentValues = &(computationValues->currentValues[THETA]);
    thetaCurrentValues->u = thetaU;

    // ALPHA CORRECTION
    alphaPulseError *= 5.0f;
    float alphaCorrection = -0.00050f * normalSpeed * thetaError * (alphaAndThetaDiff);
    // float alphaCorrection = 0.0f;
    alphaPulseError += alphaCorrection;
    float alphaU = computePidCorrection(alphaMotionError, pidParameter, 0, alphaPulseError);

    PidCurrentValues* alphaCurrentValues = &(computationValues->currentValues[ALPHA]);
    alphaCurrentValues->u = alphaU;
    
    // LOG
    OutputStream* out = getDebugOutputStreamLogger();
    
    // appendStringAndDecf(out, "pt=", pidTime);

    appendStringAndDecf(out, ",t=", bSplineTime);

    // Normal Position
    appendStringAndDecf(out, ",nx=", normalPoint.x);
    appendStringAndDecf(out, ",ny=", normalPoint.y);
    appendStringAndDecf(out, ",na=", normalAlpha);

    // Real position

    appendStringAndDecf(out, ",rx=", robotPoint.x);
    appendStringAndDecf(out, ",ry=", robotPoint.y);
    appendStringAndDecf(out, ",ra=", realAlpha);

    // ALPHA
    appendStringAndDecf(out, ",ta=", thetaAngle);
    appendStringAndDecf(out, ",ae=", alphaError);
    //appendStringAndDecf(out, ",atdiff=", alphaAndThetaDiff);
    //appendStringAndDecf(out, ",catdiff=", cosAlphaAndThetaDiff);
    appendStringAndDecf(out, ",au=", alphaU);
    appendStringAndDecf(out, ",ac=", alphaCorrection);

    // THETA

    // appendString(out, ",np=");
    // appendDecf(out, normalPosition);

    appendStringAndDecf(out, ",te=", thetaError);
    appendStringAndDecf(out, ",tec=", thetaErrorWithCos);
    appendStringAndDecf(out, ",tu=", thetaU);
    
    appendCRLF(out);
}
Example #17
0
void debugTrajectoryVariables(char* valueName1, float value1, char* valueName2, float value2) {
    OutputStream* outputStream = getDebugOutputStreamLogger();
    appendStringAndDecf(outputStream, valueName1, value1);
    appendStringAndDecf(outputStream, valueName2, value2);
    appendCRLF(outputStream);
}
Example #18
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();
    }
}
void mainBoardCommonTofInitDrivers(I2cBus* i2cBus, I2cBus* i2cBus2, unsigned int tofSensorCount) {
    // IO Expander List
    appendString(getDebugOutputStreamLogger(), "PCFs ...");
    initIOExpanderList(&ioExpanderList, (IOExpander(*)[]) &ioExpanderArray, MAIN_BOARD_IO_EXPANDER_LIST_LENGTH);
    
    // First Expander (Beacon + Fork)
    tofIoExpander = getIOExpanderByIndex(&ioExpanderList, 0);
    tofIoExpanderBusConnection = addI2cBusConnection(i2cBus, PCF8574_ADDRESS_0, true);
    initIOExpanderPCF8574(tofIoExpander, tofIoExpanderBusConnection);
    
    // if (tofSensorCount > 8) {
        // Second Expander (Lateral Tofs)
        lateralTofIoExpander = getIOExpanderByIndex(&ioExpanderList, 1);
        lateralTofIoExpanderBusConnection = addI2cBusConnection(i2cBus2, PCF8574_ADDRESS_0, true);
        initIOExpanderPCF8574(lateralTofIoExpander, lateralTofIoExpanderBusConnection);
    // }
    
    // End of IOExpanderList
    appendStringLN(getDebugOutputStreamLogger(), "OK");
        
    // TOF
    if (tofSensorCount > 0) {
        tofSensorArray[0].enabled = true;  
    }
    if (tofSensorCount > 1) {
        tofSensorArray[1].enabled = true; 
    }
    if (tofSensorCount > 2) {
        tofSensorArray[2].enabled = true;        
    }
    if (tofSensorCount > 3) {
        tofSensorArray[3].enabled = true;
    }
    if (tofSensorCount > 4) {
        tofSensorArray[4].enabled = true;        
    }    
    if (tofSensorCount > 5) {
        tofSensorArray[5].enabled = true;
    }
    if (tofSensorCount > 6) {
        tofSensorArray[6].enabled = true;
    }
    if (tofSensorCount > 7) {
        tofSensorArray[7].enabled = true;
    }
    if (tofSensorCount > 8) {
        tofSensorArray[8].enabled = true;
    }
    if (tofSensorCount > 9) {
        tofSensorArray[9].enabled = true;
    }
    if (tofSensorCount > 10) {
        tofSensorArray[10].enabled = true;
    }
    if (tofSensorCount > 11) {
        tofSensorArray[11].enabled = true;
    }
    
    appendStringLN(getDebugOutputStreamLogger(), "TOF ...");
    initTofSensorListVL53L0X(&tofSensorList,
                             (TofSensor(*)[]) &tofSensorArray,
                             (TofSensorVL53L0X(*)[]) &tofSensorVL53L0XArray,
                              // Size
                              tofSensorCount,
                              // Bus
                              i2cBus,
                              // Other Bus if we use more than 8 tofs
                              i2cBus2,
                              // IO Expander, if null, we will not be able to
                              // Manage several tof
                              tofIoExpander,
                              // Second IO Expander
                              lateralTofIoExpander,            
                              // debug
                              true,
                              // enabledAllSensors
                              false,
                              // changeAddressAllSensors
                              true
            );

    appendStringLN(getDebugOutputStreamLogger(), "OK");
    /*
    // -> Color : IMPORTANT !!! : Initialize COLOR after TOF because they share the same address
    // And not the same bus, but it uses "getI2cBusConnectionBySlaveAddress" ... which does not distinguish the bus ...
    appendStringLN(getDebugOutputStreamLogger(), "COLOR ...");
    colorBusConnection = addI2cBusConnection(i2cBus4, TCS34725_ADDRESS, true);
    initTcs34725Struct(&tcs34725, colorBusConnection);
    initColorSensorTcs34725(&colorSensor, &colorValue, &colorSensorFindColorType2018, &tcs34725);
    appendStringLN(getDebugOutputStreamLogger(), "OK");
    */
}
Example #20
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
}