Ejemplo n.º 1
0
void writeBSplineHeader(OutputStream* outputStream) {
    appendCRLF(outputStream);
    appendKeyAndName(outputStream, "t", CSV_SEPARATOR);

    appendKeyAndName(outputStream, "p.x", CSV_SEPARATOR);
    appendKeyAndName(outputStream, "p.y", CSV_SEPARATOR);

    appendKeyAndName(outputStream, "length", CSV_SEPARATOR);

    appendKeyAndName(outputStream, "orientation", CSV_SEPARATOR);

    appendCRLF(outputStream);
}
Ejemplo n.º 2
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;
}
Ejemplo n.º 3
0
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 
}
Ejemplo n.º 4
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;
    }
Ejemplo n.º 5
0
void motorBoardWaitForInstruction(void) {

    // delaymSec(MOTOR_BOARD_PC_DELAY_CONSOLE_ANALYZE_MILLISECONDS);

    // Analyze data from the Console (Specific to PC)
    while (consoleInputStream.availableData(&consoleInputStream)) {
        unsigned char c = consoleInputStream.readChar(&consoleInputStream);
        consoleInputBuffer.outputStream.writeChar(&(consoleInputBuffer.outputStream), c);
        if (c == 13) {
            appendCRLF(&consoleOutputStream);
        }
        consoleOutputStream.writeChar(&consoleOutputStream, c);
    }

    // Analyse from the ConsoleBuffer
    handleStreamInstruction(
        &consoleInputBuffer,
        &consoleOutputBuffer,
        &consoleOutputStream,
        &filterRemoveCRLF,
        NULL);

    if (!singleModeActivated) {
        // Analyse data from the I2C
        handleStreamInstruction(
            &i2cSlaveInputBuffer,
            &i2cSlaveOutputBuffer,
            NULL,
            &filterRemoveCRLF,
            NULL);
    }

    handleInstructionAndMotion();
}
Ejemplo n.º 6
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 mechanicalBoard1PcWaitForInstruction(void) {
    // Analyze data from the Console (Specific to PC)
    while (consoleInputStream.availableData(&consoleInputStream)) {
        unsigned char c = consoleInputStream.readChar(&consoleInputStream);
        consoleInputBuffer.outputStream.writeChar(&(consoleInputBuffer.outputStream), c);
        if (c == 13) {
            appendCRLF(&consoleOutputStream);
        }
        consoleOutputStream.writeChar(&consoleOutputStream, c);
    }

    // Analyse from the ConsoleBuffer
    handleStreamInstruction(
        &consoleInputBuffer,
        &consoleOutputBuffer,
        &consoleOutputStream,
        &consoleOutputStream,
        &filterRemoveCRLF,
        NULL);

    if (!singleModeActivated) {
        // Analyse data from the I2C
        handleStreamInstruction(
            &i2cSlaveInputBuffer,
            &i2cSlaveOutputBuffer,
            NULL,
            NULL,
            &filterRemoveCRLF,
            NULL);
    }
}
Ejemplo n.º 8
0
void printSerialLinkBuffer(OutputStream* outputStream, const SerialLink* serialLink) {
    if (serialLink != NULL) {
        StreamLink* streamLink = serialLink->streamLink;

        // Serial Input Buffer
        appendCRLF(outputStream);
        Buffer* serialInputBuffer = streamLink->inputBuffer;
        appendString(outputStream, "SERIAL INPUT BUFFER : ");
        printDebugBuffer(outputStream, serialInputBuffer);

        // Serial Output Buffer
        appendCRLF(outputStream);
        Buffer* serialOutputBuffer = streamLink->outputBuffer;
        appendString(outputStream, "SERIAL OUTPUT BUFFER : ");
        printDebugBuffer(outputStream, serialOutputBuffer);
    }
}
Ejemplo n.º 9
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);
    }
}
Ejemplo n.º 10
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;
}
Ejemplo n.º 11
0
void printTimerList(OutputStream* outputStream, TimerList* timerList) {
    int i;
    for (i = 0; i < timerList->size; i++) {
        Timer* timer = (Timer*) timerList->timers;
        timer += i;

        appendCRLF(outputStream);
        printTimer(outputStream, timer);
    }
}
void printDriverDataDispatcher(OutputStream* outputStream, DriverDataDispatcher* dispatcher) {
    appendString(outputStream, "dispatcher=");
    appendString(outputStream, dispatcher->name);
    appendString(outputStream, ", transmitMode=");
    TransmitMode transmitMode = dispatcher->transmitMode;
    appendDec(outputStream, transmitMode);
    append(outputStream, '(');
    appendString(outputStream, getTransmitModeAsString(transmitMode));
    append(outputStream, ')');
    appendString(outputStream, ", address=0x");
    appendHex2(outputStream, dispatcher->address);
    appendCRLF(outputStream);
}
Ejemplo n.º 13
0
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);
}
Ejemplo n.º 14
0
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());
}
Ejemplo n.º 15
0
void printBeaconSystemConfiguration(OutputStream* outputStream) {
	Point* opponentPoint = getOpponentRobotPosition();
	
	Laser* laser1 = getLaser(LASER_INDEX_1);
	Laser* laser2 = getLaser(LASER_INDEX_2);
	
	printLaserStruct(outputStream, laser1);
	println(outputStream);
	printLaserStruct(outputStream, laser2);
	
	appendCRLF(outputStream);
	
	// Last Detection Time
	appendStringAndDecf(outputStream, "lastDetectionTime=", getLastDetectionTimeInMillis());
	println(outputStream);
	
	// Obsolete Detection Time Threshold
	appendStringAndDecf(outputStream, "obsoleteDetectionTimeThreshold=", getObsoleteDetectionTimeThreshold());
	println(outputStream);
	
	// Notify Time Delay
	appendStringAndDecf(outputStream, "notifyTimeDelay=", getNotifyTimeDelay());
	println(outputStream);
	
	// Opponent Position
	appendString(outputStream, "opponentPosition:");
	printPoint(outputStream, opponentPoint, " mm");
	
	// Distance between beacon
	appendStringAndDecf(outputStream, "distanceBetweenBeacon=", getDistanceBetweenBeacon());
	appendString(outputStream, " mm");
	println(outputStream);
	
	// Calibration Point
	appendString(outputStream, "calibrationPoint:");
	printPoint(outputStream, &(beaconSystem.calibrationPoint), " mm");
	
	// Opponent Position
	appendString(outputStream, "opponentPosition:");
	printPoint(outputStream, opponentPoint, " mm");
}
Ejemplo n.º 16
0
void printPosition(OutputStream* outputStream) {
    // clearScreen();
    appendCRLF(outputStream);
    Position* p = getPosition();
    appendStringAndDec(outputStream, "left=", getCoderValue(CODER_LEFT));
    appendStringAndDec(outputStream, " | right=", getCoderValue(CODER_RIGHT));
    println(outputStream);
    printPoint(outputStream, &(p->pos), " mm");

    appendStringAndAngleInDeg(outputStream, "ang:", p->orientation);
    appendStringAndAngleInDeg(outputStream, "\r\nang init:", p->initialOrientation);

    appendStringAndDecf(outputStream, "\r\nlastLeft:", lastLeft);
    appendString(outputStream, " pulse");

    appendStringAndDecf(outputStream, "\r\nlastRight:", lastRight);
    appendString(outputStream, " pulse");

    appendStringAndAngleInDeg(outputStream, "\r\nlastAng:", lastAngle);

    println(outputStream);
}
Ejemplo n.º 17
0
bool mainBoardPcWaitForInstruction(StartMatch* startMatch) {
    while (handleNotificationFromDispatcherList(TRANSMIT_I2C)) {
        // loop for all notification
        // notification handler must avoid to directly information in notification callback
        // and never to the call back device
    }

    // delaymSec(MAIN_BOARD_PC_DELAY_CONSOLE_ANALYZE_MILLISECONDS);

    // Fill Console Buffer
    while (consoleInputStream.availableData(&consoleInputStream)) {
        unsigned char c = consoleInputStream.readChar(&consoleInputStream);
        consoleInputBuffer.outputStream.writeChar(&(consoleInputBuffer.outputStream), c);
        if (c == 13) {
            appendCRLF(&consoleOutputStream);
        }
        consoleOutputStream.writeChar(&consoleOutputStream, c);
    }

    // From Text Console
    handleStreamInstruction(
        &consoleInputBuffer,
        &consoleOutputBuffer,
        &consoleOutputStream,
        &filterRemoveCRLF,
        NULL);

    if (connectToRobotManager) {
        // From RobotManager
        handleStreamInstruction(
            &robotManagerInputBuffer,
            &robotManagerOutputBuffer,
            &robotManagerOutputStream,
            &filterRemoveCRLF,
            NULL);
    }

    return true;
}
Ejemplo n.º 18
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.º 19
0
void debugTrajectoryVariables(char* valueName1, float value1, char* valueName2, float value2) {
    OutputStream* outputStream = getDebugOutputStreamLogger();
    appendStringAndDecf(outputStream, valueName1, value1);
    appendStringAndDecf(outputStream, valueName2, value2);
    appendCRLF(outputStream);
}
Ejemplo n.º 20
0
/**
 * The interrupt demo timer.
 */
void interruptDemoTimerCallbackFunc(Timer* timer) {
    appendStringAndDec(getAlwaysOutputStreamLogger(), "counter=", demoCounter);
    appendCRLF(getAlwaysOutputStreamLogger());
    demoCounter++;
}
Ejemplo n.º 21
0
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);
}
Ejemplo n.º 22
0
/**
 * @private
 * @return true if it's ok, false if there is an error
 */
bool printMethodOrNotificationMetaData(OutputStream* outputStream, DeviceInterface* deviceInterface, unsigned char commandHeader, char argumentLength, char resultLength, bool notification) {
    bool result = true;
    if (argumentLength != DEVICE_HEADER_NOT_HANDLED) {
        DeviceMethodMetaData* deviceMethodMetaData = getDeviceMethodMetaData();

        // Function Header
		appendCharTableData(outputStream, commandHeader, DEVICE_USAGE_HEADER_COLUMN_LENGTH);

        // functionName
		const char* functionName = deviceMethodMetaData->functionName;
		appendStringTableData(outputStream, functionName, DEVICE_USAGE_NAME_COLUMN_LENGTH);

        // arguments
		appendStringTableData(outputStream, "IN", DEVICE_USAGE_IO_COLUMN_LENGTH);
		int argumentCount = deviceMethodMetaData->argumentsSize;
		int argumentIndex;
		int realArgumentLength = 0;
		for (argumentIndex = 0; argumentIndex < argumentCount; argumentIndex++) {
			DeviceArgument deviceArgument = deviceMethodMetaData->arguments[argumentIndex];
			realArgumentLength += getLengthOfType(deviceArgument.type);
		}
		appendDecTableData(outputStream, realArgumentLength, DEVICE_USAGE_IO_SIZE_COLUMN_LENGTH);

		// param name empty
		appendStringTableData(outputStream, "", DEVICE_USAGE_PARAM_NAME_COLUMN_LENGTH);
		appendStringTableData(outputStream, "", DEVICE_USAGE_PARAM_SIZE_COLUMN_LENGTH);
		appendStringTableData(outputStream, "", DEVICE_USAGE_PARAM_TYPE_COLUMN_LENGTH);
		appendTableSeparator(outputStream);
		println(outputStream);

		for (argumentIndex = 0; argumentIndex < argumentCount; argumentIndex++) {
			DeviceArgument deviceArgument = deviceMethodMetaData->arguments[argumentIndex];
			printArgument(outputStream, &deviceArgument, argumentIndex);
		}
		// TODO : Error must be check at the beginning of the function and not Inside
        if (argumentLength != realArgumentLength) {
            writeError(DEVICE_INTERFACE_PROBLEM);
            result = false;
            appendStringLN(outputStream, "Arguments Definition ERROR !!!!!!");
            println(outputStream);
            appendStringAndDec(outputStream, "argumentCount=", argumentCount);
            println(outputStream);
            appendStringAndDec(outputStream, "argumentLength=", argumentLength);
            println(outputStream);
            appendStringAndDec(outputStream, "realArgumentLength=", realArgumentLength);
            println(outputStream);
        }
        // results
        if (!notification) {

			appendStringTableData(outputStream, "", DEVICE_USAGE_HEADER_COLUMN_LENGTH);
			appendStringTableData(outputStream, "", DEVICE_USAGE_NAME_COLUMN_LENGTH);
			appendStringTableData(outputStream, "OUT", DEVICE_USAGE_IO_COLUMN_LENGTH);

			int resultCount = deviceMethodMetaData->resultsSize;
			int realResultLength = 0;
			int resultIndex;
			for (resultIndex = 0; resultIndex < resultCount; resultIndex++) {
				DeviceArgument resultArgument = deviceMethodMetaData->results[resultIndex];
				realResultLength += getLengthOfType(resultArgument.type);
			}
			appendDecTableData(outputStream, realResultLength, DEVICE_USAGE_IO_SIZE_COLUMN_LENGTH);

			appendStringTableData(outputStream, "", DEVICE_USAGE_PARAM_NAME_COLUMN_LENGTH);
			appendStringTableData(outputStream, "", DEVICE_USAGE_PARAM_SIZE_COLUMN_LENGTH);
			appendStringTableData(outputStream, "", DEVICE_USAGE_PARAM_TYPE_COLUMN_LENGTH);
			appendTableSeparator(outputStream);
			println(outputStream);

            for (resultIndex = 0; resultIndex < resultCount; resultIndex++) {
                DeviceArgument resultArgument = deviceMethodMetaData->results[resultIndex];
                printArgument(outputStream, &resultArgument, resultIndex);
            }
            if (resultLength != realResultLength) {
                result = false;
                writeError(DEVICE_INTERFACE_PROBLEM);
                appendString(outputStream, "Result Parameters Definition ERROR !!!!!!");
                appendCRLF(outputStream);
                appendStringAndDec(outputStream, "resultCount=", resultCount);
                appendCRLF(outputStream);
                appendStringAndDec(outputStream, "resultLength=", resultLength);
                appendCRLF(outputStream);
                appendStringAndDec(outputStream, "realResultLength=", realResultLength);
                appendCRLF(outputStream);
            }
        }
		appendTableHeaderSeparatorLine(outputStream);
    }
    return result;
}
Ejemplo n.º 23
0
void appendStringCRLF(OutputStream* outputStream, const char* s) {
    appendString(outputStream, s);
    appendCRLF(outputStream);
}
Ejemplo n.º 24
0
/**
 * @private
 * @return true if it's ok, false if there is an error
 */
bool printMethodOrNotificationMetaData(OutputStream* outputStream, DeviceInterface* deviceInterface, char commandHeader, char argumentLength, char resultLength, bool notification) {
    bool result = true;
    if (argumentLength != DEVICE_HEADER_NOT_HANDLED) {
        DeviceMethodMetaData* deviceMethodMetaData = getDeviceMethodMetaData();
        char deviceHeader = deviceInterface->deviceHeader;

        // DeviceName
        const char* deviceName = deviceInterface->deviceGetName();
        appendString(outputStream, deviceName);
        append(outputStream, DEVICE_NAME_AND_HEADER_SEPARATOR);

        // Header
        append(outputStream, deviceHeader);
        append(outputStream, commandHeader);
        append(outputStream, DEVICE_HEADER_AND_TYPE_SEPARATOR);

        // functionName
        appendString(outputStream, deviceMethodMetaData->functionName);
        append(outputStream,  ARGUMENTS_START_CHAR);

        // arguments
        int argumentCount = deviceMethodMetaData->argumentsSize;
        int argumentIndex;
        int realArgumentLength = 0;
        for (argumentIndex = 0; argumentIndex < argumentCount; argumentIndex++) {
            DeviceArgument deviceArgument = deviceMethodMetaData->arguments[argumentIndex];
            realArgumentLength += printArgument(outputStream, &deviceArgument, argumentIndex);
        }
        append(outputStream,  ARGUMENTS_STOP_CHAR);

        if (argumentLength != realArgumentLength) {
            writeError(DEVICE_INTERFACE_PROBLEM);
            result = false;
            appendString(outputStream, "Arguments Definition ERROR !!!!!!\n");
            appendCRLF(outputStream);
            appendStringAndDec(outputStream, "argumentCount=", argumentCount);
            appendCRLF(outputStream);
            appendStringAndDec(outputStream, "argumentLength=", argumentLength);
            appendCRLF(outputStream);
            appendStringAndDec(outputStream, "realArgumentLength=", realArgumentLength);
            appendCRLF(outputStream);
        }

        // results
        if (!notification) {
            appendString(outputStream, ARGUMENTS_AND_RESULTS_SEPARATOR);
            append(outputStream,  ARGUMENTS_START_CHAR);
            int resultCount = deviceMethodMetaData->resultsSize;
            int realResultLength = 0;
            int resultIndex;
            for (resultIndex = 0; resultIndex < resultCount; resultIndex++) {
                DeviceArgument resultArgument = deviceMethodMetaData->results[resultIndex];
                realResultLength += printArgument(outputStream, &resultArgument, resultIndex);
            }

            if (resultLength != realResultLength) {
                result = false;
                writeError(DEVICE_INTERFACE_PROBLEM);
                appendString(outputStream, "Result Parameters Definition ERROR !!!!!!");
                appendCRLF(outputStream);
                appendStringAndDec(outputStream, "resultCount=", resultCount);
                appendCRLF(outputStream);
                appendStringAndDec(outputStream, "resultLength=", resultLength);
                appendCRLF(outputStream);
                appendStringAndDec(outputStream, "realResultLength=", realResultLength);
                appendCRLF(outputStream);
            }

            if (resultCount == 0) {
                appendString(outputStream, "void");
            }
        }
        append(outputStream,  ARGUMENTS_STOP_CHAR);
        appendString(outputStream, ":");
        appendDec(outputStream, argumentLength);
        appendString(outputStream, "=>");
        appendDec(outputStream, resultLength);
        println(outputStream);
    }
    return result;
}