//------------
//=================================================
// Task to handle the gyro and the other sensors hooked up to the sensor mux
//=================================================
task gyro()
{
float currDir = 0.0; float prevDir = 0.0;
long currtime,prevtime;
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);
wait1Msec(500);
}
wait1Msec(300);
for (int i=0;i<5;i++) // check if there is too much spread in the data
{
if (gyro_noise>10)
{
PlayTone (250,25);
wait1Msec(500);
}
}
calibrate = 2;
prevtime = nPgmTime;
while(true)
{
currtime=nPgmTime;
rawgyro = (float)HTGYROreadRot(HTGYRO);
constHeading += (rawgyro - drift) * (float)(currtime-prevtime)/1000;
relHeading += (rawgyro - drift) * (float)(currtime-prevtime)/1000;
prevtime = currtime;
wait1Msec(1);
prevDir = currDir;
}
}
void initializeRobot() {
bDisplayDiagnostics=false;
setGyroPos(false);
if (HTSMUXreadPowerStatus(MUX)) { // Multiplexer is off or dead?
nxtDisplayBigStringAt(0,45," MUX ");
nxtDisplayBigStringAt(0,25,"Dead Batt");
PlayImmediateTone(440, 50);
wait1Msec(1500);
eraseDisplay();
fallbackMode=true;
selection[SONAR_MENU]=3;
}
if (SensorRaw[gyro]>=640 || SensorRaw[gyro]<=590) { // Gyro moving or disconnected
nxtDisplayBigStringAt(21,45,"Gyro");
nxtDisplayBigStringAt(15,25,"Error");
nxtDisplayCenteredTextLine(7, "%i", SensorRaw[gyro]);
PlayImmediateTone(440, 50);
wait1Msec(1500);
setGyroEnabled(false);
} else {
wait1Msec(100);
nxtDisplayBigStringAt(15,45,"Wait...");
HTGYROstartCal(gyro);
PlaySound(soundBeepBeep);
setGyroEnabled(true);
}
eraseDisplay();
setClamp(true);
initMenus();
return;
}
// main task
task main ()
{
int _dirDC = 0;
int _dirAC = 0;
int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
int acS1, acS2, acS3, acS4, acS5 = 0;
string tmpString;
// show the user what to do
displayInstructions();
eraseDisplay();
for (int i = 0; i < 8; ++i)
sTextLines[i] = "";
// display the current DSP mode
// When connected to a SMUX, the IR Seeker V2 can only be
// used in 1200Hz mode.
nxtDisplayTextLine(0, " DC 1200");
// The sensor is connected to the first port
// of the SMUX which is connected to the NXT port S1.
// To access that sensor, we must use msensor_S1_1. If the sensor
// were connected to 3rd port of the SMUX connected to the NXT port S4,
// we would use msensor_S4_3
while (true)
{
// Read the current non modulated signal direction
_dirDC = HTIRS2readDCDir(HTIRS2);
if (_dirDC < 0)
break; // I2C read error occurred
// read the current modulated signal direction
_dirAC = HTIRS2readACDir(HTIRS2);
if (_dirAC < 0)
break; // I2C read error occurred
// Read the individual signal strengths of the internal sensors
// Do this for both unmodulated (DC) and modulated signals (AC)
if (!HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5))
break; // I2C read error occurred
if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 ))
break; // I2C read error occurred
displayText(1, "D", _dirDC, _dirAC);
displayText(2, "0", dcS1, acS1);
displayText(3, "1", dcS2, acS2);
displayText(4, "2", dcS3, acS3);
displayText(5, "3", dcS4, acS4);
displayText(6, "4", dcS5, acS5);
if (HTSMUXreadPowerStatus(HTSMUX))
nxtDisplayTextLine(7, "Batt: bad");
else
nxtDisplayTextLine(7, "Batt: good");
}
}
//------------
task gyro()
{
long currtime,prevtime;
while (HTSMUXreadPowerStatus(S2)) // check battery power is on
{
PlayTone(750,25);
wait1Msec(500);
}
SMUX_good = true;
while(calibrate != 1){};
wait1Msec(300);
HTGYROstartCal(HTGYRO);
float drift = MyHTCal(gyroCalTime*1000);
for (int i=0;i<5;i++) // check if there is too much spread in the data
{
if (abs(highest-lowest)>10)
{
PlayTone (250,25);
wait1Msec(500);
}
}
calibrate = 2;
prevtime = nPgmTime;
while(true)
{
//nxtDisplayBigTextLine(1, "G: %3f", relHeading);
currtime=nPgmTime;
newgyro = (float)HTGYROreadRot(HTGYRO);
constHeading += (newgyro - drift) * (float)(currtime-prevtime)/1000;
relHeading += (newgyro - drift) * (float)(currtime-prevtime)/1000;
prevtime = currtime;
wait1Msec(1);
prevDir = currDir;
}
}
// main task
task main ()
{
int _dirDC = 0;
int _dirAC = 0;
int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
int acS1, acS2, acS3, acS4, acS5 = 0;
// show the user what to do
displayInstructions();
while(true)
{
PlaySound(soundBeepBeep);
while(bSoundActive)
{}
eraseDisplay();
nNumbCyles = 0;
++nInits;
while (true)
{
if ((nNumbCyles & 0x04) == 0)
nxtDisplayTextLine(0, "Initializing...");
else
nxtDisplayTextLine(0, "");
nxtDisplayCenteredBigTextLine(1, "SMUX");
// Before using the SMUX, you need to initialise the driver
HTSMUXinit();
// Tell the SMUX to scan its ports for connected sensors
if (HTSMUXscanPorts(HTSMUX))
break;
++nNumbCyles;
PlaySound(soundShortBlip);
nxtDisplayTextLine(4, "Inits: %d / %d", nInits, nNumbCyles);
nxtDisplayCenteredTextLine(6, "Connect SMUX");
nxtDisplayCenteredTextLine(7, "to Port S1");
wait1Msec(100);
}
eraseDisplay();
for (int i = 0; i < 8; ++i)
sTextLines[i] = "";
// display the current DSP mode
// When connected to a SMUX, the IR Seeker V2 can only be
// used in 1200Hz mode.
nxtDisplayTextLine(0, " DC 1200");
// The sensor is connected to the first port
// of the SMUX which is connected to the NXT port S1.
// To access that sensor, we must use msensor_S1_1. If the sensor
// were connected to 3rd port of the SMUX connected to the NXT port S4,
// we would use msensor_S4_3
while (true)
{
// Read the current non modulated signal direction
_dirDC = HTIRS2readDCDir(msensor_S1_1);
if (_dirDC < 0)
break; // I2C read error occurred
// read the current modulated signal direction
_dirAC = HTIRS2readACDir(msensor_S1_1);
if (_dirAC < 0)
break; // I2C read error occurred
// Read the individual signal strengths of the internal sensors
// Do this for both unmodulated (DC) and modulated signals (AC)
if (!HTIRS2readAllDCStrength(msensor_S1_1, dcS1, dcS2, dcS3, dcS4, dcS5))
break; // I2C read error occurred
if (!HTIRS2readAllACStrength(msensor_S1_1, acS1, acS2, acS3, acS4, acS5 ))
break; // I2C read error occurred
displayText(1, "D", _dirDC, _dirAC);
displayText(2, "0", dcS1, acS1);
displayText(3, "1", dcS2, acS2);
displayText(4, "2", dcS3, acS3);
displayText(5, "3", dcS4, acS4);
displayText(6, "4", dcS5, acS5);
if (HTSMUXreadPowerStatus(HTSMUX))
nxtDisplayTextLine(7, "Batt: bad");
else
nxtDisplayTextLine(7, "Batt: good");
}
}
}
//===================================================
// 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
}
}
}
task sensors()
{
//-------------------------
// gyro
//-------------------------
long currtime,prevtime;
int acS[5];
while (HTSMUXreadPowerStatus(S3)) // check battery power is on
{
PlayTone(750,25);
wait1Msec(500);
}
SMUX_good = true;
while(calibrate != 1){};
wait1Msec(300);
HTGYROstartCal(HTGYRO);
float drift = MyHTCal(gyroCalTime*1000);
for (int i=0;i<5;i++) // check if there is too much spread in the data
{
if (abs(highest-lowest)>10)
{
PlayTone (250,25);
wait1Msec(500);
}
}
calibrate = 2;
prevtime = nPgmTime;
while(true)
{
currtime=nPgmTime;
newgyro = (float)HTGYROreadRot(HTGYRO);
constHeading += (newgyro - drift) * (float)(currtime-prevtime)/1000;
relHeading += (newgyro - drift) * (float)(currtime-prevtime)/1000;
prevtime = currtime;
wait1Msec(1);
//-------------------------
// IR
//-------------------------
bearingAC = HTIRS2readACDir(HTIRS2);
#define max(a, b) (((a) > (b))? (a): (b))
#define min(a, b) (((a) < (b))? (a): (b))
currDir = (float) bearingAC;
if (bearingAC == 0)
{
currDir = prevDir;
}
else
{
if (HTIRS2readAllACStrength(HTIRS2, acS[0], acS[1], acS[2], acS[3], acS[4]))
{
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;
////-------------------------
//// Sonar
////-------------------------
//num = USreadDist(LEGOUS);
//num2 = USreadDist(LEGOUS2);
//if(num != 255) sonarLive = num;
//if(num2 != 255) sonarLive2 = num2;
//-------------------------
// light
//-------------------------
LSsetActive(LEGOLS);
nrm = LSvalNorm(LEGOLS);
}
}