task MapRoom()
{
float botLastXCoordinate = 0.0;
float botLastYCoordinate = 0.0;
float botCurrentXCoordinate = 0.0;
float botCurrentYCoordinate = 0.0;
int wallXCoordinate = 0;
int wallYCoordinate = 0;
int xCoordinateDisplayOffset = 50;
int yCoordinateDisplayOffset = 40;
startGyro();
resetGyro();
eraseDisplay();
while(true)
{
//Robot position
botCurrentXCoordinate = botLastXCoordinate + EncoderDistance(ReadEncoderValue()) * sinDegrees(readGyro());
botCurrentYCoordinate = botLastYCoordinate + EncoderDistance(ReadEncoderValue()) * cosDegrees(readGyro());
//Wall mapping
wallXCoordinate = botCurrentXCoordinate + ReadSonar(2) * CENTIMETERS_TO_INCHES * cosDegrees(readGyro());
wallYCoordinate = botCurrentYCoordinate + ReadSonar(2) * CENTIMETERS_TO_INCHES * sinDegrees(readGyro());
nxtSetPixel(wallXCoordinate + xCoordinateDisplayOffset, wallYCoordinate + yCoordinateDisplayOffset);
ResetEncoderValue();
botLastXCoordinate = botCurrentXCoordinate;
botLastYCoordinate = botCurrentYCoordinate;
wait1Msec(20);
}
}
void DrawParticles()
{
for (int i=0; i < NUM_PARTICLES; i++)
{
nxtSetPixel((int)(99 - ((particles[i][1]+xoff)/DISPLAY_SCALE)), (int)(63 - ((particles[i][0]+yoff)/DISPLAY_SCALE)));
}
}
task main()
{
eraseDisplay();
// Array for accumulating Gaussian values
int accumulator[100];
for (int i = 0; i < 99; i++) {
accumulator[i] = 0;
}
// Infinite loop
while(1)
{
// Get and print uniform-distributed and Gaussian-distributed random numbers
float uniform_float = sampleUniform(1.0);
float gaussian_float = sampleGaussian(0.0, 1.0);
nxtDisplayString(1, "Uniform : %04f", uniform_float);
nxtDisplayString(2, "Gaussian: %04f", gaussian_float);
// Build a histogram of Gaussian numbers and plot on the NXT screen
// Scale random number to screen resolution
int gaussian_int = (int) (gaussian_float * 10.0) + 50;
accumulator[gaussian_int]++;
nxtSetPixel(gaussian_int, accumulator[gaussian_int]);
wait1Msec(10);
}
}
void drawParticles()
{
// Draw the particle set
for (int i = 0; i < NUMBER_OF_PARTICLES; i++) {
nxtSetPixel((int)(xArray[i]/DISPLAY_SCALE), (int)(yArray[i]/DISPLAY_SCALE));
}
}
void Backward40cm(){
motor[rightWheel] = ForwardSpeed*-1;
motor[leftWheel] = ForwardSpeed*-1;
int i;
for ( i = 0 ; i < StraightTime ; i+=timeUnit) {
nxtSetPixel( x , y );
x -= cosDegrees(angle);
y -= sinDegrees(angle);
//wait1Msec(timeUnit);
}
}
void Forward40cm(){
motor[rightWheel] = ForwardSpeed;
motor[leftWheel] = ForwardSpeed;
int i;
for (i = 0; i < StraightTime; i += timeUnit) {
nxtSetPixel( x , y );
x += cosDegrees(angle);
y += sinDegrees(angle);
wait1Msec(timeUnit);
}
}
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);
}
task main () {
nxtDisplayCenteredTextLine(0, "MindSensors");
nxtDisplayCenteredBigTextLine(1, "TouchPnl");
nxtDisplayCenteredTextLine(3, "Noise-a-Tron");
nxtDisplayCenteredTextLine(5, "Connect sensor");
nxtDisplayCenteredTextLine(6, "to S3");
wait1Msec(2000);
eraseDisplay();
int x, y = 0;
ubyte buttons = 0;
int multiplierX = 10;
int multiplierY = 10;
while (true) {
if (!MSTPgetTouch(MSTP, x, y, buttons))
PlaySound(soundBlip);
else if (isButtonTouched(buttons, BUTTON_L1))
multiplierX = 10;
else if (isButtonTouched(buttons, BUTTON_L2))
multiplierX = 15;
else if (isButtonTouched(buttons, BUTTON_L3))
multiplierX = 20;
else if (isButtonTouched(buttons, BUTTON_L4))
multiplierX = 25;
else if (isButtonTouched(buttons, BUTTON_R1))
multiplierY = 10;
else if (isButtonTouched(buttons, BUTTON_R2))
multiplierY = 15;
else if (isButtonTouched(buttons, BUTTON_R3))
multiplierY = 20;
else if (isButtonTouched(buttons, BUTTON_R4))
multiplierY = 25;
else if (x > 0 && y > 0)
{
PlayImmediateTone(x * multiplierX, 1);
wait1Msec(10);
PlayImmediateTone(y * multiplierY, 1);
nxtSetPixel(x, y);
}
EndTimeSlice();
}
}
/*****************************************
* Function that draws a grid on the LCD
* for easier readout of whatever is plot
*****************************************/
void draw_grid()
{
for(int i = 0; i < 65; i++)
{
nxtSetPixel(50, i);
int grid5 = (i - 32) % 5;
int grid10 = (i - 32) % 10;
if(!grid5 && grid10)
{
for(int j = -2; j < 3; j++)
{
nxtSetPixel(50 + j, i);
}
}
else if(!grid10)
{
for(int j = -4; j < 5; j++)
{
nxtSetPixel(50 + j, i);
}
}
}
for(int i = 0; i < 101; i++)
{
nxtSetPixel(i, 32);
int grid5 = (i - 100) % 5;
int grid10 = (i - 100) % 10;
if(!grid5 && grid10)
{
for(int j = -2; j < 3; j++)
{
nxtSetPixel(i, 32 + j);
}
}
else if(!grid10)
{
for(int j = -4; j < 5; j++)
{
nxtSetPixel(i, 32 + j);
}
}
}
}
void drawPosition()
{
nxtSetPixel((x )/scale +10,(y )/scale+10 );
}
void dead_reckoning()
{
float cur_time = nPgmTime;
if(first_time){
prev_time = cur_time;
first_time = 0;
wait1Msec(velocityUpdateInterval);
return;
}
//
//Fill in code for numerical integration / position estimation here
//
float dt = 1000*(cur_time-prev_time);
/* Numerical Integration */
float cur_r = nMotorEncoder[motorB];
float cur_l = nMotorEncoder[motorC];
//writeDebugStreamLine("cur_r = %f, cur_l = %f", cur_r, cur_l);
vl = (cur_l - previous_tick_l)/dt*(PI/180);
vr = (cur_r - previous_tick_r)/dt*(PI/180);
//Left_Wheel_Velocity = vl;
//Right_Wheel_Velocity = vr;
//writeDebugStreamLine("previous_tick_r = %f, previous_tick_l = %f", previous_tick_r, previous_tick_l);
float Vleft = vl*Radius;
float Vright = vr*Radius;
float v = (Vleft+Vright)/2;
float w = (Vright-Vleft)/L;
float k00 = v*cos(robot_TH);
float k01 = v*sin(robot_TH);
float k02 = w;
float k10 = v*cos(robot_TH+(dt/2)*k02);
float k11 = v*sin(robot_TH+(dt/2)*k02);
float k12 = w;
float k20 = v*cos(robot_TH+(dt/2)*k12);
float k21 = v*sin(robot_TH+(dt/2)*k12);
float k22 = w;
float k30 = v*cos(robot_TH+dt*k22);
float k31 = v*sin(robot_TH+dt*k22);
float k32 = w;
robot_X = robot_X + (dt/6)*(k00+2*(k10+k20)+k30);
robot_Y = robot_Y + (dt/6)*(k01+2*(k11+k21)+k31);
robot_TH = robot_TH + (dt/6)*(k02+2*(k12+k22)+k32);
robot_TH = modulus(robot_TH, 2*PI);
//Code that plots the robot's current position and also prints it out as text
nxtSetPixel(50 + (int)(100.0 * robot_X), 32 + (int)(100.0 * robot_Y));
nxtDisplayTextLine(0, "X: %f", robot_X);
nxtDisplayTextLine(1, "Y: %f", robot_Y);
nxtDisplayTextLine(2, "t: %f", 57.2958 * robot_TH);
prev_time = cur_time;
previous_tick_l = cur_l;
previous_tick_r = cur_r;
pen_X = cos(robot_TH)*LX - sin(robot_TH)*LY + robot_X;
pen_Y = sin(robot_TH)*LX + cos(robot_TH)*LY + robot_Y;
/* float xp, yp;
xp = cosDegrees(57.2958*robot_TH)*lx - sinDegrees(57.2958*robot_TH)*ly;
yp = sinDegrees(57.2958*robot_TH)*lx + cosDegrees(57.2958*robot_TH)*ly;
nxtDisplayTextLine(3, "xp: %f", xp);
nxtDisplayTextLine(4, "yp: %f", yp);*/
wait1Msec(velocityUpdateInterval);
}
void drawPosition(float x, float y)
{
// Draw new position
nxtSetPixel((int) (encoderToCm(x)/DISPLAY_SCALE) + OFFSET, (int) (encoderToCm(y)/DISPLAY_SCALE) + OFFSET);
}
void drawPosition(float x, float y)
{
// Draw new position
nxtSetPixel((int) (x/DISPLAY_SCALE) + OFFSET, (int) (y/DISPLAY_SCALE) + OFFSET);
}
/*
* Draws pixels to screen. Note screen coords are flipped so that start orientation appears same as robot's
* start orientation.
*/
void Draw()
{
nxtSetPixel((int)(99 - ((y+xoff)/DISPLAY_SCALE)), (int)(63 - ((x+yoff)/DISPLAY_SCALE)));
}
void drawRect(char xCo, char yCo, char percent)
{
char startX = xCo * 6 + 2;
char startY = yCo * 6 + 2;
if(percent == 100)
{
nxtFillRect(startX, startY, startX + 5, startY + 5);
}
else if(percent == 85)
{
int count = 0;
char yOffset = 0;
char xOffset = 0;
while(count<6)
{
char x;
for(x = 0; x<6; x++)
{
if(eightyFivePercent[x+xOffset]==1)
{
nxtSetPixel(startX+x, startY + yOffset);
}
}
xOffset += 6;
yOffset+=1;
count+=1;
}
}
else if(percent == 75)
{
int count = 0;
char yOffset = 0;
char xOffset = 0;
while(count<6)
{
char x;
for(x = 0; x<6; x++)
{
if(seventyFivePercent[x+xOffset]==1)
{
nxtSetPixel(startX+x, startY + yOffset);
}
}
xOffset += 6;
yOffset+=1;
count+=1;
}
}
else if(percent == 67)
{
int count = 0;
char yOffset = 0;
char xOffset = 0;
while(count<6)
{
char x;
for(x = 0; x<6; x++)
{
if(sixtySevenPercent[x+xOffset]==1)
{
nxtSetPixel(startX+x, startY + yOffset);
}
}
xOffset += 6;
yOffset+=1;
count+=1;
}
}
else if(percent == 50)
{
int count = 0;
char yOffset = 0;
char xOffset = 0;
while(count<6)
{
char x;
for(x = 0; x<6; x++)
{
if(fiftyPercent[x+xOffset]==1)
{
nxtSetPixel(startX+x, startY + yOffset);
}
}
xOffset += 6;
yOffset+=1;
count+=1;
}
}
else if(percent == 33)
{
int count = 0;
char yOffset = 0;
char xOffset = 0;
while(count<6)
{
char x;
for(x = 0; x<6; x++)
{
if(thirtyThreePercent[x+xOffset]==1)
{
nxtSetPixel(startX+x, startY + yOffset);
}
}
xOffset += 6;
yOffset+=1;
count+=1;
}
}
else if(percent == 25)
{
int count = 0;
char yOffset = 0;
char xOffset = 0;
while(count<6)
{
char x;
for(x = 0; x<6; x++)
{
if(twentyfivePercent[x+xOffset]==1)
{
nxtSetPixel(startX+x, startY + yOffset);
}
}
xOffset += 6;
yOffset+=1;
count+=1;
}
}
else if(percent == 15)
{
int count = 0;
char yOffset = 0;
char xOffset = 0;
while(count<6)
{
char x;
for(x = 0; x<6; x++)
{
if(fifteenPercent[x+xOffset]==1)
{
nxtSetPixel(startX+x, startY + yOffset);
}
}
xOffset += 6;
yOffset+=1;
count+=1;
}
}
else if(percent == 2)
{
int count = 0;
char yOffset = 0;
char xOffset = 0;
while(count<6)
{
char x;
for(x = 0; x<6; x++)
{
if(twoPercent[x+xOffset]==1)
{
nxtSetPixel(startX+x, startY + yOffset);
}
}
xOffset += 6;
yOffset+=1;
count+=1;
}
}
}
/* Start task functions */
task vehicle_draw_position() {
while (true) {
wait1Msec(10);
nxtSetPixel(20 + (int)(position.x / DISPLAY_SCALE), PRINT_OFFSET_Y + 20 + (int)(position.y / DISPLAY_SCALE));
}
}
