| |
In lugnet.org.ca.rtltoronto, Calum Tsang wrote:
<snip>
> BTW I deliberately did not build such a robot because I knew I wouldn't know how
> to write the code to "process" the sensors and movement. That's why I'm super
> impressed with Dave's so far.
<snip>
Seriously, my code for scanning is as simple as it gets--
NQC'd--
task scanner_task()
{
while(true)
{
if (found_light == 1) //found light source--keep scanner on light
source
{
<snipped extraneous code>
{
if((SENSOR_1 > (SENSOR_2 + 10)))
{
scanner_right;
}
// so if the right sensor has a higher value than the left sensor (+ 10 to
accomodate any small variances), turn the scanner to the right
else if((SENSOR_2 > (SENSOR_1 + 10)))
{
scanner_left;
}
// so if the left sensor has a higher value than the right sensor (+ 10 to
accomodate any small variances), turn the scanner to the left
else
{
scanner_off;
}
//so the sensor should be pretty much looking at hte light right now.
}
}
then for the actual chassis movement-- it looks complicated 'cause I added
different turning depending on how far the 'bot is from facing the light source.
Actually, take that part out and there's just two if statements--turn the 'bot
right if the light source is to the right, and turn the 'bot left if the light
source is to the left. Drive straight if the light is right ahead.
{
if ((SENSOR_3 > (center_scan_number + 2)) && (SENSOR_3 <= (center_scan_number + (center_scan_number / 2)))) //no bumpers pressed--keep light source centered between scanners
{
drive_right;
}
// k, this is a complicated way of saying that if the rotation sensor (SENSOR 3)
value is to the right of center (+2, again for the variances--don't need to be
too accurate) but less than the 45 degree mark (for reasons explained later),
turn the 'bot right
else if (SENSOR_3 > (center_scan_number + (center_scan_number / 2))) //no bumpers pressed--keep light source centered between scanners
{
drive_right_spin;
}
// But if the 'bot is more than 45 degrees from pointing to the brightest light
source, spin the 'bot instead of turining--I found that the turning radius of
the 'bot is too big when it's pointing perpendicualr to the light source
else if ((SENSOR_3 < (center_scan_number - 2)) && (SENSOR_3 >=
(center_scan_number - (center_scan_number / 2)))) //no bumpers pressed--keep
light source centered between scanners
{
drive_left;
}
//same as above, except turn left
else if (SENSOR_3 < (center_scan_number - (center_scan_number /
2))) //no bumpers pressed--keep light source centered between scanners
{
drive_left_spin;
}
//ditto above
else
{
drive_forward;
//move towards light
}
//and as my rem statement says, if the rotation value is centered, then, well,
drive straight
}
}
those were the easy bits--I took the scanning and the driving and made them
different tasks--they run completely independantly.
The reason--the scanner can target the light and keep the light in target no
matter which way the 'bot is facing as long as it's within the front 180 degrees
of the 'bot.
And the 'bot chassis can move around at will and not affect the scanner, and
when the scanner locks onto a light, the 'bot can turn into it.
As the video shows, quite nice.
Adding the bumpers and such--there's the level of complication. However, my
code is broken into 5 distinct parts right now--
1 setup
2 scanning routine
3 driving routine
4 bumper routine
5 light routine
the entire code (not fully tested) it gets a little bigger as the testing
process continues--
//rtl 20 robot 1
//motion definitions
#define scanner_left SetPower(OUT_B,7);OnRev(OUT_B);
#define scanner_right SetPower(OUT_B,7);OnFwd(OUT_B);
#define scanner_off Off(OUT_B);
#define drive_forward SetPower(OUT_A+OUT_C,7);OnFwd(OUT_A+OUT_C);
#define drive_reverse SetPower(OUT_A+OUT_C,7);OnRev(OUT_A+OUT_C);
#define drive_left SetPower(OUT_C,7);OnFwd(OUT_C);Off(OUT_A)
#define drive_right SetPower(OUT_A,7);OnFwd(OUT_A);Off(OUT_C)
#define drive_reverse_left SetPower(OUT_C,7);OnRev(OUT_C);Off(OUT_A)
#define drive_reverse_right SetPower(OUT_A,7);OnRev(OUT_A);Off(OUT_C)
#define drive_left_spin SetPower(OUT_A+OUT_C,7);OnFwd(OUT_C);OnRev(OUT_A)
#define drive_right_spin SetPower(OUT_A+OUT_C,7);OnFwd(OUT_A);OnRev(OUT_C)
#define drive_off Off(OUT_A+OUT_C)
//declare variables
int ambient_light;
int temp_light;
int scan_direction;
int center_scan_number;
int full_scan_number;
int touch_sensor_value;
int bumpers_pressed;
int found_light;
int drive_time;
int temp_value;
task main()
{
//set up initial values
SetSensor(SENSOR_1,SENSOR_LIGHT); SetSensorMode(SENSOR_1,SENSOR_MODE_RAW);
SetSensor(SENSOR_2,SENSOR_LIGHT); SetSensorMode(SENSOR_2,SENSOR_MODE_RAW);
SetSensor(SENSOR_3,SENSOR_ROTATION);
SetSensorMode(SENSOR_3,SENSOR_MODE_ROTATION);
ClearTimer(0);
ambient_light=1024;
scan_direction=1;
full_scan_number=0;
found_light=0;
//0 = not found 1 = found
touch_sensor_value=450;
//touch sensor pressed when sensor values less than this number
bumpers_pressed=0;
drive_time=50;
start setup_task;
while (true)
{
}
}
task setup_task()
{
Wait(100); //for doors to open
drive_forward;Wait(50); // get out of shell
drive_off;
do
{
temp_value = (SENSOR_3);
scanner_right;
Wait(2);
scanner_off;
temp_light = (SENSOR_1);
if (ambient_light > temp_light)
{
ambient_light = temp_light;
//find highest light value of background
}
temp_light = (SENSOR_2);
if (ambient_light > temp_light)
{
ambient_light = temp_light;
//find highest light value of background
}
}
until ((SENSOR_3) == temp_value);
// scanner_left;Wait(300);
//set scanner to the left
//figure out code to turn scanner until rotation sensor stops spinning
ClearSensor(SENSOR_3);
//clear rotation sensor
//figure out code to turn scanner until rotation sensor stops spinning and
ascertain full scan rotation value
do
{
temp_value = (SENSOR_3);
scanner_right;
Wait(2);
scanner_off;
temp_light = (SENSOR_1);
if (ambient_light > temp_light)
{
ambient_light = temp_light;
//find highest light value of background
}
temp_light = (SENSOR_2);
if (ambient_light > temp_light)
{
ambient_light = temp_light;
//find highest light value of background
}
}
until ((SENSOR_3) == temp_value);
// until (SENSOR_3 > (full_scan_number - 1));
ambient_light = ambient_light - 30;
full_scan_number = (SENSOR_3);
center_scan_number = full_scan_number / 2;
//set center_scan_number position of scanner
do
{
scanner_left;
}
until (SENSOR_3 <= center_scan_number);
//move scanner to center_scan_number position
scanner_off;
PlaySound(5);
start scanner_task;
start drive_task;
start bumper_task;
start finding_light_task;
}
/*
Scanner/Drive Task States--
1. Scanner sees light--bumpers not pressed
2. Scanner sees light--left bumper pressed
3. Scanner sees light--right bumper pressed
4. Scanner sees light--both bumpers pressed
5. Scanner does not see light--bumpers not pressed
6. Scanner does not see light--bumpers pressed
Bumper States--
0. No bumpers pressed
1. Right bumper pressed
2. Left bumper pressed
3. Both bumpers pressed
*/
task scanner_task()
{
while(true)
{
if (found_light == 1) //found light source--keep scanner on light
source
{
ClearTimer(0);
if (bumpers_pressed == 3) //4. Scanner sees light--both bumpers
pressed
{
scanner_off;
}
else if (bumpers_pressed == 2) //2. Scanner sees light--left bumper
pressed
{
scanner_left;
}
else if (bumpers_pressed == 1) //3. Scanner sees light--right bumper pressed
{
scanner_right;
}
else if (bumpers_pressed == 0) //1. Scanner sees light--bumpers
not pressed
{
if((SENSOR_1 > (SENSOR_2 + 10)))
{
scanner_right;
}
else if((SENSOR_2 > (SENSOR_1 + 10)))
{
scanner_left;
}
else
{
scanner_off;
}
}
}
else if (found_light == 0) //scanner doesn't care if bumpers are
pressed when no light found//5. Scanner does not see light--bumpers not
pressed//6. Scanner does not see light--bumpers pressed
{
//add code to check to see if scanner is stuck
if (bumpers_pressed != 0)
{
scanner_off;
}
else if (bumpers_pressed == 0)
{
if ((SENSOR_3 < full_scan_number) && (scan_direction == 1))
{
scanner_right;
}
else if ((SENSOR_3 > (full_scan_number -1))&& (scan_direction ==
1))
{
scan_direction=2;
}
else if ((SENSOR_3 > 0)&& (scan_direction == 2))
{
scanner_left;
}
else if ((SENSOR_3 < 1) && (scan_direction == 2))
{
scan_direction=1;
}
}
}
}
}
task drive_task()
{
while(true)
{
if (found_light == 1) //found light source--drive 'bot to
light source
{
if (bumpers_pressed == 3) //both bumpers pressed--we've
transferred //4. Scanner sees light--both bumpers pressed
{
PlaySound(1);
drive_reverse;Wait(100);
drive_left_spin;Wait(100);
found_light = 0;
temp_value = 0;
}
else if (bumpers_pressed == 2) //left bumper pressed--near
transfer opening--bot to go right//2. Scanner sees light--left bumper pressed
{
if (temp_value == 1)
{
PlaySound(1);
drive_reverse;Wait(100);
drive_left_spin;Wait(100);
found_light = 0;
temp_value = 0;
}
else if (temp_value != 1)
{
temp_value = 2;
drive_left;
}
}
else if (bumpers_pressed == 1) //right bumper pressed--near
transfer opening--bot to go left//3. Scanner sees light--right bumper pressed
{
if (temp_value == 2)
{
PlaySound(1);
drive_reverse;Wait(100);
drive_left_spin;Wait(100);
found_light = 0;
temp_value = 0;
}
else if (temp_value != 2)
{
temp_value = 1;
drive_right;
}
}
else if (bumpers_pressed == 0) //1. Scanner sees light--bumpers not pressed
{
if ((SENSOR_3 > (center_scan_number + 2)) && (SENSOR_3 <= (center_scan_number + (center_scan_number / 2)))) //no bumpers pressed--keep light source centered between scanners
{
drive_right;
}
else if (SENSOR_3 > (center_scan_number + (center_scan_number / 2))) //no bumpers pressed--keep light source centered between scanners
{
drive_right_spin;
}
else if ((SENSOR_3 < (center_scan_number - 2)) && (SENSOR_3 >=
(center_scan_number - (center_scan_number / 2)))) //no bumpers pressed--keep
light source centered between scanners
{
drive_left;
}
else if (SENSOR_3 < (center_scan_number - (center_scan_number /
2))) //no bumpers pressed--keep light source centered between scanners
{
drive_left_spin;
}
else
{
drive_forward;
//move towards light
}
}
}
else if (found_light == 0)
{
temp_value = 0;
if (bumpers_pressed != 0) //turn left if any touch sensor is
pressed //6. Scanner does not see light--bumpers pressed
{
drive_left_spin;
ClearTimer(0);
}
else if (bumpers_pressed == 0) //5. Scanner does not see light--bumpers not pressed
{
if ((Timer(0) >= 0) && (Timer(0) <= drive_time))
//drive straight for x seconds
{
drive_forward;
}
else if ((Timer(0) >= (drive_time + 1)) && (Timer(0) <=
(drive_time + 10))) //turn left for x seconds
{
drive_left;
}
else
{
ClearTimer(0);
}
}
}
}
}
task bumper_task()
//check to see if bumpers are pressed --bumpers_pressed bit set to yes if either
bumper is pressed
{
while(true)
{
if ((SENSOR_1 < touch_sensor_value) && (SENSOR_2 < touch_sensor_value)) //both bumpers pressed
{
bumpers_pressed=3;
}
else if ((SENSOR_1 < touch_sensor_value) && (SENSOR_2 >= touch_sensor_value)) //left bumper pressed
{
bumpers_pressed=2;
}
else if ((SENSOR_1 >= touch_sensor_value) && (SENSOR_2 < touch_sensor_value)) //right bumper pressed
{
bumpers_pressed=1;
}
else // if ((SENSOR_1 >= touch_sensor_value) && (SENSOR_2 >=
touch_sensor_value)) //no bumper pressed
{
bumpers_pressed=0;
}
}
}
task finding_light_task()
//check to see if we found light --found_light bit set to yes if either sensor
has found value
{
while(true)
{
if (((SENSOR_1 >= touch_sensor_value) && (SENSOR_1 <=
ambient_light))||((SENSOR_2 >= touch_sensor_value) && (SENSOR_2 <=
ambient_light)))
{
found_light=1;
}
else //if ((SENSOR_1 > ambient_light)&&(SENSOR_2 > ambient_light))
{
found_light=0;
}
}
}
|
|
Message has 1 Reply:
 | | Re: Gear lash....
|
| In lugnet.org.ca.rtltoronto, David Koudys wrote: <snip> Forgot to mention where my issues are where the coding's concerned-- I wanted the setup routine to take care of, well, just about everyting--I didn't wnat to hard-code values into the 'bot but (...) (19 years ago, 2-Mar-06, to lugnet.org.ca.rtltoronto)
|
Message is in Reply To:
| | Re: Gear lash....
|
| (...) But if you are "close" to the TO...then your precision usually is higher anyways--less movement, less slop? Here's some made up numbers to illustrate what I don't understand... If I read at time 0 at 30 degrees, a "contact". By the time I come (...) (19 years ago, 2-Mar-06, to lugnet.org.ca.rtltoronto)
|
33 Messages in This Thread:
 
 
- Entire Thread on One Page:
- Nested:
All | Brief | Compact | Dots
Linear:
All | Brief | Compact
This Message and its Replies on One Page:
- Nested:
All | Brief | Compact | Dots
Linear:
All | Brief | Compact
|
|
|
|
