Control of a minisumo robot

Transcription

1 Control of a minisumo robot J. Jakubiak, based on L. Ma lek s Sterowanie robotem minisumo Wroc law, Introduction The exercise aims to introduce problems of controlling mobile robots in bounded, dynamically changing environment with limited sensor information. The exercise is carried out in the Webots simulation environment, with a simplified model of a minisumo class robot. 1.1 Simulation Environment Webots is the program which allows modeling and simulating any kind of robots, It contains several ready-to-use libraries for modeling sensory, gear and other robot subsystems. Physical properties of models are incorporated in Webots by use of the ODE (Open Dynamics Engine) library. Controllers for the models of robots made in this environment can be programmed in in C, C++ or Java. After running webots, on the screen there will appear four windows (Figure 1): Figure 1: General view of Webots application and model of robot visualization scene tree program editor program logs Visualization window is the main element of the application. It allows opening new scenes and running simulations. For this exercise open minisumo.wbt. 1

2 1.2 Robot The scheme of the robot is shown in Fig. 2 Robor dimensions are chosen to fulfill rules of Figure 2: Scheme of a minisumo robot minisumo class competition: to fit in a box with base 10cm 10cm and total mass 0.5kg. It was cosen that 0.05kg is a mass of each of wheels, remaining 0.4kg is robot body. Mass is spread uniformly in the robot. The robot is driven by two independent wheels, with an axis moved back from symetry axis of robot (therefore robot does not swing during acceleration). Third support point is the plow in the front. Sensor system is limited to 6 sensors, first four (ds tp, ds tl, ds pp, ds pl) are IR sensors pointed to the bottom, used to detect the white line. They are placed in corners of the robot (last two letters mark the placement of a sensor: first: p front or t rear, second: p right or l left). Two other sensors are distance sensors heading horizontaly ahead from the robot (ds gp, ds gl). Those sensors have nonlinear characteristic shown in Fig. 3. Figure 3: Function of distance sensor reading against real distance from an object 2

3 #include <device/robot.h> #include <device/differential_wheels.h> #include <device/distance_sensor.h> * Here one can add other additional libraries #define TIME_STEP 64 cycle step in ms #define SPEED 120 max wheel speed * Here enter required macros static void reset(void); static int run(int); * Here enter prototypes of functions used in program static DeviceTag ds_pl, ds_pp, ds_tl, ds_tp, ds_gl, ds_gp; * Here enter definitions of global variables, like * DeviceTag, which refer to robot devices * (use "static" modifier!) * reset() is a function run once, at the initialization of robot static void reset(void) { * Here enter robot_get_device() function to associate * variables with robot devices (DeviceTag), which are used * in main control loop ds_pl = robot_get_device("ds_pl"); ds_pp = robot_get_device("ds_pp"); ds_gl = robot_get_device("ds_gl"); ds_gp = robot_get_device("ds_gp"); ds_tl = robot_get_device("ds_tl"); ds_tp = robot_get_device("ds_tp"); * Here define frequency of sensor sampling distance_sensor_enable(ds_pl, TIME_STEP); distance_sensor_enable(ds_pp, TIME_STEP); distance_sensor_enable(ds_gl, TIME_STEP); distance_sensor_enable(ds_gp, TIME_STEP); distance_sensor_enable(ds_tl, TIME_STEP); distance_sensor_enable(ds_tp, TIME_STEP); } return; 3

4 * Main control loop run periodicaly by Webots static int run(int ms) { unsigned short ds_pl_value; unsigned short ds_pp_value; unsigned short ds_gl_value; unsigned short ds_gp_value; unsigned short ds_tl_value; unsigned short ds_tp_value; * Here enter sensor reading function with: * unsigned short distance_sensor_get_value (DeviceTag sensor); ds_pl_value = distance_sensor_get_value(ds_pl); ds_pp_value = distance_sensor_get_value(ds_pp); ds_gl_value = distance_sensor_get_value(ds_gl); ds_gp_value = distance_sensor_get_value(ds_gp); ds_tl_value = distance_sensor_get_value(ds_tl); ds_tp_value = distance_sensor_get_value(ds_tp); * Here process sensor data * Here enter functions responsible for execution, like: * void differential_wheels_set_speed(short left, short right); differential_wheels_set_speed( 0, 0); } return TIME_STEP; time step value in ms * Main program which defines initialization function * and main program loop int main() { robot_live(reset); initialization robot_run(run); start of controller } return 0; Hint: It is useful in tests to use void robot_console_printf(const char *format,...); which allows to display values (like in standard printf) in log window. 4

5 2 Task 1 Preparation: 1. Select new controller creator wizard from the menu bar of the window Kreator->Nowy kontroler robota (Wizard->New robot controller) 2. Choose C language 3. Give a name to a new controller: minisumo1 4. In the scene tree expand DifferentialWheels item by clicking on the triangle to its left 5. Indicate controller and select minisumo1 controller in the window to the right. 1, 6. To build a controller use the gear icon located in the editor window. Things To Do: Figure 4: Robot moving along the diameter of the ring 1. Program robot controller in such a way that it reads line sensors (ds tl, ds tp, ds pl, ds pp) and writes read values in log window. 2. Note readings of sensors when they are facing down (the direction in which they are faced are indicated by red lines) 3. Use your mouse to move the robot, so that the sensors are located over the white line surrounding the ring and note sensor reading in this case. 4. Determine a value of reading which allows to detect the white line. 5. Set the robot in the center of the ring, and then program it to move straight until it detects a white line, then it should change direction to the opposite. 1 After any change of the controller save the scene by pressing a floppy disk and then restart the scene by pressing the button with two arrows forming a circle. 5

6 3 Task 2 Preparation 1. Collapse the open node DifferentialWheels. 2. Add a second robot to the scene by pressing the scene tree window, click Import (a disk with an arrow pointing to the left). 3. Open the directory minisumo/worlds and select the file sumita.wbt, press Otwórz (Open) 4. In the newly added (last) DifferentialWheels node change controller to void. 5. Place the newly added robot (green) in the center of the ring. 6. Place the old robot (blue) on the edge of the ring so that the robots are placed like in Fig Add a new controller called minisumo2. 8. Set the controller to minisumo2 for the blue robot. Figure 5: Blue robot tracking the green robot moving round the ring 1. Program blue robot to read distance sensors (ds gl, ds gp) and write them in log window. 2. Observe change of sensor readings for different distances from an obstacle (use the green robot as the obstacle). Confirm the nonlinearity of sensors. 3. Set green robot controller to minisumo1 and place the robot like in Fig Program the blue robot to track green robot motion (blue robot should always turn in direction of the green robot) 6

7 4 Task 3 Preparation Figure 6: Initial position of blue and green robot in task 3 1. Define new controller minisumo3 for the blue robot Things to do 1. Program blue robot so it finds the green robot and pushes it out of the ring, but remaining on the ring (use white line sensors) 2. Change green robot controller to the same (minisumo3), place robots like on Fig. 6 and run simulation. 7