Can anyone tell me whats wrong with this code?
My e-puck does not seem to follow the black line and read shapes...
#include <webots/differential_wheels.h>
#include <webots/camera.h>
#include <math.h>
#include <webots/robot.h>
#include <stdlib.h>
#include <webots/robot.h>
#include <webots/led.h>
#include <string.h>
#include <time.h>
#include <webots/distance_sensor.h>
#define TIME_STEP_CAM 64
#define TIME_STEP 64
#define TRUE 1
#define FALSE 0
#define LEFT 0
#define RIGHT 1
#define NB_LEDS 10
#define NB_DIST_SENS 8
#define PS_RIGHT_10 0
#define PS_RIGHT_45 1
#define PS_RIGHT_90 2
#define PS_RIGHT_REAR 3
#define PS_LEFT_REAR 4
#define PS_LEFT_90 5
#define PS_LEFT_45 6
#define PS_LEFT_10 7
#define NB_FLOOR_SENS 3
#define FS_WHITE 900
#define FS_LEFT 0
#define FS_CENTER 1
#define FS_RIGHT 2
WbDeviceTag cam;
WbDeviceTag ps[NB_DIST_SENS];
WbDeviceTag fs[NB_FLOOR_SENS];
WbDeviceTag led[NB_LEDS];
int speed[2]={0,0};
int ps_value[NB_DIST_SENS]={0,0,0,0,0,0,0,0};
unsigned short fs_value[NB_FLOOR_SENS]={0,0,0};
unsigned short width,height;
int i;
int shapesize=0;
int delay=0;
int shapechecking,whitechecking;
int Itriangle,square,triangle;
int stop=0;
void Sensor_check()
{
for(i=0;i<NB_FLOOR_SENS;i++)
{
fs_value[i]=(int)wb_distance_sensor_get_value(fs[i]);
}
for(i=0;i<NB_DIST_SENS;i++)
{
ps_value[i]=(int)wb_distance_sensor_get_value(ps[i]);
}
}
void UTurn()
{
while((int)fs_value[0]>330)
{
speed[RIGHT]=400;//rotate right
speed[LEFT]=-400;
wb_differential_wheels_set_speed(speed[LEFT],speed[RIGHT]);
Sensor_check();
}
}
void Offled()
{
for(i=0;i<9;i++)
{
wb_led_set(led[i],FALSE);
}
}
void MoveObstacle()
{
Sensor_check();
while ((int)ps_value[0]<2000&&(int)ps_value[7]<2000)
{
Sensor_check();
speed[RIGHT]=300;
speed[LEFT]=300;
wb_differential_wheels_set_speed(speed[LEFT],speed[RIGHT]);
}
delay=10000;
while (delay!=0)
{
wb_led_set(led[0],TRUE);
wb_led_set(led[7],TRUE);
wb_led_set(led[6],TRUE);
speed[RIGHT]=450;
speed[LEFT]=-450;
wb_differential_wheels_set_speed(speed[LEFT],speed[RIGHT]);
delay--;
}
Offled();
delay=12000;
while (delay!=0)
{
wb_led_set(led[0],TRUE);
wb_led_set(led[8],TRUE);
speed[RIGHT]=650;
speed[LEFT]=650;
wb_differential_wheels_set_speed(speed[LEFT],speed[RIGHT]);
delay--;
}
Offled();
delay=12000;
while (delay!=0)
{
wb_led_set(led[5],TRUE);
wb_led_set(led[4],TRUE);
speed[RIGHT]=-650;
speed[LEFT]=-650;
wb_differential_wheels_set_speed(speed[LEFT],speed[RIGHT]);
delay--;
}
Offled();
wb_led_set(led[0],TRUE);
wb_led_set(led[1],TRUE);
wb_led_set(led[2],TRUE);
speed[RIGHT]=-300;
speed[LEFT]=300;
wb_differential_wheels_set_speed(speed[LEFT],speed[RIGHT]);
}
void black_line()
{
Sensor_check();
if((int)fs_value[2]<330)
{
speed[RIGHT]=speed[RIGHT]-180;//rotate right
speed[LEFT]=speed[LEFT]+100;
wb_differential_wheels_set_speed(speed[LEFT],speed[RIGHT]);
}
else if((int)fs_value[0]<330)
{
speed[RIGHT]=speed[RIGHT]+100;// rotate left
speed[LEFT]=speed[LEFT]-180;
wb_differential_wheels_set_speed(speed[LEFT],speed[RIGHT]);
}
else if((int)fs_value[1]<330)
{
speed[RIGHT]=400;// foward
speed[LEFT]=400;
wb_differential_wheels_set_speed(speed[LEFT],speed[RIGHT]);
}
}
void movewall()
{
speed[RIGHT]=400;
speed[LEFT]=400;
wb_differential_wheels_set_speed(speed[LEFT],speed[RIGHT]);
Sensor_check();
while((int)ps_value[0]>200||(int)ps_value[7]>200)
{
Sensor_check();
black_line();
}
speed[RIGHT]=400;
speed[LEFT]=400;
wb_differential_wheels_set_speed(speed[LEFT],speed[RIGHT]);
}
void shape()
{
if(shapesize==0)
{
shapesize=shapechecking;
}
else if(shapesize<shapechecking&&whitechecking>3)
{
triangle++;
}
else if(shapesize>shapechecking&&whitechecking>3)
{
Itriangle++;
}
else if(shapesize==shapechecking&&whitechecking>3)
{
square++;
}
shapesize=shapechecking;
}
void shapecompare()
{
if(triangle>square&&triangle>Itriangle)//triangle
{
movewall();
}
else if(triangle<square&&square>Itriangle)//square
{
MoveObstacle();
}
else if(Itriangle>triangle&&square<Itriangle)// Itriangle
{
UTurn();
}
Offled();
triangle=0;
square=0;
Itriangle=0;
shapesize=0;
whitechecking=0;
}
void CheckCamera()
{
int reg1,x,y;
const unsigned char *image;
cam = wb_robot_get_device("camera");
wb_camera_enable(cam,TIME_STEP_CAM);
width = wb_camera_get_width(cam);
height = wb_camera_get_height(cam);
image = wb_camera_get_image(cam);
for (y=38;y>0;y--)
{
for (x=0;x<width;x++)
{
reg1 = wb_camera_image_get_red(image,width,x,y);
if (reg1<50)
{
shapechecking=x;
whitechecking=x-1;
shape();
y--;
x=0;
}
}
}
}
int main() {
wb_robot_init();
char light[]="led0";
for(i=0;i<NB_LEDS;i++)
{
led[i]=wb_robot_get_device(light);
light[3]++;
}
char named[]="fs0";
for (i = 0; i < NB_FLOOR_SENS; i++)
{
fs[i] = wb_robot_get_device(named); /* floor sensors */
wb_distance_sensor_enable(fs[i],TIME_STEP);
named[2]++;
}
char textPS[]="ps0";
for(i=0;i<NB_DIST_SENS;i++)
{
ps[i]=wb_robot_get_device(textPS);
wb_distance_sensor_enable(ps[i],TIME_STEP);
textPS[2]++;
}
//main loop
while(stop==0)
{
Sensor_check();
if((int)ps_value[0]>200||(int)ps_value[7]>200)
{
speed[RIGHT]=0;
speed[LEFT]=0;
wb_differential_wheels_set_speed(speed[LEFT],speed[RIGHT]);
CheckCamera();
shapecompare();
}
else
{
black_line();
}
}
/* Enter here exit cleanup code */
/* Necessary to cleanup webots stuff */
wb_robot_cleanup();
return 0;
}
_______________________________________________
E-puck-user mailing list
[email protected]
https://mail.gna.org/listinfo/e-puck-user
