driver sample

driver_sample.c — C source code, 2 kB (2.212 bytes)

Dateiinhalt

#pragma config(Sensor, S1,     sensorLight,         sensorLightActive)
#pragma config(Motor,  motorA,          motorLeft,     tmotorNormal, openLoop, encoder)
#pragma config(Motor,  motorB,          motorRight,    tmotorNormal, openLoop, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(NXT)

// tics2meters converts encoder tics to travelled distance in meters.
#define wheelDiam 0.056
float tics2meters(float tics)
{
  ...
}

// reverse
float meters2tics(float x)
{
  ...
}

// Converts velocity with meters per second to motor control
// which is given in percent of nMaxRegulatedSpeedNxt.
// nMaxRegulatedSpeedNxt is measured in tics per second.
float fullPowerVelocity;
int metersPerSecond2MotorControl(float v)
{
  ...
}

// tanh required in optimal Riccatti control
float tanh(float x)
{
  ... 
}

// Main task
task main()
{
    eraseDisplay();

    // Disable internal PID speed control
    nMotorPIDSpeedCtrl[motorLeft]  = mtrNoReg;
    nMotorPIDSpeedCtrl[motorRight] = mtrNoReg;

    // define variable
    fullPowerVelocity = 880; // in tics/s

    // write to display
    nxtDisplayTextLine(0, "text");
    nxtDisplayTextLine(1, " float:  %f", fullPowerVelocity);

    // Input file
    const string datFileName = "robotc.dat";
    TFileIOResult nIoResult;
    TFileHandle datFile;
    int nFileSize;

    // read from file
    float tf;
    OpenRead(datFile, nIoResult, datFileName, nFileSize);
    while(nIoResult == ioRsltSuccess)
    {
        ReadFloat(datFile, nIoResult, tf);
        float tmp;
        for (int i=1; i <= 4; i++)
            ReadFloat(datFile, nIoResult, tmp);
    }
    Close(datFile, nIoResult); 

    // initialize rotation sensor (the rotation sensor values are contained in the variables 
    // nMotorEncoder[motorLeft] and nMotorEncoder[motorRight] 
    wait1Msec(1000);
    nMotorEncoder[motorLeft] = 0;
    nMotorEncoder[motorRight] = 0;
    ClearTimer(T1);
    wait1Msec(10);

    // current time in seconds
    float t = ((float)time10[T1])/100.0;

    // set motor speed
    motor[motorLeft]  = ... 
    motor[motorRight] = ... 

    // wait 1 s before ending the program
    wait1Msec(1000);
}