Rex
From ZENotes
NXC program to run Alpha-rex from the base NXT set.
Instructions to build Rex are here
Graham Hawkins (29 Jan 2007)
Performs the tasks set in the 'Test Guide' sections of the construction pane of the Mindstorms NXT-G for Alpha-Rex:
1. Walk from the start of the rest pad to the ball & back. Actually it keeps walking in loops ad infinitum.
2. Display beating heart.
3. Say 'hello' when something comes near to the US sensor, and 'goodbye' when it goes away.
4. Say 'Play music please' when touch sensor operated. Then dance if loud noise detected.
5. Go to sleep when the lights go out, wake up when they come on. Movements are performed as an integer number of 'steps' to try and keep the 'lift' and 'pace' actions in phase as per pages 46-49 of the building guide.
//Some Constants:
#define MY_ONE_STEP 1080 //Motor has an 8 tooth gear. Leg drive has a 24 tooth gear. So 1 step is 360*24/8 degrees = 1080
#define MY_STEPS_TO_BALL 11 //Each step is about 4.25cm. The ball is about 50cm up the test pad from the start position, so that's about 11 steps.
// ...YMMV depending on friction...
#define MY_STEPS_TO_TURN 8 // It takse about 8 steps from motor B to execute a 180 degree turn
#define MY_FWD_PWR 30 // Define some motor power levels
#define MY_BOOGIE_PWR 70 // Define some motor power levels
// We need a mutex to stop the 'move' task when we want to dance instead of walk.
// The shared resource consists of the drive motors B & C and the arm/head motor A.
mutex motors_a_b_c;
mutex the_display; // And a mutex for the display to stop the heart 'beat' task when the 'sleep' task wants to display 'Zzzz'.
task main() //task main just exits, and the 5 following tasks start.
{
}
/*
Move forever up to the ball position and back to the start position. This is likely to be very inaccurate since the distance
walked per step varies depending on the coefficient of friction between the foot & the surface...
*/
task move()
{
short i = 0;
Follows (main);
while (1)
{
/*
Would have used function calls here, but the mutex interaction
ith the dance task doesn't seem to work if the mutex
operators are in a function.
*/
//move_steps(MY_STEPS_TO_BALL, MY_FWD_PWR);
//turn_steps(MY_STEPS_TO_TURN, MY_FWD_PWR);
/*
After each step, release the motor mutex in case the dance task
want the motors. The re-acquire the motors and carry on.
*/
i = 0;
while (i < MY_STEPS_TO_BALL)
{
Acquire(motors_a_b_c);
OnFwd(OUT_A, MY_FWD_PWR);
/* Use synch API to try & keep drive motors in step */
RotateMotorEx(OUT_BC, MY_FWD_PWR, MY_ONE_STEP, 0, true, true);
Float(OUT_A);
Release (motors_a_b_c);
i++;
}
i = 0;
while ( i < MY_STEPS_TO_TURN)
{
Acquire(motors_a_b_c);
OnFwd(OUT_A, MY_FWD_PWR);
RotateMotor(OUT_B, MY_FWD_PWR, MY_ONE_STEP);
Float(OUT_A);
Release (motors_a_b_c);
i++;
}
}
}
/*
This task monitors the US sensor for things getting closer than
12cm. If detected, plays Hello.rso. If something is near, monitors
for it going away, and plays Goodbye.rso.
*/
task ultra_sonic ()
{
short ultra = 0;
short hello_thr = 12;
short bye_thr = 20;
bool near = false;
Follows (main);
SetSensorLowspeed(IN_4);
while (1)
{
ultra = SensorUS(IN_4);
//NumOut(10, LCD_LINE7, true, ultra);
if (ultra <= hello_thr)
{
if(!near)
{
PlayFile("Hello.rso");
near = true;
}
}
else if ( ultra > bye_thr)
{
if(near)
{
PlayFile("Goodbye.rso");
near = false;
}
}
/*
No point in using up all the CPU whizzing round this loop.
*/
Wait(200);
}
}
/*
This task displays a beating heart.
*/
task beat()
{
/*
Set up some co-ordinates so the heart appears to beat in place
rather than moving to the right and back.
*/
short big_heart_x = 16;
short big_heart_y = 1;
short little_heart_x = 20;
short little_heart_y = 4;
/*
Start the heart after main() exits.
*/
Follows (main);
while (1)
{
/*
Little heart
*/
Acquire(the_display);
GraphicOut(little_heart_x, little_heart_y, "Heart 02.ric",true);
Release(the_display);
Wait(1000);
/*
Big heart
*/
Acquire(the_display);
GraphicOut(big_heart_x, big_heart_y, "Heart 01.ric", true);
Release(the_display);
Wait(1000);
}
}
/*
Task to control dance movements in response to touch and sound
sensors.
*/
task dance()
{
long tick = 0;
long begin_tick = 0;
long wait_time = 3000;
bool wait_expired = false;
short sound_level;
short sound_threshold = 40;
Follows (main);
SetSensorTouch(IN_1);
SetSensorSound(IN_2);
while (1)
{
/*
Wait for touch sensor to be pressed. Get the motor mutex, and
play specified files.
*/
while(!Sensor(IN_1))
{
;
}
Acquire(motors_a_b_c);
PlayFile("Play.rso");
Wait(500);
PlayFile("Music.rso");
Wait(500);
PlayFile("Please.rso");
/*
Wait 3 secs for a loud noise. Dance if noise detected. Use
system tick rather than Wait() because we want to test the sound
sensor during the wait period.
*/
wait_expired = false;
begin_tick = CurrentTick();
while (!wait_expired)
{
tick = CurrentTick();
if ((tick - begin_tick)>=wait_time)
{
wait_expired = true;
}
sound_level = Sensor(IN_2);
if (sound_level >= sound_threshold)
{
RotateMotor(OUT_AC, MY_BOOGIE_PWR, 5*MY_ONE_STEP);
wait_expired = false;
begin_tick = CurrentTick();
}
}
/*
No more loud noises heard. Let move task have the motors back.
*/
Release (motors_a_b_c);
}
}
/*
Sleep task grabs the motors & display if amient light drops by hysteresis value.
Releases them if ambient goes up by hysteresis.
*/
task sleep()
{
short ambient;
short hysteresis = 20;
short light_value;
bool dark = false;
Follows(main);
SetSensorType(IN_3,IN_TYPE_LIGHT_INACTIVE);
/*
Assume it's daytime
*/
ambient = Sensor(IN_3);
dark = false;
while (1)
{
light_value = Sensor(IN_3);
//NumOut(10, LCD_LINE7, true, light_value);
if (dark)
{
if( light_value > (ambient + hysteresis))
{
PlayFile("Goodmorning.rso");
Wait(500);
Release(the_display);
Release (motors_a_b_c);
dark = false;
ambient = light_value;
}
}
if (!dark)
{
if( light_value < (ambient - hysteresis))
{
Acquire(the_display);
Acquire (motors_a_b_c);
Wait(500);
PlayFile("Good.rso");
Wait(500);
PlayFile("Night.rso");
Wait(500);
GraphicOut(10, 10, "Zzzz.ric", true);
dark = true;
ambient = light_value;
}
}
/*
No point in using up all the CPU whizzing round this loop.
*/
Wait(200); } }
