task main () {
long longitude = 0;
long latitude = 0;
long utc = 0;
bool linkstatus = false;
nxtDisplayCenteredTextLine(0, "Dexter Ind.");
nxtDisplayCenteredBigTextLine(1, "GPS");
nxtDisplayCenteredTextLine(3, "Test 1");
nxtDisplayCenteredTextLine(5, "Connect sensor");
nxtDisplayCenteredTextLine(6, "to S1");
wait1Msec(2000);
eraseDisplay();
while (true) {
// Read the sensor's data
utc = DGPSreadUTC(DGPS);
longitude = DGPSreadLongitude(DGPS);
latitude = DGPSreadLatitude(DGPS);
linkstatus = DGPSreadStatus(DGPS);
nxtDisplayCenteredTextLine(0, "DGPS Test 1");
nxtDrawLine(0, 52, 99, 52);
nxtDisplayTextLine(2, "UTC: %d", utc);
nxtDisplayTextLine(3, "Lon: %d", longitude);
nxtDisplayTextLine(4, "Lat: %d", latitude);
if (linkstatus)
nxtDisplayTextLine(7, "Link Stat: [UP]");
else
nxtDisplayTextLine(7, "Link Stat: [DOWN]");
nxtDrawLine(0, 20, 99, 20);
wait1Msec(500);
}
}
// Draw a little stick figure
void drawStickFigure(int offset)
{
nxtEraseRect(0, 0, 99, 50);
nxtDrawCircle(offset + 43, 40, 15);
nxtDrawLine(offset + 50,25,offset + 50,10);
nxtDrawLine(offset + 43, 22, offset + 58, 22);
nxtDrawLine(offset + 43, 0, offset + 50, 10);
nxtDrawLine(offset + 50, 10, offset + 58, 0);
}
void doDrawPoint(int top, int x_pos, int y_pos) {
nxtEraseRect(x_pos, 63, x_pos+5, 0);
if (displayUnit == showcurrent)
nxtDisplayCenteredTextLine(7, "Max: %d mA", top);
else if (displayUnit == showvoltage)
nxtDisplayCenteredTextLine(7, "Max: %d mV", top);
nxtDisplayStringAt(0, 12, "0");
nxtDrawLine(10, 10, 10, 60);
nxtDrawLine(10, 10, 15, 10);
nxtDrawLine(10, 35, 15, 35);
nxtDrawLine(10, 60, 15, 60);
nxtSetPixel(x_pos, y_pos);
}
//Retrieve data from the sound sensor
void i2c_read_registers_text(ubyte register_2_read, int message_size, int return_size)
{
memset(I2Creply, 0, sizeof(I2Creply));
message_size = message_size+3;
I2Cmessage[0] = message_size; // Messsage Size
I2Cmessage[1] = ARDUINO_ADDRESS;
I2Cmessage[2] = register_2_read; // Register
sendI2CMsg(S1, &I2Cmessage[0], return_size);
wait1Msec(20);
readI2CReply(ARDUINO_PORT, &I2Creply[0], return_size);
int i = 0,top=0;
//Pitch
if(cmd==1)
writeDebugStreamLine("%d", (int)I2Creply[0]+(int)I2Creply[1]*256);
//Sound Power Level
else if(cmd==2)
//For detecting clap
//if((int)I2Creply>85)
writeDebugStreamLine("%i", (int)I2Creply[0]);
//FFT
else if(cmd==3)
{
for(int x = 0; x <return_size; x++)
{
writeDebugStream("%i", I2Creply[x]);
writeDebugStream(" ");
}
writeDebugStreamLine(" ");
//Graphic Equalizer
nxtEraseRect(6,62,99,6);
nxtDrawLine(5,2,5,61);
nxtDrawLine(2,5,95,5);
for(int j=0;j<return_size;j++)
{
top=I2Creply[j]+5;
if(top>61)
top=61;
nxtDrawRect(5+6*j,top,5+6*(j+1),5);
}
}
}
void drawMap()
{
// Display the map
for (int j = 0; j < NUMBER_OF_WALLS; j++) {
nxtDrawLine((int)(wallAxArray[j]/DISPLAY_SCALE), (int)(wallAyArray[j]/DISPLAY_SCALE),
(int)(wallBxArray[j]/DISPLAY_SCALE), (int)(wallByArray[j]/DISPLAY_SCALE));
}
}
/**
* This displays an arrow on the screen pointing downwards.
* @param degreesFromDown the number of degrees from down
*/
void displayArrow(int degreesFromDown)
{
eraseDisplay();
// Otherwise, the arrow would point up.
degreesFromDown = degreesFromDown-180;
//If you don't know trigonometry, you can ignore this part
nxtDrawLine(49,
31,
(cosDegrees(degreesFromDown ) * 20) + 49,
(sinDegrees(degreesFromDown ) * 20) + 31);
nxtDrawLine((cosDegrees(degreesFromDown - 20) * 15) + 49,
(sinDegrees(degreesFromDown - 20) * 15) + 31,
(cosDegrees(degreesFromDown ) * 20) + 49,
(sinDegrees(degreesFromDown ) * 20) + 31);
nxtDrawLine((cosDegrees(degreesFromDown + 20) * 15) + 49,
(sinDegrees(degreesFromDown + 20) * 15) + 31,
(cosDegrees(degreesFromDown ) * 20) + 49,
(sinDegrees(degreesFromDown ) * 20) + 31);
}
/*
Main task
*/
task main () {
blob_array _blobs;
memset(_blobs, 0, sizeof(blob_array));
// combine all colliding blobs into one
bool _condensed = true;
//blob_array _blobs;
int _l, _t, _r, _b;
int _x, _y;
int _nblobs;
eraseDisplay();
// Initialise the camera
NXTCAMinit(cam);
while(true) {
eraseDisplay();
// Fetch all the blobs, have the driver combine all
// the colliding blobs.
_nblobs = NXTCAMgetBlobs(cam, _blobs, _condensed);
for (int i = 0; i < _nblobs; i++) {
// Draw the scaled blobs
_l = xscale(_blobs[i].x1);
_t = yscale(_blobs[i].y1);
_r = xscale(_blobs[i].x2);
_b = yscale(_blobs[i].y2);
nxtDrawRect(_l, _t, _r, _b);
}
NXTCAMgetAverageCenter(_blobs, 3, 0, _x, _y);
_x = xscale(_x);
_y = yscale(_y);
nxtEraseRect(_x-4, _y-4, _x+4, _y+4);
nxtDrawLine(_x, _y+2, _x, _y-2);
nxtDrawLine(_x+2, _y, _x-2, _y);
nxtDisplayTextLine(1, "%d", _nblobs);
wait1Msec(100);
}
}
task main () {
int pressmInHg = 0;
float presshPa = 0.0;
float pressPsi = 0.0;
float tempC = 0.0;
float tempF = 0.0;
nxtDisplayCenteredTextLine(0, "HiTechnic");
nxtDisplayCenteredBigTextLine(1, "BaroTemp");
nxtDisplayCenteredTextLine(3, "Test 1");
nxtDisplayCenteredTextLine(5, "Connect sensor");
nxtDisplayCenteredTextLine(6, "to S1");
wait1Msec(2000);
//PlaySound(soundBeepBeep);
//while(bSoundActive);
while (true) {
eraseDisplay();
// Read the sensor's data
pressmInHg = HTBMreadMInHg(HTBM); // Pressure in 1/1000th of an inch Hg
presshPa = HTBMreadhPa(HTBM); // Pressure in hecto Pascal
pressPsi = HTBMreadPsi(HTBM); // Pressure in Pounds per square inch
tempC = HTBMreadTemp(HTBM); // Temp in Celcius
tempF = HTBMreadTempF(HTBM); // Temp in Fahrenheit
nxtDisplayCenteredTextLine(0, "HTBM Test 1");
nxtDisplayTextLine(2, "%5d mInHg", pressmInHg);
nxtDisplayTextLine(3, "%5.1f hPa", presshPa);
nxtDisplayTextLine(4, "%5.1f PSI", pressPsi);
nxtDisplayTextLine(6, "%3.1f C / %3.1f F", tempC, tempF);
nxtDrawLine(0, 52, 99, 52);
nxtDrawLine(0, 20, 99, 20);
wait1Msec(100);
}
}
task updateHUD () {
int x = 0;
int y = 0;
while (true) {
nxtEraseRect(4,50, 44,10);
nxtDisplayTextLine(2, " H: %3d", angleI/100);
nxtDisplayTextLine(3, " X: %3d", x_accel/100);
nxtDisplayTextLine(4, " Y: %3d", y_accel/100);
nxtDisplayTextLine(5, " Z: %3d", z_accel/100);
nxtDrawCircle(84, 50, 4);
nxtDrawCircle(4, 50, 40);
x = (cosDegrees(-1 * (angleI/100 - 90)) * 20) + 24;
y = (sinDegrees(-1 * (angleI/100 - 90)) * 20) + 30;
nxtDrawLine(24, 30, x, y);
nxtEraseRect(0,0, 99, 8);
nxtDrawRect(0,0, 99, 8);
nxtFillRect(50,0, (float)(rotI / 150)/100.0 *50 + 50, 8);
wait1Msec(100);
}
}
/*
Main task
*/
task main () {
blob_array _blobs;
memset(_blobs, 0, sizeof(blob_array));
//blob_array _blobs;
int _x, _y, _x1, _y1;
int _nblobs;
eraseDisplay();
// pid variables
float pwr_x = 0.0;
float err_x = 0.0;
float perr_x = 0.0;
float aerr_x = 0.0;
float sp_x = 0.0;
float p_x = 0.0;
float i_x = 0.0;
float d_x = 0.0;
float pwr_y = 0.0;
float err_y = 0.0;
float perr_y = 0.0;
float aerr_y = 0.0;
float sp_y = 0.0;
float p_y = 0.0;
float i_y = 0.0;
float d_y = 0.0;
sp_y = sp_x = 87;
p_y = p_x = 0.5;
d_y = d_x = 1.8;
i_y = i_x = 0.05;
nMotorEncoder[motorA] = 0;
nMotorEncoder[motorB] = 0;
// Initialise the camera
NXTCAMinit(cam);
while(true) {
eraseDisplay();
// Fetch all the blobs, have the driver combine all
// the colliding blobs.
_nblobs = NXTCAMgetBlobs(cam, _blobs);
nxtDisplayTextLine(1, "%d, %f", _nblobs, perr_x);
if (_nblobs == 0) {
if ((perr_x > 0) && (nMotorEncoder[motorB] > 660)) {
motor[motorA] = 0;
motor[motorB] = 0;
} else if ((perr_x < 0) && (nMotorEncoder[motorB] < -660)) {
motor[motorA] = 0;
motor[motorB] = 0;
}
} else {
_x = (_blobs[0].x2 + _blobs[0].x1)/2;
_y = (_blobs[0].y2 + _blobs[0].y1)/2;
err_x = _x - sp_x;
aerr_x += err_x;
pwr_x = (err_x * p_x) + ((err_x - perr_x) * d_x) + (aerr_x * i_x);
err_y = _y - sp_y;
aerr_y += err_y;
pwr_y = (err_y * p_y) + ((err_y - perr_y) * d_y) + (aerr_y * i_y);
motor[motorA] = round(pwr_y);
motor[motorB] = round(pwr_x);
_x1 = xscale(_x);
_y1 = yscale(_y);
nxtEraseRect(_x1-6, _y1-6, _x1+6, _y1+6);
nxtDrawLine(_x1, _y1+3, _x1, _y1-3);
nxtDrawLine(_x1+3, _y1, _x1-3, _y1);
perr_y = err_y;
perr_x = err_x;
}
wait1Msec(50);
}
}
