driver_sample.c
— 2.2 KB
File contents
#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); }