task main () {
nxtDisplayCenteredTextLine(0, "HiTechnic");
nxtDisplayCenteredBigTextLine(1, "GYRO");
nxtDisplayCenteredTextLine(3, "SMUX Test");
nxtDisplayCenteredTextLine(5, "Connect SMUX to");
nxtDisplayCenteredTextLine(6, "S1 and sensor to");
nxtDisplayCenteredTextLine(7, "SMUX Port 1");
wait1Msec(2000);
nxtDisplayCenteredTextLine(5, "Press enter");
nxtDisplayCenteredTextLine(6, "to set relative");
nxtDisplayCenteredTextLine(7, "heading");
wait1Msec(2000);
// Before using the SMUX, you need to initialise the driver
HTSMUXinit();
// Tell the SMUX to scan its ports for connected sensors
HTSMUXscanPorts(HTSMUX);
// 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
wait1Msec(2000);
eraseDisplay();
time1[T1] = 0;
while(true) {
if (time1[T1] > 1000) {
eraseDisplay();
nxtDisplayTextLine(1, "Resetting");
nxtDisplayTextLine(1, "heading");
wait1Msec(500);
// Start the calibration and display the offset
nxtDisplayTextLine(2, "Offset: %4d", HTGYROstartCal(msensor_S1_1));
PlaySound(soundBlip);
while(bSoundActive);
time1[T1] = 0;
}
while(nNxtButtonPressed != kEnterButton) {
eraseDisplay();
nxtDisplayTextLine(1, "Reading");
// Read the current calibration offset and display it
nxtDisplayTextLine(2, "Offset: %4d", HTGYROreadCal(msensor_S1_1));
nxtDisplayClearTextLine(4);
// Read the current rotational speed and display it
nxtDisplayTextLine(4, "Gyro: %4d", HTGYROreadRot(msensor_S1_1));
nxtDisplayTextLine(6, "Press enter");
nxtDisplayTextLine(7, "to recalibrate");
wait1Msec(100);
}
}
}
task main () {
nxtDisplayCenteredTextLine(0, "HiTechnic");
nxtDisplayCenteredBigTextLine(1, "GYRO");
nxtDisplayCenteredTextLine(3, "SMUX Test");
nxtDisplayCenteredTextLine(5, "Connect SMUX to");
nxtDisplayCenteredTextLine(6, "S1 and sensor to");
nxtDisplayCenteredTextLine(7, "SMUX Port 1");
wait1Msec(2000);
eraseDisplay();
time1[T1] = 0;
while(true) {
if (time1[T1] > 1000) {
eraseDisplay();
nxtDisplayTextLine(1, "Resetting");
nxtDisplayTextLine(1, "heading");
wait1Msec(500);
// Start the calibration and display the offset
nxtDisplayTextLine(2, "Offset: %4d", HTGYROstartCal(HTGYRO));
PlaySound(soundBlip);
while(bSoundActive) EndTimeSlice();
time1[T1] = 0;
}
while(nNxtButtonPressed != kEnterButton) {
eraseDisplay();
nxtDisplayTextLine(1, "Reading");
// Read the current calibration offset and display it
nxtDisplayTextLine(2, "Offset: %4d", HTGYROreadCal(HTGYRO));
nxtDisplayClearTextLine(4);
// Read the current rotational speed and display it
nxtDisplayTextLine(4, "Gyro: %4d", HTGYROreadRot(HTGYRO));
nxtDisplayTextLine(6, "Press enter");
nxtDisplayTextLine(7, "to recalibrate");
wait1Msec(100);
}
}
}
task displaySensorValues()
{
while(true)
{
IRLeftValue = HTIRS2readDCDir(IRLeft);
IRRightValue = HTIRS2readDCDir(IRRight);
gyroValue = HTGYROreadCal(gyro);
gyroValue2 = HTGYROreadRot(gyro);
ultrasoundValue = USreadDist(ultrasound);
eraseDisplay();
nxtDisplayString(0, "ir1: %d", IRLeftValue);
nxtDisplayString(2, "gyrocal: %d", gyroValue);
nxtDisplayString(3, "gyrorot: %d", gyroValue2);
nxtDisplayString(5, "ir2: %d", IRRightValue);
nxtDisplayString(7, "ultra: %d", ultrasoundValue);
Wait1Msec(2);
}
}
//===================================================
// 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
}
}
}
#define sideSpeed 0.8 /*sets sideways speed. (1 = full sensitivity)*/
#define slowModeThreshold 20 /*sets "slow mode" threshold.(1-128) (when going below this speed in either
direction DON'T move diagonally. makes driving slowly easier.)*/
#define fastMultiplier 2 /*how much faster to go in fast mode (1 = standard speed.)*/
#define slowMultiplier 0.5 /*how much slower to go in slow mode(1 = standard speed.)*/
#define backboardUp 250
#define backboardDown 15
#define grabberDown 100
#define grabberUp 140
int IRLeftValue = HTIRS2readDCDir(IRLeft);
int IRRightValue = HTIRS2readDCDir(IRRight);
float gyroValue = HTGYROreadCal(gyro);
float gyroValue2 = HTGYROreadRot(gyro);
int ultrasoundValue = USreadDist(ultrasound);
task displaySensorValues()
{
while(true)
{
IRLeftValue = HTIRS2readDCDir(IRLeft);
IRRightValue = HTIRS2readDCDir(IRRight);
gyroValue = HTGYROreadCal(gyro);
gyroValue2 = HTGYROreadRot(gyro);
ultrasoundValue = USreadDist(ultrasound);