/*turn on the spot if the timer expires, return false if it could not find another branch*/
bool turnOnSpot(short target, bool greyPatch){
float cur_compass = compass();
//start rotating
stop();
control(-rotateSpeed, rotateSpeed);
while(compass() - cur_compass < 390){
float diff = compass() - cur_compass;
if(SensorValue[Light] < target && (diff < 180 || diff > 250)){ // ignore the original branch
time1[T1] = 0;
PlayTone(1175,20);
stop();
adjustCount++;
return true; // return true if it finds another branch
}
}
// on the endpoint
stop();
if(!greyPatch){ // if not yet detect a grey patch, turn back
control(-rotateSpeed, rotateSpeed);
while(SensorValue[Light] > target){}
stop();
}
return false;
}
// turn on the spot and the number of branches it finds
void turnAndDisplay(){
motor[Left] = 0;
motor[Right] = 0;
wait1Msec(100);
motor[Left] = -rotateSpeed;
motor[Right] = rotateSpeed;
int count = 0;
int cur_compass = compass();
// initialize wasDark, a flag that records whether a previous iteration was dark
bool wasDark;
if(SensorValue[Light] < threshold)
wasDark = true;
else
wasDark = false;
// turn 360 degrees to find number of branches
while(compass() - cur_compass < 360){
if(SensorValue[Light] < threshold){
if(!wasDark){
wasDark = true;
count++; // count increment if there is a bright to dark transition
}
}
else{
wasDark = false;
}
}
motor[Left] = 0;
motor[Right] = 0;
nxtDisplayTextLine(3, "#branches = %d", count);
}
// PRETTY LIGHTS
void leddriver::testSequence() {
int do_loop = 1;
int counter = 0;
int decimalz = 1;
int ledstat = 0;
while (do_loop) {
for (int x = 0; x < 16; x++) {
compass(x);
counter++;
if (counter >= 200) {
do_loop = 0;
break;
}
sevenSeg(counter % 100, decimalz);
for (int y = 0; y < 10; y++) {
tickLEDs();
_delay_ms(2);
}
}
decimalz = !decimalz;
ledstat = !ledstat;
statusLED(ledstat);
}
}
void CompassBench()
{
for( int i = 0; i < 10; i++) {
INIT_TIMER
START_TIMER
auto test = prewitt_h_kernel;
std::vector<std::unique_ptr<cv::Mat>> compass(8);
for(int i = 0; i < 8; i++)
{
compass[i] = ImageConvolute32S(src, test);
if(i < 7) {
test = RotateKernel(test, 45);
}
}
std::vector<cv::Mat*> compass2(8);
for(int i = 0; i < 8; i++)
{
compass2[i] = compass[i].get();
}
(void)FindHighestResponseValues(compass2);
STOP_TIMER("Time")
}
}
int main(void)
{
struct heading result;
/*u08 k=0;
u08 b=0;
u08 choice=0;
u08 button=0;*/
u08 ir=0;
u08 i=0;
x = 64;
y = 48;
initialize();
motor_init();
servo_init();
set_motor_power(0,0);
set_motor_power(1,0);
set_motor_power(2,0);
set_motor_power(3,0);
compass_init();
sonar_init();
calCompass();
while(1)
{
clear_screen();
result = compass();
/* use calibration data */
result.x -= compZero.x;
result.y -= compZero.y;
print_string("x "); /* 2 */
print_int(result.x); /* 3 */
print_string(" y "); /* 3 */
print_int(result.y); /* 3 */
/* print_string(" s "); */ /* 3 */
/* print_int(getSonar(0)); */ /* 3 */
/* =17 */
next_line();
/* print_string("a "); */ /* 2 */
/* print_int(IR(0)); */ /* 3 */
/* print_string(" b "); */ /* 3 */
/* print_int(analog(1)); */ /* 3 */
/* print_string(" c "); */ /* 3 */
/* print_int(analog(2)); */ /* 3 */
/* print_int(i++); */ /* =17 */
/* print_int(distance(0));
print_string(" "); */
print_int(sizeof(int));
delay_ms(200);
/* print_int(i);
i=sweep(); */
}
}
// get signed compass value with adjustment
float signedCompass(){
float comp = compass() + adjustAmount * adjustCount;
while(comp > 180.0){
comp -= 360.0;
}
while(comp < -180.0){
comp += 360.0;
}
return comp;
}
int
main(int argc, char *argv[]) try
{
if (argc != 2)
{
std::cerr << argv[0] << ": Need argument - the number of the channel" << std::endl;
std::cerr << "\tUsage: " << argv[0] << " n " << std::endl;
std::cerr << "\tWhere n = 0,1,2..." << std::endl;
return EXIT_FAILURE;
}
struct sigaction act;
act.sa_handler = sig_handler;
sigemptyset(&act.sa_mask);
act.sa_flags = 0;
if (sigaction(SIGINT, &act, NULL) < 0)
{
throw std::runtime_error("Error set signal handler");
}
std::ostringstream oss;
oss << NAME_FILE << argv[1];
Compass compass(oss.str());
compass.setScale(1.3);
compass.setMeasurementMode(MEASUREMENT_CONTINUOUS);
while (work)
{
usleep(1000000);
MagnetometerRaw raw = compass.readRawAxis();
MagnetometerScaled scaled = compass.readScaledAxis();
double heading = atan2(scaled.YAxis, scaled.XAxis);
double declinationAngle = 0.1821; // magnetic declination in radians for Moscow
heading += declinationAngle;
if(heading < 0)
heading += 2 * M_PI;
if(heading > 2 * M_PI)
heading -= 2 * M_PI;
double headingDegrees = heading * 180 / M_PI;
std::cout << "Raw:" << std::setw(5) << raw.XAxis << std::setw(5) << raw.YAxis << std::setw(5)
<< raw.ZAxis << " Scaled:" << std::fixed << std::setprecision(3) << std::setw(10)
<< scaled.XAxis << std::setw(10) << scaled.YAxis << std::setw(10) << scaled.ZAxis
<< " Heading:" << std::setprecision(6) << std::setw(10) << heading << " rad"
<< std::setprecision(2) << std::setw(8) << headingDegrees << " deg" << std::endl;
}
return EXIT_SUCCESS;
}
catch(const std::exception& er)
{
std::cerr << argv[0] << ": " << er.what() << std::endl;
return EXIT_FAILURE;
}
catch(...)
{
std::cerr << argv[0] << ": Unknown error" << std::endl;
return EXIT_FAILURE;
}
void PIDDriver(short target){
float e, e_dot, old_e = threshold - SensorValue[Light], E = 0;
short u;
time1[T1] = 0;
time1[T2] = 0;
time1[T4] = 0;
float pre_compass = compass();
while(true){
if(time1[T1] < timeout){ // T1 for timeout
e = threshold - SensorValue[Light];
e_dot = e - old_e;
E += e;
u = (short)(KP * e + KD * e_dot + KI * E);
old_e = e;
motor[Left] = cruise_speed - u;
motor[Right] = cruise_speed + u;
if(SensorValue[Light] < target){ //target can be make more conservative than threshold
time1[T1] = 0;
}
if(time1[T2] > sampleFreq && SensorValue[Light] < target){ //the light sensor should be on the tape when it beeps
float cur_compass = compass();
if(time1[T3] > beepInterval && ((cur_compass - pre_compass) > beepThreshold || (pre_compass - cur_compass) > beepThreshold)){
PlayTone(1175, 20);
time1[T3] = 0; // T3 for beepInterval
}
pre_compass = cur_compass;
time1[T2] = 0; // T2 for sampling frequency
float tmp = signedCompass();
nxtDisplayStringAt(0,31,"%f", tmp);
logCompass(tmp);
}
}
else{
if(!turnOnSpot(target)) return;
}
}
}
void PIDDriver(short cruise_speed, short target, float KP, float KD, float KI){
float e, e_dot, old_e = threshold - SensorValue[Light], E = 0;
short u;
time1[T1] = 0;
time1[T2] = 0;
float pre_compass = compass();
while(true){
if(time1[T1] < timeout){
e = threshold - SensorValue[Light];
e_dot = e - old_e;
E += e;
u = (short)(KP * e + KD * e_dot + KI * E);
old_e = e;
motor[Left]=cruise_speed - u;
motor[Right]=cruise_speed + u;
if(SensorValue[Light] < target){ //target can be make more conservative than threshold
time1[T1] = 0;
}
if(time1[T2] > sampleFreq && SensorValue[Light] < target ){ //the light sensor should be on the tape when it beeps
float cur_compass = compass();
if(time1[T3] > beepInterval && ((cur_compass - pre_compass) > beepThreshold || (pre_compass - cur_compass) > beepThreshold)){
PlayTone(1175, 20);
turnAndDisplay();
break;
}
pre_compass = cur_compass;
time1[T2] = 0;
}
}
else{
turnAndDisplay(); return;
}
}
}
/*DRAFT IF IN A CITY*/
void draft(void) {
short armynum, x, y, i;
long men = 0, mercs;
short army = (-1), isgod = FALSE, newtype = 0;
long i_cost, e_cost;
char ch;
clear_bottom(0);
if (country == 0) {
isgod = TRUE;
country = sct[XREAL][YREAL].owner;
curntn = &ntn[country];
} else if (sct[XREAL][YREAL].owner != country) {
errormsg("You do not own");
return;
}
if ((sct[XREAL][YREAL].designation != DTOWN) &&
(sct[XREAL][YREAL].designation != DCAPITOL) &&
(sct[XREAL][YREAL].designation != DCITY)) {
errormsg("must raise in towns/cities/capitols");
if (isgod == TRUE)
reset_god();
return;
}
if (curntn->tgold <= 0) {
errormsg("You are broke");
if (isgod == TRUE)
reset_god();
return;
}
if (ISCITY(sct[XREAL][YREAL].designation) &&
(sct[XREAL][YREAL].people * (3 * CITYLIMIT + (curntn->tsctrs / 2)) <
curntn->tciv)) {
mvprintw(LINES - 1, 0, "Need %d people in sector: hit any key",
curntn->tciv / (3 * CITYLIMIT + (curntn->tsctrs / 2)));
refresh();
getch();
if (isgod == TRUE)
reset_god();
return;
}
/* ask what type of unit */
y = LINES - 2;
mvaddstr(y, 0, "options: 1) spy 2) scout");
clrtoeol();
x = 25;
for (i = 0; i <= NOUNITTYPES; i++) {
if (unitvalid(i) == TRUE) {
mvprintw(y, x + 2, "%s", *(shunittype + i));
mvprintw(y, x, "(%c)", *(shunittype + i)[0]);
x += strlen(*(shunittype + i)) + 3;
if (x > COLS - 10) {
x = 0;
y++;
}
}
}
move(y, x);
clrtoeol();
if ((magic(country, WARRIOR) == TRUE) || (magic(country, WARLORD) == TRUE) ||
(magic(country, CAPTAIN) == TRUE))
mvaddstr(LINES - 3, 0,
"(Warrior = 1/2 enlist cost) what type of unit do you want:");
else
mvaddstr(LINES - 3, 0, "what type of unit do you want to raise:");
clrtoeol();
refresh();
ch = getch();
for (newtype = 0; newtype <= NOUNITTYPES; newtype++)
if (ch == *(shunittype + newtype)[0])
break;
if ((newtype == NOUNITTYPES + 1) || (unitvalid(newtype) == FALSE)) {
if (ch == '1')
newtype = A_SPY;
else if (ch == '2')
newtype = A_SCOUT;
else {
errormsg("Invalid type");
if (isgod == TRUE)
reset_god();
return;
}
}
clear_bottom(0);
/* marines and sailors may only be drafted in harbors */
if (newtype == A_MARINES || newtype == A_SAILOR) {
i = FALSE;
for (x = XREAL - 1; x <= XREAL + 1; x++)
for (y = YREAL - 1; y <= YREAL + 1; y++)
if (sct[x][y].altitude == WATER)
i = TRUE;
/* not a harbor */
if (i == FALSE) {
if (newtype == A_MARINES)
errormsg("Huh? What would marines do without the water?");
else
errormsg("You gotta be kinding!? Sailors on land?");
if (isgod == TRUE)
reset_god();
return;
}
}
/* raise an untrained army */
i = FALSE;
if (newtype == A_SPY || newtype == A_SCOUT) {
men = 1;
} else {
mvprintw(LINES - 3, 0, "how many %s do you wish to raise:",
unittype[newtype]);
clrtoeol();
refresh();
men = get_number();
if (men <= 0) {
if (isgod == TRUE)
reset_god();
return;
}
}
/* i_people*256 is initial people -> can draft up to following */
/* draftable = max_draft - already drafted */
/* = imen/4 - ( imen - people) */
/* = -3/4 * imen + people) */
/* 192 comes from 3*256/4 */
if ((newtype != A_MERCENARY &&
(men > sct[XREAL][YREAL].people - (sct[XREAL][YREAL].i_people * 192))) ||
(sct[XREAL][YREAL].i_people < 0)) {
if (sct[XREAL][YREAL].i_people < 0)
errormsg("error: sector wasn't city at beginning of turn");
else
errormsg("error: raising too many soldiers");
if (isgod == TRUE)
reset_god();
return;
}
/* check that you dont have too many mercenaries */
mercs = 0;
if (newtype == A_MERCENARY) {
int totalsolds = 0;
for (armynum = 0; armynum < MAXARM; armynum++) {
if (P_ATYPE < MINLEADER) {
if (P_ATYPE == A_MERCENARY)
mercs += P_ASOLD;
totalsolds += P_ASOLD;
}
}
if (men + mercs > (totalsolds + men) / 2) {
errormsg("you would then have more than 50%% mercenaries");
if (isgod == TRUE)
reset_god();
return;
}
if (mercgot + men > MERCMEN / NTOTAL) {
errormsg("there are not that many mercanaries available");
if (isgod == TRUE)
reset_god();
return;
}
}
e_cost = (long) * (u_encost + newtype) * men;
i_cost = (long) * (u_enmetal + newtype) * men;
/* magiced get 1/2 enlistment costs */
if ((magic(country, WARRIOR) == TRUE) || (magic(country, WARLORD) == TRUE) ||
(magic(country, CAPTAIN) == TRUE))
e_cost /= 2;
if ((magic(country, SAPPER) == TRUE) &&
((newtype == A_SIEGE) || (newtype == A_CATAPULT))) {
e_cost /= 2;
i_cost /= 2;
}
/* check to see if enough gold */
if (e_cost > curntn->tgold) {
errormsg("You don't have enough talons");
if (isgod == TRUE)
reset_god();
return;
} else if (i_cost > curntn->metals) {
mvprintw(LINES - 1, 0, "You don't have %ld metal", i_cost);
mvaddstr(LINES - 1, COLS - 20, "PRESS ANY KEY");
clrtoeol();
refresh();
getch();
if (isgod == TRUE)
reset_god();
return;
} else {
move(LINES - 2, 0);
clrtoeol();
}
/* count is order of that army in sector */
/* armynum is number of that army */
if ((armynum = getselunit()) >= 0) {
if (armynum >= MAXARM || newtype == A_SPY || newtype == A_SCOUT) {
army = -1;
} else {
/* if different types, must raise new army */
if ((newtype == P_ATYPE) && (P_ASTAT != ONBOARD)) {
mvaddstr(LINES - 1, 0, "Do you wish to raise a new army:");
clrtoeol();
refresh();
if (getch() != 'y')
army = armynum;
else
army = -1;
} else
army = (-1);
}
}
if (army == (-1)) {
mvprintw(LINES - 2, 0,
"(%s, gold talons=%ld, metal=%ld) raising a new army",
*(unittype + newtype), e_cost, i_cost);
clrtoeol();
refresh();
sleep(1);
armynum = 0;
while ((army == (-1)) && (armynum < MAXARM)) {
if (P_ASOLD <= 0) {
army = armynum;
P_ASOLD = 0;
if (newtype == A_MILITIA)
P_ASTAT = MILITIA; /* new militia units=MILITIA */
else
P_ASTAT = DEFEND; /* set new armies to DEFEND */
AADJSTAT;
AADJMEN;
}
armynum++;
}
if (army == (-1)) {
errormsg("NO FREE ARMIES");
if (isgod == TRUE)
reset_god();
return;
}
armynum = army;
} else {
mvprintw(LINES - 2, 0,
"(%s, gold talons=%ld, metal=%ld) adding to existing army",
*(unittype + newtype), e_cost, i_cost);
clrtoeol();
refresh();
sleep(2);
}
if (newtype == A_SPY) {
while (TRUE) {
clear_bottom(0);
mvaddstr(LINES - 3, 0, "Spy Against What Nation: ");
refresh();
if ((i = get_country()) == (-1)) {
if (isgod == TRUE)
reset_god();
return;
}
if (i == country) {
errormsg("What? You don't even trust yourself?");
i = NTOTAL;
}
if (!(isntn(ntn[i].active))) {
errormsg("You can't spy against them");
i = NTOTAL;
}
if (i < NTOTAL && isactive(i))
break;
}
if (curntn->dstatus[i] != UNMET) {
P_AYLOC = ntn[i].capy;
P_AXLOC = ntn[i].capx;
mvprintw(LINES - 2, 0, "The Spy Starts in %s's Capitol (%d,%d)",
ntn[i].name, rel_x((int) P_AXLOC), rel_y((int) P_AYLOC));
clrtoeol();
} else {
clear_bottom(0);
mvprintw(LINES - 4, 0, "You do not yet know where %s is", ntn[i].name);
mvaddstr(LINES - 3, 0, "Have the Spy start from this sector? [y or n]");
refresh();
if (getch() != 'y') {
if (isgod == TRUE)
reset_god();
return;
}
P_AYLOC = YREAL;
P_AXLOC = XREAL;
mvprintw(LINES - 2, 0, "Intelligence indicates that %s lies to the %s",
ntn[i].name,
*(directions + compass((int) P_AXLOC, (int) P_AYLOC,
(int) ntn[i].capx, (int) ntn[i].capy)));
clrtoeol();
}
errormsg("");
redraw = PART;
} else {
P_AYLOC = YREAL;
P_AXLOC = XREAL;
}
if (newtype == A_SPY || newtype == A_SCOUT) {
P_ASTAT = SCOUT;
AADJSTAT;
}
P_ATYPE = newtype;
if (P_ATYPE != A_MERCENARY) {
sct[XREAL][YREAL].people -= men;
SADJCIV;
} else {
mercgot += men;
AADJMERC;
}
AADJLOC;
P_AMOVE = 0;
AADJMOV;
P_ASOLD += men;
AADJMEN;
if (P_ASTAT >= NUMSTATUS) {
P_ASTAT = ATTACK;
AADJSTAT;
}
curntn->metals -= i_cost;
curntn->tgold -= e_cost;
makemap(); /* if display 'y' is set, this will show new
* army */
if (isgod == TRUE)
reset_god();
}
/* map resolution: 5cm/grid */
void scan()
{
s08 rawX[64];
s08 rawY[64];
u08 i;
s16 localX, localY;
u08 x, y;
s08 minX, minY, maxX, maxY;
s08 posX, posY;
u16 tmp, tmp2, tmp3;
struct heading cmp;
/* zero servo */
set_servo_position(0,0);
/* make compass reading, apply calibration */
cmp = compass();
cmp.x -= compZero.x;
cmp.y -= compZero.y;
/* sweep sonar, step = 4 */
for(i=0; i<255; i+=4)
{
tmp = distance(0);
set_servo_position(0,i+4);
localX = (cmp.x * sine[63-i]) - (cmp.y * tmp);
localY = (cmp.y * sine[63-i]) - (cmp.x * tmp);
tmp2 = localX * localY;
tmp2 = sqrt(tmp2);
localX *= tmp; /* multiply before divide */
localY *= tmp; /* for more precision */
localX /= tmp2;
localY /= tmp2;
rawX[i>>2] /= 5; /* 5cm grid */
rawY[i>>2] /= 5;
delay_ms(9);
}
/* find min and max X and Y */
for(i=0; i<64; i++)
{
if(rawX[i] < minX)
minX = rawX[i];
if(rawX[i] > maxX)
maxX = rawX[i];
if(rawY[i] < minY)
minY = rawY[i];
if(rawY[i] > maxY)
maxY = rawY[i];
}
/* for each grid location */
tmp = 0; /* maximum observed error metric */
posX = 64; /* X of maximum observed error metric */
posY = 48; /* Y of maximum observed error metric */
for(x=0; x<128; x++)
{
for(y=0; y<96; y++)
{
/* compute error metric */
tmp2 = 0;
for(i=0; i<64; i++)
{
/* basic error metric */
if(rawX[i] > x);
}
}
}
}
void calCompass()
{
int xmin, xmax;
int ymin, ymax;
struct heading temp;
u08 i=0;
temp = compass();
xmin = temp.x;
xmax = temp.x;
ymin = temp.y;
ymax = temp.y;
clear_screen();
print_string("Calibrating");
next_line();
print_string("Compass");
set_motor_power(0,25);
set_motor_power(1,-25);
for(i=0; i<255; i++)
{
clear_screen();
print_string("Calibrating");
next_line();
print_string("Compass");
temp = compass();
if(xmin > temp.x)
xmin = temp.x;
else if(xmax < temp.x)
xmax = temp.x;
if(ymin > temp.y)
ymin = temp.y;
else if(ymax < temp.y)
ymax = temp.y;
delay_ms(10);
}
set_motor_power(0,0);
set_motor_power(1,0);
compZero.x = (xmin+xmax)>>1;
compZero.y = (ymin+ymax)>>1;
clear_screen();
print_string("x: ");
print_int( (xmin+xmax)>>1 );
next_line();
print_string("y: ");
print_int( (ymin+ymax)>>1 );
while(!get_sw1());
/* test calibration results
* x: -3, y: 5
* x: -4, y: 7
* x: -4, y: 6
* x: -5, y: 6
* x: -8, y: 8
*/
/* i = sine[i];
i = sine[i];*/
}
//******************************************************************************
void SHSensors::handleUpdate()
{
const float fToC = 9.0 / 5.0;
const float fOffset = 32;
//*** read all data ***
while ( imu_->IMURead() )
{
//*** get IMU data ***
RTIMU_DATA imuData = imu_->getIMUData();
//*** pressure / temperature ***
if ( ((enabled_ & IMU_PRESSURE) || (enabled_ & IMU_TEMP)) && pressure_ )
{
pressure_->pressureRead( imuData );
if ( enabled_ & IMU_PRESSURE )
{
//*** pressure in hPa ***
emit pressure( imuData.pressure, RTMath::convertPressureToHeight(imuData.pressure) );
}
if ( enabled_ & IMU_TEMP )
{
//*** temp celsius, fahrenheit ***
emit temperature( imuData.temperature,
imuData.temperature * fToC + fOffset );
}
}
//*** humidity ***
if ( (enabled_ & IMU_HUMIDITY) && humidity_ )
{
humidity_->humidityRead( imuData );
//*** relative humidity ***
emit humidity( imuData.humidity );
}
//*** gyroscope ***
if ( enabled_ & IMU_GYRO )
{
//*** gyroscope in degrees per second ***
emit gyro( imuData.gyro.x() * RTMATH_RAD_TO_DEGREE,
imuData.gyro.y() * RTMATH_RAD_TO_DEGREE,
imuData.gyro.z() * RTMATH_RAD_TO_DEGREE );
}
//*** accelerometer ***
if ( enabled_ & IMU_ACCEL )
{
//*** acceleration in g's ***'
emit accel( imuData.accel.x(), imuData.accel.y(),
imuData.accel.z(), imuData.accel.length() );
}
//*** compass ***
if ( enabled_ & IMU_COMPASS )
{
//*** compas (magnetometer) in uT ***
emit compass( imuData.compass.x(), imuData.compass.y(),
imuData.compass.z(), imuData.compass.length() );
}
//*** always emit fusion data - degrees ***
float fusionX = imuData.fusionPose.x() * RTMATH_RAD_TO_DEGREE;
float fusionY = imuData.fusionPose.y() * RTMATH_RAD_TO_DEGREE;
float fusionZ = imuData.fusionPose.z() * RTMATH_RAD_TO_DEGREE;
emit fusionPose( fusionX, fusionY, fusionZ );
}
}
void PIDDriver(short target){
float e, e_dot, old_e = threshold - SensorValue[Light], E = 0;
short u;
time1[T1] = 0;
time1[T2] = 0;
time1[T4] = 0;
float pre_compass = compass();
bool greyPatch = false;
while(true){
if(time1[T1] < timeout){ // T1 for timeout
e = threshold - SensorValue[Light];
e_dot = e - old_e;
E += e;
u = (short)(KP * e + KD * e_dot + KI * E);
old_e = e;
motor[Left] = cruise_speed - u;
motor[Right] = cruise_speed + u;
if(SensorValue[Light] < target){ //target can be make more conservative than threshold
time1[T1] = 0;
}
// When grey patch detected
if(SensorValue[Light] > 38 && SensorValue[Light] < 43){
if(time1[T4] > 300){
greyPatch = true;
stop();
// beep three times
PlayTone(1500, 40);
wait1Msec(800);
PlayTone(1500, 40);
wait1Msec(800);
PlayTone(1500, 40);
wait1Msec(800);
control(rotateSpeed, -rotateSpeed, 800); // 800 is for getting rid of the grey patch
while(SensorValue[Light] > threshold){}
stop();
time1[T1] = 0;
time1[T2] = 0;
time1[T3] = 0;
nMotorEncoder[Left]=0;
nMotorEncoder[Right]=0;
pre_compass = compass();
continue;
}
}
else{
time1[T4] = 0;
}
if(time1[T2] > sampleFreq && SensorValue[Light] < target){ //the light sensor should be on the tape when it beeps
float cur_compass = compass();
if(time1[T3] > beepInterval && ((cur_compass - pre_compass) > beepThreshold || (pre_compass - cur_compass) > beepThreshold)){
PlayTone(1175, 20);
time1[T3] = 0; // T3 for beepInterval
}
pre_compass = cur_compass;
time1[T2] = 0; // T2 for sampling frequency
if(greyPatch){ // log the compass value from grey patch to the other endpoint
float tmp = signedCompass();
nxtDisplayStringAt(0,31,"%f", tmp);
logCompass(tmp);
}
}
}
else{
if(greyPatch){
if(!turnOnSpot(target, greyPatch)) return; // reach the endpoint other than grey patch, return
}
else{
turnOnSpot(target, greyPatch);
}
}
}
}
