static
void GenerateGiantSteps(const ZZ_pX& f, const ZZ_pX& h, long l, long verbose)
{
double t;
if (verbose) { cerr << "generating giant steps..."; t = GetTime(); }
ZZ_pXModulus F;
build(F, f);
ZZ_pXArgument H;
build(H, h, F, 2*SqrRoot(F.n));
ZZ_pX h1;
h1 = h;
long i;
if (!use_files) {
GiantStepFile.kill();
GiantStepFile.SetLength(l);
}
for (i = 1; i <= l-1; i++) {
if (use_files) {
ofstream s;
OpenWrite(s, FileName(ZZ_pX_stem, "giant", i));
s << h1 << "\n";
s.close();
}
else
GiantStepFile(i) = h1;
CompMod(h1, h1, H, F);
if (verbose) cerr << "+";
}
if (use_files) {
ofstream s;
OpenWrite(s, FileName(ZZ_pX_stem, "giant", i));
s << h1 << "\n";
s.close();
}
else
GiantStepFile(i) = h1;
if (verbose)
cerr << (GetTime()-t) << "\n";
}
void NRLib::WriteStormBinarySurf(const RegularSurface<A> & surf,
const std::string & filename)
{
std::ofstream file;
OpenWrite(file, filename.c_str(), std::ios::out | std::ios::binary);
file.precision(14);
file << "STORMGRID_BINARY\n\n"
<< surf.GetNI() << " " << surf.GetNJ() << " "
<< surf.GetDX() << " " << surf.GetDY() << "\n"
<< surf.GetXMin() << " " << surf.GetXMax() << " "
<< surf.GetYMin() << " " << surf.GetYMax() << "\n";
if (surf.GetMissingValue() == STORM_MISSING) {
// Purify *sometimes* claims a UMR for the call below. No-one understands why...
WriteBinaryDoubleArray(file, surf.begin(), surf.end());
}
else {
std::vector<double> data(surf.GetN());
std::copy(surf.begin(), surf.end(), data.begin());
std::replace(data.begin(), data.end(), surf.GetMissingValue(), static_cast<A>(STORM_MISSING));
WriteBinaryDoubleArray(file, data.begin(), data.end());
}
file.close();
}
//----handles the storage of data----//
void swapFiles()
{
OpenWrite(hFileHandle2,nIoResultRead,sFileName2,nFileSize2);
OpenRead(hFileHandle,nIoResult,sFileName1,nFileSize);
float tempVar;
char tempX, tempY, tempTheta, z;
while(nIoResult != ioRsltEndOfFile)
{
for(z = 0; z<numNeuralUnits; z++)
{
ReadFloat(hFileHandle,nIoResult,tempVar);
if(nIoResult == ioRsltEndOfFile) {break;}
WriteFloat(hFileHandle2,nIoResultRead,tempVar);
}
if(nIoResult == ioRsltEndOfFile) {break;}
ReadByte(hFileHandle, nIoResult, tempX);
ReadByte(hFileHandle, nIoResult, tempY);
ReadByte(hFileHandle, nIoResult, tempTheta);
WriteByte(hFileHandle2,nIoResultRead,tempX);
WriteByte(hFileHandle2,nIoResultRead,tempY);
WriteByte(hFileHandle2,nIoResultRead,tempTheta);
}
char x;
for(x = 0; x<numNeuralUnits; x++)
{
WriteFloat(hFileHandle2, nIoResult, localTemp.localArray[x]);
}
WriteByte(hFileHandle2, nIoResult, poseWorld.maxActivatedCell.x);
WriteByte(hFileHandle2, nIoResult, poseWorld.maxActivatedCell.y);
WriteByte(hFileHandle2, nIoResult, poseWorld.maxActivatedCell.theta);
Close(hFileHandle2,nIoResultRead);
Close(hFileHandle,nIoResult);
Delete(sFileName1,nIoResult);
Rename(sFileName1,nIoResult,sFileName2);
}
task main()
{
TFileHandle Handle; // will keep track of our file
TFileIOResult Result; // will store our IO results
string Name = "test.txt"; // the name of our file
int Size = 100; // will store our file size
byte CR = 0x0D; // define CR (carriage return) ""
byte LF = 0x0A; // define LF (line feed) "changed for ANSI compatibility"
string sMessage; // we will write this to the file
Delete("test.txt", Result);
OpenWrite(Handle, Result, Name, Size); // open the file for writing (creates the file if it does not exist)
nMotorEncoder[motorA] = 0;
ClearTimer(T1);
while (time1[T1] < 5000)
{
sMessage = nMotorEncoder[motorA];
WriteText(Handle, Result, sMessage); // write 'sMessageToWrite' to the file
WriteByte(Handle, Result, CR); // write 'CR' to the file (carriage return)
WriteByte(Handle, Result, LF); // write 'LF' to the file (line feed)
wait1Msec(250);
}
Close(Handle, Result); // close the file (DON'T FORGET THIS STEP!)
}
task main(){
start_gyro(full_scale_range); // Fire up the gyro. Initialize it. Only needs to be done once.
long x_val, y_val, z_val; // Our assembled values.
const string sFileName = "GYRO2000.txt"; // Our destination KML File
TFileIOResult nIoResult;
TFileHandle hFileHandle;
int nFileSize = 5500;
hFileHandle = 0;
Delete(sFileName, nIoResult);
OpenWrite(hFileHandle, nIoResult, sFileName, nFileSize);
while (true){
// Read the GYROSCOPE
//x_val = gryo_axis_reading(0x00); // Get x-axis in dps.
y_val = gryo_axis_reading_linked(0x00);
//y_val = gryo_axis_reading(0x01); // Get x-axis in dps.
//z_val = gryo_axis_reading(0x02); // Get x-axis in dps.
string str_x_val = (string)x_val; // Turn it into a string to write to file.
nxtDisplayTextLine(5, "%i", x_val);
nxtDisplayTextLine(6, "%i", y_val);
nxtDisplayTextLine(7, "%i", z_val);
string write_coords = " ";
strcat(write_coords, str_x_val);
strcat(write_coords, "\r\n"); // Every line is a return and new line.
WriteString(hFileHandle, nIoResult, write_coords); // Write it all to file.
wait1Msec(50);
}
}
void initializeRobot()
{
// Place code here to sinitialize servos to starting positions.
// Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
string sFileName = "debug.txt";
int nFileSize =10000;
eraseDisplay();
nxtDisplayTextLine(0, "Autonomous Period");
Delete(sFileName,nIoResult);
OpenWrite(hFileHandle, nIoResult, sFileName, nFileSize);
WriteText(hFileHandle, nIoResult, "Autonomous Period\n");
nPidUpdateInterval = 20;
nMotorEncoder[motorR] = 0;
nMotorEncoder[motorL] = 0;
servoChangeRate[liftHigh] = 1;
servoChangeRate[liftLow] = 1;
liftBottom();
wait1Msec(10);
return;
}
task main()
{
eraseDisplay();
T("pie2");
//Calibrate();
Delete(sFileName, nIoResult);
nFileSize = 21 * 10000; // room for 10,000 entries
OpenWrite( hFileHandle, nIoResult, sFileName, nFileSize);
if (nIoResult != ioRsltSuccess)
{
T("Open failed");
writeDebugStream("nAvailFlash: %d", nAvailFlash);
StopAllTasks();
}
PressBumperToContinue();
FollowSegment();
motor[LEFT] = 0;
motor[RIGHT] = 0;
LLsleep(LINELEADER);
Close(hFileHandle, nIoResult);
PlaySound(soundFastUpwardTones);
wait1Msec(2000);
}
boolean CA_WriteFile(const char *filename, const void *ptr, long length)
{
ssize_t l;
int handle;
handle = OpenWrite(filename);
if (handle == -1)
return false;
l = WriteBytes(handle, (const byte *)ptr, length);
if (l == -1) {
perror("CA_FarWrite");
return false;
} else if (l == 0) {
fprintf(stderr, "CA_FarWrite hit EOF?\n");
return false;
} else if (l != length) {
fprintf(stderr, "CA_FarWrite only wrote %d out of %ld\n", l, length);
return false;
}
CloseWrite(handle);
return true;
}
bool IPlatformFile::CopyFile(const TCHAR* To, const TCHAR* From)
{
const int64 MaxBufferSize = 1024*1024;
TAutoPtr<IFileHandle> FromFile(OpenRead(From));
if (!FromFile.IsValid())
{
return false;
}
TAutoPtr<IFileHandle> ToFile(OpenWrite(To));
if (!ToFile.IsValid())
{
return false;
}
int64 Size = FromFile->Size();
if (Size < 1)
{
check(Size == 0);
return true;
}
int64 AllocSize = FMath::Min<int64>(MaxBufferSize, Size);
check(AllocSize);
uint8* Buffer = (uint8*)FMemory::Malloc(int32(AllocSize));
check(Buffer);
while (Size)
{
int64 ThisSize = FMath::Min<int64>(AllocSize, Size);
FromFile->Read(Buffer, ThisSize);
ToFile->Write(Buffer, ThisSize);
Size -= ThisSize;
check(Size >= 0);
}
FMemory::Free(Buffer);
return true;
}
void createTeleopConfigFile(string &sExecutableName)
{
TFileIOResult nIoResult;
TFileHandle hFileHandle;
short nFileSize;
deleteTeleOpConfigFile(); // Erase existing file
// Create the file
nFileSize = strlen(sExecutableName) + 4;
OpenWrite(hFileHandle, nIoResult, sTextFileName, nFileSize);
WriteText(hFileHandle, nIoResult, sExecutableName);
WriteText(hFileHandle, nIoResult, ".rxe");
Close(hFileHandle, nIoResult);
// Display Message
displayTextLine(5, "");
if(nIoResult == ioRsltSuccess)
displayCenteredTextLine(6, "File Created" );
else
displayCenteredTextLine(6, "File Error");
displayTextLine(7, "");
wait1Msec(1250);
return;
}
/**Creates file instance linked to filename. \sa Open.*/
File::File( const string& filename, bool writable ):
fp(NULL), contentSize(0), validName("")
{
if( writable )
OpenWrite( filename );
else
OpenRead( filename );
}
static
void GenerateBabySteps(GF2EX& h1, const GF2EX& f, const GF2EX& h, long k,
long verbose)
{
double t;
if (verbose) { cerr << "generating baby steps..."; t = GetTime(); }
GF2EXModulus F;
build(F, f);
GF2EXArgument H;
#if 0
double n2 = sqrt(double(F.n));
double n4 = sqrt(n2);
double n34 = n2*n4;
long sz = long(ceil(n34/sqrt(sqrt(2.0))));
#else
long sz = 2*SqrRoot(F.n);
#endif
build(H, h, F, sz);
h1 = h;
long i;
long HexOutput = GF2X::HexOutput;
GF2X::HexOutput = 1;
if (!use_files) {
BabyStepFile.kill();
BabyStepFile.SetLength(k-1);
}
for (i = 1; i <= k-1; i++) {
if (use_files) {
ofstream s;
OpenWrite(s, FileName(GF2EX_stem, "baby", i));
s << h1 << "\n";
s.close();
}
else
BabyStepFile(i) = h1;
CompMod(h1, h1, H, F);
if (verbose) cerr << "+";
}
if (verbose)
cerr << (GetTime()-t) << "\n";
GF2X::HexOutput = HexOutput;
}
void writeSide(short isLeft)
{
TFileHandle hFileHandle;
TFileIOResult nIoResult;
short nFileSize = 16;
Delete(SIDEDATADAT, nIoResult);
OpenWrite(hFileHandle, nIoResult, SIDEDATADAT, nFileSize);
WriteShort(hFileHandle, nIoResult, isLeft);
Close(hFileHandle, nIoResult);
}
void writeTeleopForwards()
{
TFileHandle hFileHandle;
TFileIOResult nIoResult;
float teleopForwards=readCompass();
short nFileSize=sizeof(teleopForwards);
OpenWrite(hFileHandle, nIoResult, TELEOPFORWARDSDAT, nFileSize);
WriteFloat(hFileHandle, nIoResult, teleopForwards);
Close(hFileHandle, nIoResult);
}
STDMETHODIMP_(BOOL) CKpFileTransactionImpl::OpenWrite(LPTSTR lpOutBufferFilePath)
{
std_string strOut;
const BOOL bResult = (m_pTx->OpenWrite(strOut) ? TRUE : FALSE);
if(lpOutBufferFilePath != NULL)
_tcscpy(lpOutBufferFilePath, strOut.c_str());
return bResult;
}
OutputStreamPtr Directory::OpenWrite(const String& fileName, bool append) {
String path;
MultiStringHelper pathsL(paths);
MultiStringHelper::Iterator it = pathsL.Iterate();
while (it.HasNext(path)) {
OutputStreamPtr ptr = OpenWrite(path, fileName, append);
if (ptr)
return ptr;
}
return OutputStreamPtr();
}
static
void BKZStatus(double tt, double enum_time, unsigned long NumIterations,
unsigned long NumTrivial, unsigned long NumNonTrivial,
unsigned long NumNoOps, long m,
const mat_ZZ& B)
{
cerr << "---- BKZ_XD status ----\n";
cerr << "elapsed time: ";
PrintTime(cerr, tt-StartTime);
cerr << ", enum time: ";
PrintTime(cerr, enum_time);
cerr << ", iter: " << NumIterations << "\n";
cerr << "triv: " << NumTrivial;
cerr << ", nontriv: " << NumNonTrivial;
cerr << ", no ops: " << NumNoOps;
cerr << ", rank: " << m;
cerr << ", swaps: " << NumSwaps << "\n";
ZZ t1;
long i;
double prodlen = 0;
for (i = 1; i <= m; i++) {
InnerProduct(t1, B(i), B(i));
if (!IsZero(t1))
prodlen += log(t1);
}
cerr << "log of prod of lengths: " << prodlen/(2.0*log(2.0)) << "\n";
if (LLLDumpFile) {
cerr << "dumping to " << LLLDumpFile << "...";
ofstream f;
OpenWrite(f, LLLDumpFile);
f << "[";
for (i = 1; i <= m; i++) {
f << B(i) << "\n";
}
f << "]\n";
f.close();
cerr << "\n";
}
LastTime = tt;
}
static
void GenerateBabySteps(ZZ_pEX& h1, const ZZ_pEX& f, const ZZ_pEX& h, long k,
FileList& flist, long verbose)
{
double t;
if (verbose) { cerr << "generating baby steps..."; t = GetTime(); }
ZZ_pEXModulus F;
build(F, f);
ZZ_pEXArgument H;
#if 0
double n2 = sqrt(double(F.n));
double n4 = sqrt(n2);
double n34 = n2*n4;
long sz = long(ceil(n34/sqrt(sqrt(2.0))));
#else
long sz = 2*SqrRoot(F.n);
#endif
build(H, h, F, sz);
h1 = h;
long i;
if (!use_files) {
(*BabyStepFile).SetLength(k-1);
}
for (i = 1; i <= k-1; i++) {
if (use_files) {
ofstream s;
OpenWrite(s, FileName("baby", i), flist);
s << h1 << "\n";
CloseWrite(s);
}
else
(*BabyStepFile)(i) = h1;
CompMod(h1, h1, H, F);
if (verbose) cerr << "+";
}
if (verbose)
cerr << (GetTime()-t) << "\n";
}
//----Writes the initial data----//
void writeLocal()
{
CloseAllHandles(nIoResult);
OpenWrite(hFileHandle,nIoResult,sFileName1,nFileSize);
char x;
for(x = 0; x<numNeuralUnits; x++)
{
WriteFloat(hFileHandle, nIoResult, localTemp.localArray[x]);
}
WriteByte(hFileHandle, nIoResult, poseWorld.maxActivatedCell.x);
WriteByte(hFileHandle, nIoResult, poseWorld.maxActivatedCell.y);
WriteByte(hFileHandle, nIoResult, poseWorld.maxActivatedCell.theta);
Close(hFileHandle,nIoResult);
}
/** Writes the signature ls to the file whose named can be
identified using index (e.g, if index is 1, then ls is going
to be written in file loc_01.dat). If file already exists,
it should be replaced (FOR SECURITY, IT IS BETTER THAT FILE
DID NOT ALREADY EXIST). */
void write_signature_to_file(loc_sig& ls, short index)
{
ASSERT(index >= 0 && index < NO_LOCS);
TFileIOResult nIoResult;
TFileHandle hFileHandle;
string sFileName = file_names[index];
Delete(sFileName, nIoResult);
OpenWrite(hFileHandle, nIoResult, sFileName, nFileSize);
for ( short i=0; i<NO_BINS; ++i )
WriteShort(hFileHandle, nIoResult, ls.sig[i]);
Close(hFileHandle, nIoResult);
}
task main(){
if(!readCalib()) return; //reads calibration informations, return if it fails.
// initialize the log file
const string logName = "E2Log.txt";
short logSize = 2000;
Delete(logName, logResultIO);
OpenWrite(logHandle, logResultIO,logName, logSize);
//use PID controller to follow the line
PIDDriver(threshold + targetCalib);
Close(logHandle, logResultIO);
return;
}
void writeScanArray(){
TFileHandle myFileHandle; // create a file handle variable 'myFileHandle'
TFileIOResult IOResult; // create an IO result variable 'IOResult'
string myFileName = "readA.txt"; // create and initialize a string variable 'myFileName'
int myFileSize = 400; // create and initialize an integer variable 'myFileSize'
OpenWrite(myFileHandle, IOResult, myFileName, myFileSize); // open for write: "myFile.txt"
int i;
string svalue;
for(i=0;i<40;i++){
sprintf(svalue,"%3d ",readings[i]);
WriteString(myFileHandle, IOResult, svalue);
}
Close(myFileHandle, IOResult);
}
//============================================================================
// remove old file, open new one and write the header
//
int open_flight_record(const string sFileName) {
TFileIOResult nIoResult;
int nFileSize = 15 * 1024;
ClearTimer(T4);
Delete(sFileName, nIoResult);
OpenWrite(flightDatFH, nIoResult, sFileName, nFileSize);
if (nIoResult != 0) {
wait10Msec(50);
StopAllTasks();
}
WriteText(flightDatFH, nIoResult, "int flight[][X]={\n"); //X=MotorNumber+ 1
wait10Msec(5);
return(0);
}
void NRLib::WriteIrapClassicAsciiSurf(const RegularSurface<A> & surf,
double angle,
const std::string & filename)
{
std::ofstream file;
OpenWrite(file, filename);
file << std::fixed
<< std::setprecision(6)
<< -996 << " "
<< surf.GetNJ() << " "
<< surf.GetDX() << " "
<< surf.GetDY() << "\n"
<< std::setprecision(2)
<< surf.GetXMin() << " "
<< surf.GetXMax() << " "
<< surf.GetYMin() << " "
<< surf.GetYMax() << "\n"
<< surf.GetNI() << " "
<< std::setprecision(6)
<< angle*180/NRLib::Pi << " "
<< std::setprecision(2)
<< surf.GetXMin() << " "
<< surf.GetYMin() << "\n"
<< " 0 0 0 0 0 0 0\n";
file.precision(6);
if (surf.GetMissingValue() == IRAP_MISSING) {
for (size_t i = 0; i < surf.GetN(); i++) {
file << surf(i) << " ";
if((i+1) % 6 == 0)
file << "\n";
}
}
else {
for (size_t i = 0; i < surf.GetN(); i++) {
if (surf.IsMissing(surf(i)))
file << IRAP_MISSING << " ";
else
file << surf(i) << " ";
if((i+1) % 6 == 0)
file << "\n";
}
}
file.close();
}
static void LLLStatus(long max_k, double t, long m, const mat_ZZ& B)
{
cerr << "---- LLL_XD status ----\n";
cerr << "elapsed time: ";
PrintTime(cerr, t-StartTime);
cerr << ", stage: " << max_k;
cerr << ", rank: " << m;
cerr << ", swaps: " << NumSwaps << "\n";
ZZ t1;
long i;
double prodlen = 0;
for (i = 1; i <= m; i++) {
InnerProduct(t1, B(i), B(i));
if (!IsZero(t1))
prodlen += log(t1);
}
cerr << "log of prod of lengths: " << prodlen/(2.0*log(2.0)) << "\n";
if (LLLDumpFile) {
cerr << "dumping to " << LLLDumpFile << "...";
ofstream f;
OpenWrite(f, LLLDumpFile);
f << "[";
for (i = 1; i <= m; i++) {
f << B(i) << "\n";
}
f << "]\n";
f.close();
cerr << "\n";
}
LastTime = t;
}
static
void GenerateBabySteps(ZZ_pX& h1, const ZZ_pX& f, const ZZ_pX& h, long k,
FileList& flist, long verbose)
{
double t;
if (verbose) { cerr << "generating baby steps..."; t = GetTime(); }
ZZ_pXModulus F;
build(F, f);
ZZ_pXArgument H;
build(H, h, F, 2*SqrRoot(F.n));
h1 = h;
long i;
if (!use_files) {
(*BabyStepFile).SetLength(k-1);
}
for (i = 1; i <= k-1; i++) {
if (use_files) {
ofstream s;
OpenWrite(s, FileName("baby", i), flist);
s << h1 << "\n";
CloseWrite(s);
}
else
(*BabyStepFile)(i) = h1;
CompMod(h1, h1, H, F);
if (verbose) cerr << "+";
}
if (verbose)
cerr << (GetTime()-t) << "\n";
}
task datalog()
{
Delete(IRFileName, IRIoResult); // delete any preexisting file
OpenWrite( IRFileHandle, IRIoResult, IRFileName, IRFileSize); // create and open a new file
while(!LogData){EndTimeSlice();} // wait until we are instructed to start gathering data
while(LogData) // add samples to the file
{
StringFormat(sString, "%4d", step); // autonomous program step number
WriteText(IRFileHandle, IRIoResult, sString);
StringFormat(sString, "%7d", IR_Bearing); //our calculated bearing IR one
WriteText(IRFileHandle, IRIoResult, sString);
StringFormat(sString, "%7d", bearingAC); // IR calculated bearing IR one
WriteText(IRFileHandle, IRIoResult, sString);
StringFormat(sString, "%7d", IR_Bearing2); // our calculated bearing IR two
WriteText(IRFileHandle, IRIoResult, sString);
StringFormat(sString, "%7d", bearingAC2); // IR calculated bearing IR two
strcat(sString,CRLF);
WriteText(IRFileHandle, IRIoResult, sString);
wait1Msec(50); // sample frequency
}
Close(IRFileHandle, IRIoResult); // close the file once we are told to stop
wait10Msec(20);
}
task main()
{
CloseAllHandles(nIoResult); // make sure everything is closed
wait1Msec(500);
OpenRead(hFileHandle, nIoResult, sFileName , nFileSize); // open the existing file
ReadFloat(hFileHandle, nIoResult, fParmToReadFloat); // read a single float value from it
Close(hFileHandle, nIoResult); // and close the file
while(nNxtButtonPressed!=3) // continue in editor until enter button is pressed
{if (nNxtButtonPressed==1)
{
while(nNxtButtonPressed==1){}; // right arrow increases value
fParmToReadFloat+=1;
}
if (nNxtButtonPressed==2)
{
while(nNxtButtonPressed==2){}; // left arrow decreases value
fParmToReadFloat-=1;
}
nxtDisplayCenteredBigTextLine(6, "%3.1f", fParmToReadFloat); // update the display
}
Delete(sFileName, nIoResult); // delete pre-existing file (if any)
nxtDisplayBigTextLine(0, "Deleted");
wait1Msec(2000);
nFileSize = 100;
OpenWrite( hFileHandle, nIoResult, sFileName, nFileSize); // create new file
nxtDisplayBigTextLine(2, "Opened");
wait1Msec(2000);
WriteFloat( hFileHandle, nIoResult, fParmToReadFloat); // write our value
nxtDisplayBigTextLine(4, "Written");
wait1Msec(2000);
Close(hFileHandle, nIoResult); // and close the file
nxtDisplayBigTextLine(6, "Closed");
wait1Msec(2000);
}
//===================================================
// task to read in all sensors to workspace variables
//===================================================
task sensors()
{
float currDir = 0.0; //prevDir = 0.0,
long currtime,prevtime;
LSsetActive(LEGOLS); // set the LEGO light sensor to active mode
//-------------------------
// gyro
//-------------------------
ir_mux_status=HTSMUXreadPowerStatus(IR_MUX); // read the sensor multiplexor status
gyro_mux_status=HTSMUXreadPowerStatus(GYRO_MUX); // read the sensor multiplexor status
while (ir_mux_status || gyro_mux_status) // check good battery power on both muxes
{
PlayTone(750,25); // if not we beep indefinitely
wait1Msec(500);
}
//SMUX_good = true;
while(calibrate != 1){}; // wait for a request to start calibrating the gyro
wait1Msec(300); // short delay to ensure that user has released the button
HTGYROstartCal(HTGYRO); // initiate the GYRO calibration
drift = MyHTCal(gyroCalTime*1000);
Driver_Cal = HTGYROreadCal(HTGYRO); // read the calculated calibration value for saving to file
//---------------------------------------
// write the GYRO calibration data to file for Tele-Op
//---------------------------------------
Delete(sFileName, nIoResult); // delete any pre-existing file
nFileSize = 100; // new file size will be 100 bytes
OpenWrite( hFileHandle, nIoResult, sFileName, nFileSize); // create and open the new file
WriteFloat( hFileHandle, nIoResult, drift); // write the current drift value to the file
WriteFloat( hFileHandle, nIoResult, Driver_Cal); // write the driver calibration to the file
Close(hFileHandle, nIoResult); // close the file
//---------------------------------------
for (int i=0;i<5;i++) // check if there is too much spread in the data
{
if (gyro_noise>10) // if there is too much spread we beep 5 times to alert the drive team
{
gyroTrue = true;
PlayTone (250,25);
wait1Msec(500);
}
}
calibrate = 2; // this signifies to the main program that calibration has been completed
prevtime = nPgmTime;
while(true)
{
currtime=nPgmTime;
rawgyro = HTGYROreadRot(HTGYRO);
constHeading += (rawgyro - drift) * (float)(currtime-prevtime)/1000;
relHeading += (rawgyro - drift) * (float)(currtime-prevtime)/1000;
prevtime = currtime;
//wait1Msec(1);
//---------------------------------------------------------------------
// Read both sonar sensors and filter out non-valid echo readings (255)
// If there is no echo the filter just retains the last good reading
//---------------------------------------------------------------------
sonarRaw = USreadDist(LEGOUS); // read the rear mounted sensor
if (sonarRaw!=255) sonarLive = sonarRaw; // and copy valid results to workspace
sonarRaw2 = USreadDist(LEGOUS2); // read the side mounted sensor
if (sonarRaw2!=255) sonarLive2 = sonarRaw2; // and copy valid results to workspace
//-------------------------
// LEGO light sensor
//-------------------------
light_normalised = LSvalNorm(LEGOLS); // read the LEGO light sensor
//-------------------------
// HiTechnic IR Sensor
//-------------------------
bearingAC = HTIRS2readACDir(HTIRS2); // Read the IR bearing from the sensor
bearingAC2 = HTIRS2readACDir(HTIRS2_2);//here 12334
currDir = (float) bearingAC; // copy into workspace -
/*if (bearingAC == 0) // No IR signal is being detected
{
currDir = prevDir; // so retain the previous reading
}
else // otherwise read all the IR segments
{
{
bearingAC = (bearingAC - 1)/2;
if ((bearingAC < 4) && (acS[bearingAC] != 0) && (acS[bearingAC + 1] != 0))
{
currDir += (float)(acS[bearingAC + 1] - acS[bearingAC])/
max(acS[bearingAC], acS[bearingAC + 1]);
}
}
}
prevDir = currDir;
IR_Bearing=currDir-5; // and setup the main variable for others to use
*/
HTIRS2readAllACStrength(HTIRS2, acS[0], acS[1], acS[2], acS[3], acS[4]);
HTIRS2readAllACStrength(HTIRS2_2, acS2[0], acS2[1], acS2[2], acS2[3], acS2[4]);
//-----------------------------------
// code for the peaks of IR sensor 1
//-----------------------------------
if (bearingAC!=0) // we have a valid IR signal
{
int maximum = -1;
int peak = 0, offset=0;
for (int i=0;i<5;i++) // scan array to find the peak entry
{ if (acS[i]>maximum)
{peak = i;
maximum = acS[i];
}
}
offset=0;
if ((peak < 4) && (peak>0) && (acS[peak] != 0)) // we are not working with extreme value
{
if (acS[peak-1]!=acS[peak+1]) // if the values either side of the peak are identical then peak is peak
{
if (acS[peak-1]>acS[peak+1]) // otherwise decide which side has higher signal
{
offset = -25*(1-(float)(acS[peak]-acS[peak-1])/ // calculate the bias away from the peak
max(acS[peak], acS[peak-1]));
}
else
{
offset = 25*(1-(float)(acS[peak]-acS[peak+1])/
max(acS[peak], acS[peak+1]));
}
}
}
IR_Bearing = (float)((peak-2)*50) + offset; // direction is the total of the peak bias plus the adjacent bias
// range is -100 to +100, zero is straight ahead
}
//-----------------------------------
// code for the peaks of IR sensor 2
//-----------------------------------
if (bearingAC2!=0) // we have a valid IR signal
{
int maximum = -1;
int peak = 0, offset=0;
for (int i=0;i<5;i++) // scan array to find the peak entry
{ if (acS2[i]>maximum)
{peak = i;
maximum = acS2[i];
}
}
offset=0;
if ((peak < 4) && (peak>0) && (acS2[peak] != 0)) // we are not working with extreme value
{
if (acS2[peak-1]!=acS2[peak+1]) // if the values either side of the peak are identical then peak is peak
{
if (acS2[peak-1]>acS2[peak+1]) // otherwise decide which side has higher signal
{
offset = -25*(1-(float)(acS2[peak]-acS2[peak-1])/ // calculate the bias away from the peak
max(acS2[peak], acS2[peak-1]));
}
else
{
offset = 25*(1-(float)(acS2[peak]-acS2[peak+1])/
max(acS2[peak], acS2[peak+1]));
}
}
}
IR_Bearing2 = (float)((peak-2)*50) + offset; // direction is the total of the peak bias plus the adjacent bias
// range is -100 to +100, zero is straight ahead
}
}
}
/* Displays a menu allowing the user to select between the
options in options[], and returns the index of the
selected item */
int menu(void) {
/* Initialize variables */
int index = 0;
bool selected = false;
/* Run until and entry is selected */
while(!selected) {
/* Pause, to remove LCD flickering */
wait1Msec(25);
/* Clear the display */
eraseDisplay();
/* Display the current option and index */
nxtDisplayCenteredTextLine(4, "%s", options[index]);
nxtDisplayCenteredTextLine(6, "%d", index);
/* Check for a button press */
switch(nNxtButtonPressed) {
/* Right arrow pressed */
case kRightButton:
index++;
break;
/* Left arrow pressed */
case kLeftButton: /* left */
index--;
break;
/* Enter button pressed */
case kEnterButton: /* enter */
selected = true;
break;
/* Anything else */
default:
/* Do nothing */
}
/* Keep the index inbounds */
index = index < 0 ? size - 1 : index;
index = index > size - 1 ? 0 : index;
/* Wait for a depressed button to be released */
while(nNxtButtonPressed != -1) {
/* do nothing */
}
}
return index;
}
/* Entry point for the code */
task main() {
/* File IO variables */
TFileHandle file;
TFileIOResult result;
/* Setup teleop options */
string selection = "";
options[0] = "TNK_EXP";
options[1] = "TNK_LIN";
options[2] = "TNK_LOW";
options[3] = "ARC_EXP";
options[4] = "ARC_LIN";
options[5] = "ARC_LOW";
size = 6;
/* Get the selection */
selection = menu();
/* Write the option to teleoprc.txt, overwriting it if necessary */
Delete("teleoprc.txt", result);
OpenWrite(file, result, "teleoprc.txt", FILE_SIZE);
WriteText(file, result, selection);
Close(file, result);
}