Commit b724b467 authored by Nicolas Peslerbe's avatar Nicolas Peslerbe

State

parent 83b40fa7
No preview for this file type
......@@ -38,9 +38,9 @@
#define ROBOT_TO_MONITOR 0
#define TIME_STEP 64 // [ms] Length of time step
#define EPS 0.8
#define MIGR_SPEED 0.5
#define SPEED_CORR 0.05
#define EPS 0.1
#define MIGR_SPEED 0.05
#define SPEED_CORR 0.02
// ********
#define NB_SENSORS 8 // Number of distance sensors
......@@ -58,8 +58,8 @@ static void reset(){
robot_name=(char*) wb_robot_get_name();
sscanf(robot_name,"epuck%d",&robot_id_u);
robot_id = robot_id_u%FLOCK_SIZE;
//if((int)(robot_name[5]-'0') == ROBOT_TO_MONITOR)
// utilities_changeDebugState(1);
if((int)(robot_name[5]-'0') == ROBOT_TO_MONITOR)
utilities_changeDebugState(1);
printf("Reset: robot %d\n",robot_id_u);
deadReckoning_init();
......@@ -100,10 +100,11 @@ int main(){
// compute new speed according to laplace method in initial referential
laplace_compute(list, my_speed, weights, my_position);
if(utilities_debug()) printf("Before migr x = %f, y = %f\n", my_speed[X], my_speed[Y]);
utilities_transformReferential(my_speed, my_speed_tmp, -my_position[2]); //from Rt to R0
// add migration urge and realignment in intial referential
if(utilities_debug()) printf("Before migr x = %f, y = %f\n", my_speed_tmp[X], my_speed_tmp[Y]);
migratory(my_speed_tmp, my_position);
......
......@@ -25,8 +25,8 @@ void deadReckoning_updateSelfPosition(float* my_position, int* applied_speed){
float dtheta = (dr - dl)/AXLE_LENGTH;
// Compute deltas in the environment
float dx = du * cosf(my_position[2]+dtheta/2);
float dy = du * sinf(my_position[2]+dtheta/2);
float dx = du * cosf(my_position[2]);
float dy = du * sinf(my_position[2]);
// Update position ??? maybe add time step???
......
......@@ -20,11 +20,11 @@
#include <stdio.h>
#define NB_SENSORS 8
#define DISTANCE_THRESHOLD 150
#define DISTANCE_THRESHOLD 200
#define EPS 0.78
float ps_orientations[8] = {-0.3,-0.8,-1.57,3.64,2.64,1.57,0.8,0.3}; //proximity sensors orientations
float ps_weights[8] = {10, 8, 6, 3, 3,10, 14, 18};
static WbDeviceTag ds[NB_SENSORS];
bool mustPrint = false;
......@@ -47,12 +47,10 @@ Measurement* infrared_updateList(Measurement* measurementList, int* obstacle){
{
intensity = wb_distance_sensor_get_value(ds[i]);
if(utilities_debug()) printf("sensor %d intensity at orientation %f = %f\n", i, ps_orientations[i], intensity);
//if(i == 3 || i == 4) continue;
if (intensity > DISTANCE_THRESHOLD){
float distance = sqrt(1/intensity);
float theta = ps_orientations[i];
bool isRobot = false;
/*if(measurementList){ // BUG ifno element in list
......@@ -85,12 +83,12 @@ Measurement* infrared_updateList(Measurement* measurementList, int* obstacle){
temp->relativePosition[DISTANCE] = distance; // relative x pos
temp->relativePosition[THETA] = theta; // relative y pos
if(temp->relativePosition[THETA] > 0 && temp->relativePosition[THETA] < 3.1415926)
temp->weight = 30;
temp->weight = ps_weights[i]*2;
/*if(temp->relativePosition[THETA] > 0 && temp->relativePosition[THETA] < 3.1415926 && my_position[Y] > 0 )
temp->weight = 15;
else
temp->weight = 10;
*/
if(utilities_debug()) printf("Object detected: distance = %f, theta = %f\n", temp->relativePosition[DISTANCE], temp->relativePosition[THETA]);
......
......@@ -203,9 +203,8 @@ void laplace_compute(Measurement* objectsInSurround, float* newSpeed, float* wei
}
}
newSpeed[X] = vel_graph[X];
newSpeed[Y] = vel_graph[Y];
newSpeed[X] = vel_graph[X]/20;
newSpeed[Y] = vel_graph[Y]/20;
if(utilities_debug()) printf("Speed laplacian x = %f, y = %f\n", newSpeed[X], newSpeed[Y]);
......
......@@ -17,12 +17,12 @@
#include <stdio.h>
#include <math.h>
#define KU 1
#define KW 1
#define KU 0.2
#define KW 1.0
#define AXLE_LENGTH 0.052
#define WHEEL_RADIUS 0.0205
//#define MAX_SPEED 1000
#define MAX_SPEED 628
#define MAX_SPEED_WEB 6.28
#define MAX_SPEED_EPUCK 1000
static WbDeviceTag left_motor;
static WbDeviceTag right_motor;
......@@ -45,20 +45,24 @@ void motors_init(){
void motors_transformSpeed(float* speed, int *speed_for_motors){
float linearSpeed = KU * sqrt(pow(speed[X],2)+pow(speed[Y],2)) * cosf(atan2(speed[Y],speed[X]));
float rotationalSpeed = KW * atan2(speed[Y],speed[X]);
float range = sqrtf(speed[X]*speed[X] + speed[Y]*speed[Y]);
if(utilities_debug()) printf("linearSpeed = %f and rotationalSpeed = %f\n", linearSpeed, rotationalSpeed);
float bearing = atan2( speed[Y], speed[X]);
if(utilities_debug()) printf("range = %f and bearing = %f\n", range, bearing);
float u = KU*range*cosf(bearing);
float w = KW*bearing;
speed_for_motors[LEFT] = (u - AXLE_LENGTH*w/2.0) * (1000.0 / WHEEL_RADIUS);
speed_for_motors[RIGHT]= (u + AXLE_LENGTH*w/2.0) * (1000.0 / WHEEL_RADIUS);
speed_for_motors[RIGHT] = (linearSpeed + AXLE_LENGTH * rotationalSpeed)/2.0 * (10 / WHEEL_RADIUS);
speed_for_motors[LEFT] = (linearSpeed - AXLE_LENGTH * rotationalSpeed)/2.0 * (10 / WHEEL_RADIUS);
if(utilities_debug()) printf("After apply trans (motor cmd) left = %d, right = %d\n", speed_for_motors[LEFT], speed_for_motors[RIGHT]);
//For epuck
motors_limit(&(speed_for_motors[LEFT]),MAX_SPEED);
motors_limit(&(speed_for_motors[RIGHT]),MAX_SPEED);
motors_limit(&(speed_for_motors[LEFT]), MAX_SPEED_EPUCK);
motors_limit(&(speed_for_motors[RIGHT]), MAX_SPEED_EPUCK);
if(utilities_debug()) printf("After limiting apply trans (motor cmd) left = %d, right = %d\n", speed_for_motors[LEFT], speed_for_motors[RIGHT]);
......@@ -72,6 +76,6 @@ void motors_applySpeed(float* speed, int* appliedSpeed){
if(utilities_debug()) printf("Motor speed left = %d, right = %d\n", appliedSpeed[LEFT], appliedSpeed[RIGHT]);
wb_motor_set_velocity(left_motor, (double) appliedSpeed[LEFT]/100);
wb_motor_set_velocity(right_motor, (double) appliedSpeed[RIGHT]/100);
wb_motor_set_velocity(left_motor, ((double) appliedSpeed[LEFT])*MAX_SPEED_WEB/MAX_SPEED_EPUCK);
wb_motor_set_velocity(right_motor, ((double) appliedSpeed[RIGHT])*MAX_SPEED_WEB/MAX_SPEED_EPUCK);
}
\ No newline at end of file
......@@ -126,7 +126,7 @@ static Measurement* process_received_ping_messages(char* robot_name)
element->relativePosition[THETA] = theta; // relative x pos
element->relativePosition[DISTANCE] = range; // relative y pos
element->weight = 1;
element->weight = 4;
if(utilities_debug()) printf("%d: ",receivedId);
......
......@@ -13,7 +13,7 @@ void utilities_changeDebugState(int enabledDebugTemp){
}
int utilities_debug(){
return ((numberPrint%10 == 0) && enabledDebug);
return ((numberPrint%50 == 0) && enabledDebug);
}
......
......@@ -9,7 +9,7 @@ viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 2 "controllers/ctrl1/ctrl1.c" "../../../../../Applications/Webots.app/projects/default/controllers/braitenberg/braitenberg.c" "controllers/ctrl1/inc/utilities.h"
renderingDevicePerspectives: epuck0:camera;1;1;0;0
renderingDevicePerspectives: epuck4:camera;1;1;0;0
renderingDevicePerspectives: epuck2:camera;1;1;0;0
renderingDevicePerspectives: epuck1:camera;1;1;0;0
renderingDevicePerspectives: epuck4:camera;1;1;0;0
renderingDevicePerspectives: epuck3:camera;1;1;0;0
renderingDevicePerspectives: epuck1:camera;1;1;0;0
......@@ -8,8 +8,8 @@ WorldInfo {
basicTimeStep 16
}
Viewpoint {
orientation -0.9931345111565514 -0.11262694456245013 -0.03160718444222321 1.03351
position -1.6585415096424505 5.666309726373233 3.637095016590059
orientation -0.9995612401798605 -0.02948766436304838 -0.002793703726497503 1.5969407859670073
position -0.5420695088824684 6.544208424848971 0.4480437997906027
}
PointLight {
intensity 0.5
......@@ -174,6 +174,31 @@ DEF rock Solid {
size 0.15 0.2 0.15
}
}
DEF rock Solid {
translation -0.923761 0 -0.00839935
rotation 0 1 0 5.02662
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 1 1
}
texture ImageTexture {
url [
"textures/stone-2.png"
]
}
}
geometry Box {
size 0.15 0.2 0.15
}
}
]
name "short brick(4)"
boundingObject DEF U_END_BOX Box {
size 0.15 0.2 0.15
}
}
DEF rock Solid {
translation -1.38774 0 -1.06532
rotation 0 -1 0 2.05704
......@@ -224,6 +249,81 @@ DEF rock Solid {
size 0.05 0.2 0.05
}
}
DEF rock Solid {
translation 1.09475 0 0.775884
rotation 0 -1 0 2.05704
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 1 1
}
texture ImageTexture {
url [
"textures/stone-2.png"
]
}
}
geometry Box {
size 0.05 0.2 0.05
}
}
]
name "short brick(8)"
boundingObject DEF U_END_BOX Box {
size 0.05 0.2 0.05
}
}
DEF rock Solid {
translation 0.617345 0 0.523538
rotation 0 -1 0 2.05704
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 1 1
}
texture ImageTexture {
url [
"textures/stone-2.png"
]
}
}
geometry Box {
size 0.05 0.2 0.05
}
}
]
name "short brick(9)"
boundingObject DEF U_END_BOX Box {
size 0.05 0.2 0.05
}
}
DEF rock Solid {
translation 0.687841 0 0.176218
rotation 0 -1 0 2.05704
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 1 1
}
texture ImageTexture {
url [
"textures/stone-2.png"
]
}
}
geometry Box {
size 0.05 0.2 0.05
}
}
]
name "short brick(7)"
boundingObject DEF U_END_BOX Box {
size 0.05 0.2 0.05
}
}
DEF rock Solid {
translation -1.53541 0 0.16115
rotation 0 -1 0 2.05704
......@@ -249,6 +349,31 @@ DEF rock Solid {
size 0.05 0.2 0.05
}
}
DEF rock Solid {
translation -1.82049 0 0.70045
rotation 0 -1 0 2.05704
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 1 1
}
texture ImageTexture {
url [
"textures/stone-2.png"
]
}
}
geometry Box {
size 0.05 0.2 0.05
}
}
]
name "short brick(2)"
boundingObject DEF U_END_BOX Box {
size 0.05 0.2 0.05
}
}
DEF rock Solid {
translation -1.34035 0 0.776227
rotation 0 -1 0 2.05704
......@@ -274,6 +399,31 @@ DEF rock Solid {
size 0.05 0.2 0.05
}
}
DEF rock Solid {
translation -1.11138 0 0.343018
rotation 0 -1 0 2.05704
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 1 1
}
texture ImageTexture {
url [
"textures/stone-2.png"
]
}
}
geometry Box {
size 0.05 0.2 0.05
}
}
]
name "short brick(3)"
boundingObject DEF U_END_BOX Box {
size 0.05 0.2 0.05
}
}
DEF rock Solid {
translation -2.34293 0 -0.0741107
rotation 0 -1 0 2.05704
......@@ -397,6 +547,30 @@ DEF rock Solid {
size 0.15 0.2 0.15
}
}
DEF rock Solid {
translation -0.905547 0 0.846635
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 1 1
}
texture ImageTexture {
url [
"textures/stone-2.png"
]
}
}
geometry Box {
size 0.15 0.2 0.15
}
}
]
name "short brick(5)"
boundingObject DEF U_END_BOX Box {
size 0.15 0.2 0.15
}
}
DEF rock Solid {
translation 1.91494 0 0.107609
children [
......@@ -445,6 +619,30 @@ DEF rock Solid {
size 0.15 0.2 0.15
}
}
DEF rock Solid {
translation -0.191978 0 0.392288
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 1 1
}
texture ImageTexture {
url [
"textures/stone-2.png"
]
}
}
geometry Box {
size 0.15 0.2 0.15
}
}
]
name "short brick(6)"
boundingObject DEF U_END_BOX Box {
size 0.15 0.2 0.15
}
}
DEF rock Solid {
translation -1.05597 -1.77636e-15 1.72366
children [
......@@ -493,6 +691,54 @@ DEF rock Solid {
size 0.15 0.2 0.37
}
}
DEF rock Solid {
translation -1.95487 0 0.243985
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 1 1
}
texture ImageTexture {
url [
"textures/stone-2.png"
]
}
}
geometry Box {
size 0.15 0.2 0.37
}
}
]
name "short brick(1)"
boundingObject DEF U_END_BOX Box {
size 0.15 0.2 0.37
}
}
DEF rock Solid {
translation 0.30513 0 0.243985
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 1 1
}
texture ImageTexture {
url [
"textures/stone-2.png"
]
}
}
geometry Box {
size 0.15 0.2 0.37
}
}
]
name "short brick(10)"
boundingObject DEF U_END_BOX Box {
size 0.15 0.2 0.37
}
}
DEF rock Solid {
translation -1.20749 0 -0.478731
rotation 0 -1 0 2.05704
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment