Hi,
My code here seems to be working but the ground sensor area still does
not seem to be following the line. Can anybody help? I'm using webots. Also...
that is the voltage of e-puck batteries?
#include <webots/robot.h>
#include <webots/differential_wheels.h>
#include <webots/camera.h>
#include <math.h>
#include <stdlib.h>
#include <stdio.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 WHITE 0
#define BLACK 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};
const int PS_OFFSET_REALITY[NB_DIST_SENS] = {480,170,320,500,600,680,210,640};
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]=-180;//rotate right
speed[LEFT]=100;
wb_differential_wheels_set_speed(speed[LEFT],speed[RIGHT]);
}
else if((int)fs_value[0]>330)
{
speed[RIGHT]=100;// rotate left
speed[LEFT]=-180;
wb_differential_wheels_set_speed(speed[LEFT],speed[RIGHT]);
}
else if((int)fs_value[1]>330)
{
speed[RIGHT]=400;// forward
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();
}
}
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
E-puck-user@gna.org
https://mail.gna.org/listinfo/e-puck-user
