[currently viewing: /LegoNXT/NXT_robot.nxc]
* The second release version of
* 'A Trail Following Robot With Obstacle Navigation'
* Using NXC to program a Lego NXT Robot named B.E.R.N.I.E.
* By Group Delta Dawn: David, Kynan, Michael and Ross.
*/
/* This Version:
* Calibrates and outputs the light sensor readings to the screen.
* The robot can navigate along a straight line, and most corners.
* After a failed scan for the line, the robot attempts to jump
* a 5cm break in the line.
* This Versions Main Flaws:
* The straightness of the jump at the end of the line has to be optimised.
* This can be done by adjusting angle_increment, and or the angle_total.
* No obstacle navigation.
*/
#include "NXCDefs.h" /* Not eXactly C Definitions file */
#define FORWARD OnRev /* Main robot action definitions */
#define REVERSE OnFwd
#define SPEED01 25
#define SPEED02 40
#define SPEED03 50
#define SPEED04 70
#define TIME01 300
#define TIME02 400
#define TIME03 1000
#define SETTHRESHOLD 10
#define MUSWTIME 100 /* Music definitions */
#define MAXVOL 4
#define MINVOL 1
#define MIDVOL 2
int light_intensity_line = 0; /* Variable declarations */
int light_intensity_carpet = 0;
int calculated_threshold = 0;
int i_am_on_the_line = 0;
int random_first_scan_direction = 0;
int current_speed = 0;
int count = 0;
int angle_increment = 1; /* Angle variable declarations */
int angle_total = 600; /* Try: 500 - 1500 for angle_total */
int angle_half = 0;
int angle_quarter = 0;
int angle_curr = 0;
sub song01() {PlayFileEx("Woops.rso",MIDVOL,FALSE); Wait(2000);}
sub song02() {PlayFileEx("! Attention.rso",MIDVOL,FALSE); Wait(2000);}
sub song03() {PlayFileEx("! Click.rso",MIDVOL,FALSE); Wait(2000);}
sub song04() {PlayFileEx("! Startup.rso",MIDVOL,FALSE); Wait(2000);}
sub set_sensors() {
SetSensorLight(IN_1);
SetSensorTouch(IN_2);
SetSensorTouch(IN_3);
}
sub print_intro() {
ClearScreen();
GraphicOut(27,10,"faceclosed.ric"); Wait (500);
GraphicOut(27,10,"faceopen.ric"); Wait (500);
GraphicOut(27,10,"smile01.ric"); Wait (500);
GraphicOut(27,10,"smile02.ric"); Wait (500);
GraphicOut(27,10,"smile03.ric"); Wait (500);
ClearScreen();
TextOut(15,LCD_LINE2, "Hi i am a Lego"); Wait (100);
TextOut(15,LCD_LINE3, "NXT called"); Wait (100);
TextOut(15,LCD_LINE4, "B.E.R.N.I.E"); Wait (800);
ClearScreen();
TextOut(35,LCD_LINE2, "Best"); Wait (100);
TextOut(35,LCD_LINE3, "Ever"); Wait (100);
TextOut(35,LCD_LINE4, "Robot"); Wait (100);
TextOut(35,LCD_LINE5, "Name"); Wait (100);
TextOut(35,LCD_LINE6, "In"); Wait (100);
TextOut(35,LCD_LINE7, "Eons"); Wait (800);
ClearScreen();
GraphicOut(26,26,"calibration.ric"); Wait (1000);
}
sub print_calibrate_left() {
PlayToneEx(1175,400,MIDVOL,FALSE); Wait(200);
PlayToneEx(2349,400,MIDVOL,FALSE); Wait(400);
ClearScreen();
TextOut(20,LCD_LINE2, "Tap the");
TextOut(20,LCD_LINE3, "LEFT sensor");
TextOut(20,LCD_LINE4, "while ON");
TextOut(20,LCD_LINE5, "the line.");
}
sub print_calibrate_right() {
PlayToneEx(392,400,MIDVOL,FALSE); Wait(200);
PlayToneEx(349,400,MIDVOL,FALSE); Wait(200);
ClearScreen();
TextOut(20,LCD_LINE5, "Tap the");
TextOut(20,LCD_LINE6, "RIGHT sensor");
TextOut(20,LCD_LINE7, "while OFF");
TextOut(20,LCD_LINE8, "the line.");
}
sub print_light_reading_stats() {
ClearScreen();
TextOut(04,LCD_LINE2, "NXT B.E.R.N.I.E");
TextOut(05,LCD_LINE4, "Reading one: ");
NumOut(80,LCD_LINE4, light_intensity_line);
TextOut(05,LCD_LINE5, "Reading two: ");
NumOut(80,LCD_LINE5, light_intensity_carpet);
if (light_intensity_line == light_intensity_carpet) {
TextOut(21,LCD_LINE7, "Duplicate");
TextOut(18,LCD_LINE8, " value!");
}
}
sub print_i_am_on_the_line() {
TextOut(05,LCD_LINE6, "on the line: ");
NumOut(80,LCD_LINE6, i_am_on_the_line);
}
sub print_final_stats() {
ClearScreen();
TextOut(00,LCD_LINE2, "NXT Lego Robot");
TextOut(05,LCD_LINE4, "count: ");
NumOut(80,LCD_LINE4, count);
TextOut(05,LCD_LINE6, "-Game Over-");
}
sub calibrate() { //Calibration phase
Off(OUT_BC);
print_calibrate_left();
until (SENSOR_3 == 1); //Left sensor tap
light_intensity_line = SENSOR_1;
print_calibrate_right();
until (SENSOR_2 == 1); //Right sensor tap
light_intensity_carpet = SENSOR_1;
print_light_reading_stats();
if (light_intensity_line == light_intensity_carpet) {
song01(); //Whoops error
} else {
repeat(3) { PlayToneEx(440,400,MIDVOL,FALSE); Wait(200); //Three tones
PlayToneEx(494,400,MIDVOL,FALSE); Wait(200); } //Three tones
}
}
sub check_light_sensor() {
if (SENSOR_1 >= (light_intensity_line - 10) &&
SENSOR_1 <= (light_intensity_line + 10)) {
i_am_on_the_line = 1;
} else {
i_am_on_the_line = 0;
}
}
sub when_on_the_line() {
while (i_am_on_the_line == 1) {
FORWARD(OUT_BC, SPEED02);
check_light_sensor();
}
Off(OUT_BC);
}
/*---Start-Turns--------------------------------------------------------------*/
sub rotate_left01() {
FORWARD(OUT_C, SPEED02);
REVERSE(OUT_B, SPEED02);
}
sub rotate_right01() {
FORWARD(OUT_B, SPEED02);
REVERSE(OUT_C, SPEED02);
}
sub rotate_left02(int time_to_wait) {
do {
FORWARD(OUT_C, SPEED02);
REVERSE(OUT_B, SPEED02);
Wait(time_to_wait);
check_light_sensor();
check_light_sensor();
if (i_am_on_the_line == 1) {
Float(OUT_BC);
}
} while (i_am_on_the_line == 0);
}
sub rotate_right02(int time_to_wait) {
do {
FORWARD(OUT_B, SPEED02);
REVERSE(OUT_C, SPEED02);
Wait(time_to_wait);
check_light_sensor();
if (i_am_on_the_line == 1) {
Float(OUT_BC);
}
} while (i_am_on_the_line == 0);
}
sub rotate_left03(int time_to_wait) {
FORWARD(OUT_C, SPEED02);
REVERSE(OUT_B, SPEED02);
Wait(time_to_wait);
until(i_am_on_the_line == 1);
//Wait(time_to_wait);
Float(OUT_BC);
}
sub rotate_right03(int time_to_wait) {
FORWARD(OUT_B, SPEED02);
REVERSE(OUT_C, SPEED02);
Wait(time_to_wait);
until(i_am_on_the_line == 1);
//Wait(time_to_wait);
Float(OUT_BC);
}
/*-End-Turns------------------------------------------------------------------*/
sub scan_for_the_line() {
print_i_am_on_the_line();
angle_half = angle_total/2;
while (angle_half <= angle_total && i_am_on_the_line == 0) {
angle_quarter = angle_half/2;
angle_curr = 0;
while (angle_curr < angle_quarter && i_am_on_the_line == 0) {
rotate_left01();
angle_curr += angle_increment;
check_light_sensor();
}
Off(OUT_BC);
angle_curr = 0;
while (angle_curr < 2 * angle_quarter && i_am_on_the_line == 0) {
rotate_right01();
angle_curr += angle_increment;
check_light_sensor();
}
Off(OUT_BC);
angle_curr = 0;
while (angle_curr < angle_quarter && i_am_on_the_line == 0) {
rotate_left01();
angle_curr += angle_increment;
check_light_sensor();
}
Off(OUT_BC);
angle_half = (angle_half * 2);
}
Off(OUT_BC);
FORWARD(OUT_C, SPEED02); Wait(200); /* Try: 200 */
while (i_am_on_the_line == 0 && angle_half >= angle_total) {
FORWARD(OUT_BC, SPEED03); Wait(100); /* Try: 100 */
check_light_sensor();
}
}
task main() {
set_sensors();
print_intro();
calibrate(); /* Robot is placed on the line, */
until (SENSOR_3 == 1); /* with a left sensor tap to begin. */
while (SENSOR_2 == 0) {
while (true) {
check_light_sensor();
when_on_the_line();
scan_for_the_line();
}
REVERSE(OUT_BC, SPEED03);
}
song04(); Wait(300); /* End of program */
FORWARD(OUT_BC, SPEED03); Wait(1500); /* Skitzo */
Float(OUT_BC);
REVERSE(OUT_C, SPEED03); Wait(1000);
Float(OUT_BC);
FORWARD(OUT_BC, SPEED04); Wait(2500);
Float(OUT_BC);
REVERSE(OUT_B, SPEED04); Wait(1000);
Float(OUT_ABC);
print_final_stats(); Wait(15000);
}
