| |
Hi, I'm struggling to get threads working (in a simple motor speed matching
program), the code
just keeps hanging up. I've isolated the problem to when I restart the
threads after killing them
if a bumper is hit in the bumper_task() function. The program appears to
hang up at various
points, but typically in the stop_motors() function? Any help would be great
as I have totally
run out of ideas ;(
thanks, mike
CODE
**********************
//
**************************************************
// Program:
rover_pid_control_1
// Description: simple rover with left and right motors
// speed
matched with a PID algorithm
// Notes:
// Outputs: A - right motor, B - front motor, C - left
motor
// Inputs: 1 - right encoder, 2 - bumper, 3 - left encoder
// Versions: 0.1 get it up and
running
// Programmer: Mike McFarlane
// Date: 5-1-02
// legOS version: 0.2.5
//
development OS: win98 with cygwin
//
**************************************************
//
**************************************************
// Includes
//
**************************************************
#include <conio.h> // console
i/o
#include <dmotor.h>
#include <dsensor.h>
#include <unistd.h> // UNIX std for
timers and task management
#include <dsound.h>
#include <semaphore.h>
#include
<stdlib.h> // for random numbers
//
**************************************************
// Define
//
**************************************************
//
**************************************************
// Declarations
//
**************************************************
// global process
id's
pid_t bumper_thread;
pid_t left_thread;
pid_t right_thread;
pid_t
speed_thread;
// **************************************************
//
Function prototypes
// **************************************************
int
bumper_task (int argc, char *argv[]);
int left_control_task (int argc, char
*argv[]);
int right_control_task (int argc, char *argv[]);
int speed_target_task (int
argc, char *argv[]);
wakeup_t bumper_hit (wakeup_t data);
wakeup_t bumper_release
(wakeup_t data);
void start_motors_full();
void stop_motors ();
//
**************************************************
// Functions
//
**************************************************
//
**************************************************
// Function: main
//
Description:
// Notes:
// Returns: none
//
**************************************************
int main(int argc, char
*argv[])
{
// ensure safe startup
motor_a_speed (0);
motor_b_speed
(0);
motor_c_speed (0);
motor_a_dir (off);
motor_b_dir (off);
motor_c_dir
(off);
// print prog name to screen
cputs("rover");
msleep
(300);
cputs("PID");
msleep (300);
cputs("control");
msleep (300);
//
initialise sensors
ds_active (&SENSOR_1);
ds_active
(&SENSOR_3);
ds_rotation_on (&SENSOR_1);
ds_rotation_on (&SENSOR_3);
//
calibrate sensors
ds_rotation_set (&SENSOR_1, 0);
ds_rotation_set (&SENSOR_3,
0);
msleep (100);
// seed the random number generator
srandom (100);
// start
tasks
// two tasks are used for speed control of left and right drives
// a high priority
task is used to monitor the bumper
// a low priority task is used to calc an average max
speed
bumper_thread = execi (&bumper_task, 0, 0, 5, DEFAULT_STACK_SIZE);
left_thread
= execi (&left_control_task, 0, 0, 3, DEFAULT_STACK_SIZE);
right_thread = execi
(&right_control_task, 0, 0, 3, DEFAULT_STACK_SIZE);
speed_thread = execi
(&speed_target_task, 0, 0, 1, DEFAULT_STACK_SIZE);
// start
motors
start_motors_full ();
return 0;
} // end of main()
void
start_motors_full()
{
motor_a_speed (255);
motor_b_speed
(255);
motor_c_speed (255);
motor_a_dir (fwd);
motor_b_dir
(fwd);
motor_c_dir (fwd);
return;
} // end of start_motors_full()
void
stop_motors ()
{
dsound_system (0);
motor_a_speed (0);
motor_b_speed
(0);
motor_c_speed (0);
motor_a_dir (brake);
motor_b_dir
(brake);
motor_c_dir (brake);
msleep (50);
motor_a_dir (off);
motor_b_dir
(off);
motor_c_dir (off);
return;
} // end of stop_motors ()
wakeup_t
bumper_hit (wakeup_t data)
{
return (TOUCH_2);
} // end of bumper_hit_wakeup
()
wakeup_t bumper_release (wakeup_t data)
{
return (!TOUCH_2);
} // end of
bumper_release
int bumper_task (int argc, char *argv[])
{
long int
random_delay;
while (1)
{
// wait for bumper to be hit (and released to
debounce)
wait_event (&bumper_hit, 0);
msleep (250);
wait_event
(&bumper_release, 0);
// hit an object so kill all other threads and stop
kill
(left_thread);
kill (right_thread);
kill (speed_thread);
stop_motors
();
// back up
motor_a_speed (200);
motor_b_speed (200);
motor_c_speed
(200);
motor_a_dir (rev);
motor_b_dir (rev);
motor_c_dir (rev);
msleep
(500);
// turn a random amount
motor_a_dir (fwd);
random_delay =
random()/1000000;
msleep(random_delay);
stop_motors ();
// restart
threads and motors
******** SEEMS TO BE THE NEXT 2 LINES THAT HANG IT
********
left_thread = execi (&left_control_task, 0, 0, 3,
DEFAULT_STACK_SIZE);
right_thread = execi (&right_control_task, 0, 0, 3,
DEFAULT_STACK_SIZE);
speed_thread = execi (&speed_target_task, 0, 0, 1,
DEFAULT_STACK_SIZE);
start_motors_full ();
}
return 0;
} // end of
bumper_task ()
int left_control_task (int argc, char *argv[])
{
// dummy code to keep
it simple till main structure correct
int count = 0;
for (count = 255; count > 100; count--
)
{
motor_a_speed (count);
lcd_int (count);
msleep
(100);
}
stop_motors ();
return 0;
} // end of left_control_task ()
int
right_control_task (int argc, char *argv[])
{
// dummy code
int count = 0;
for
(count = 255; count > 100; count--)
{
motor_c_speed (count);
msleep
(100);
}
return 0;
} // end of right_control_task ()
int speed_target_task
(int argc, char *argv[])
{
return 0;
} // end of speed_target_task ()
|
|
Message has 1 Reply:
 | | Re: threads
|
| Hi again, I'm now totally stuck with this problem. I've spent all day at it, stepping through the code line by line with key presses. I've marked the point that the code appears to hang in left_control_task (). Someone please help, please. Thanks (...) (22 years ago, 15-Jan-02, to lugnet.robotics.rcx.legos)
|
18 Messages in This Thread:
  
- Entire Thread on One Page:
- Nested:
All | Brief | Compact | Dots
Linear:
All | Brief | Compact
|
|
|
|
