Commit 83b40fa7 authored by Nicolas Peslerbe's avatar Nicolas Peslerbe

WORKING WORKING

parent 2922f911
No preview for this file type
......@@ -105,7 +105,7 @@ int main(){
// 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]);
if(!obstacle) migratory(my_speed_tmp, my_position);
migratory(my_speed_tmp, my_position);
// change speed in inital referential to current referential
utilities_transformReferential(my_speed_tmp, my_speed, my_position[2]); //from R0 to Rt
......
......@@ -25,8 +25,9 @@ void deadReckoning_updateSelfPosition(float* my_position, int* applied_speed){
float dtheta = (dr - dl)/AXLE_LENGTH;
// Compute deltas in the environment
float dx = du * cosf(dtheta);
float dy = du * sinf(dtheta);
float dx = du * cosf(my_position[2]+dtheta/2);
float dy = du * sinf(my_position[2]+dtheta/2);
// Update position ??? maybe add time step???
my_position[0] += dx;
......
......@@ -13,6 +13,7 @@ struct Measurement_t{
int robotID; // -1 if not a robot
float relativePosition[2];
float biasPosition[2]; // position in the graph of the robot
float weight;
Measurement* next;
};
......
......@@ -43,11 +43,10 @@ void infrared_init(){
Measurement* infrared_updateList(Measurement* measurementList, int* obstacle){
double intensity;
for(int i=0;i<NB_SENSORS;i++)
{
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 (intensity > DISTANCE_THRESHOLD){
......@@ -56,22 +55,22 @@ Measurement* infrared_updateList(Measurement* measurementList, int* obstacle){
bool isRobot = false;
if(measurementList){ // BUG ifno element in list
/*if(measurementList){ // BUG ifno element in list
Measurement* element = measurementList;
do{
// Check if element overlaps
/*if( element->robotID != -1 &&
if( element->robotID != -1 &&
element->relativePosition[DISTANCE] < 0.12 &&
( element->relativePosition[THETA] > theta - EPS ||
element->relativePosition[THETA] < theta + EPS ) ){
isRobot = true;
break;
}*/
}
element = element->next;
}while(element != NULL);
}
if(isRobot) continue;
if(isRobot) continue;*/
*obstacle = 1;
......@@ -87,6 +86,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;
else
temp->weight = 10;
if(utilities_debug()) printf("Object detected: distance = %f, theta = %f\n", temp->relativePosition[DISTANCE], temp->relativePosition[THETA]);
measurementList = temp;
......
......@@ -146,9 +146,9 @@ void laplace_compute(Measurement* objectsInSurround, float* newSpeed, float* wei
while(nextObject){
if (nextObject->robotID != -1)
weightsMatrix[j*numberOfObjects+j] = fabs(weights[0]);
weightsMatrix[j*numberOfObjects+j] = fabs(nextObject->weight);
else{
weightsMatrix[j*numberOfObjects+j] = -fabs(weights[1]);
weightsMatrix[j*numberOfObjects+j] = -fabs(nextObject->weight);
obstacleInSurround = 1;
}
nextObject = nextObject->next;
......@@ -186,8 +186,8 @@ void laplace_compute(Measurement* objectsInSurround, float* newSpeed, float* wei
float bias_trans[2];
nextObject->biasPosition[X] /= 10;
nextObject->biasPosition[Y] /= 10;
nextObject->biasPosition[X] /= 12;
nextObject->biasPosition[Y] /= 12;
utilities_transformReferential(nextObject->biasPosition, bias_trans, my_position[2]);
// if it does not work like that, maybe try to rotate the bias position to get right referential (orientation)
......
......@@ -126,6 +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;
if(utilities_debug()) printf("%d: ",receivedId);
......
Webots Project File version R2019b
perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000008700ffffff0000000100000236000002cbfc0200000001fb0000001400540065007800740045006400690074006f00720100000000000002cb0000008b00ffffff000000030000078000000163fc0100000001fb0000000e0043006f006e0073006f006c00650100000000000007800000008700ffffff00000548000002cb00000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff00000001000000020000016b000007750100000002010000000101
sceneTreePerspectives: 000000ff0000000100000002000000c0000000fa0100000002010000000201
maximizedDockId: -1
centralWidgetVisible: 1
selectionDisabled: 0
viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 0 "controllers/ctrl1/ctrl1.c"
renderingDevicePerspectives: epuck6:camera;1;1;0;0
renderingDevicePerspectives: epuck0:camera;1;1;0;0
renderingDevicePerspectives: epuck7:camera;1;1;0;0
renderingDevicePerspectives: epuck8:camera;1;1;0;0
renderingDevicePerspectives: epuck2:camera;1;1;0;0
renderingDevicePerspectives: epuck9:camera;1;1;0;0
renderingDevicePerspectives: epuck3:camera;1;1;0;0
renderingDevicePerspectives: epuck4:camera;1;1;0;0
renderingDevicePerspectives: epuck1:camera;1;1;0;0
renderingDevicePerspectives: epuck5:camera;1;1;0;0
Webots Project File version R2019b
perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000008700ffffff0000000100000236000001d5fc0200000001fb0000001400540065007800740045006400690074006f00720100000000000001d50000008b00ffffff000000030000078000000259fc0100000001fb0000000e0043006f006e0073006f006c00650100000000000007800000008700ffffff00000548000001d500000001000000020000000100000008fc00000000
perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000008700ffffff0000000100000236000002cbfc0200000001fb0000001400540065007800740045006400690074006f00720100000000000002cb0000008b00ffffff000000030000078000000163fc0100000001fb0000000e0043006f006e0073006f006c00650100000000000007800000008700ffffff00000548000002cb00000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff00000001000000020000016b000007750100000002010000000101
sceneTreePerspectives: 000000ff0000000100000002000000c0000000fa0100000002010000000201
maximizedDockId: -1
......@@ -10,13 +10,13 @@ selectionDisabled: 0
viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 0 "controllers/ctrl1/ctrl1.c"
renderingDevicePerspectives: epuck9:camera;1;1;0;0
renderingDevicePerspectives: epuck8:camera;1;1;0;0
renderingDevicePerspectives: epuck6:camera;1;1;0;0
renderingDevicePerspectives: epuck8:camera;1;1;0;0
renderingDevicePerspectives: epuck0:camera;1;1;0;0
renderingDevicePerspectives: epuck4:camera;1;1;0;0
renderingDevicePerspectives: epuck7:camera;1;1;0;0
renderingDevicePerspectives: epuck9:camera;1;1;0;0
renderingDevicePerspectives: epuck2: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
renderingDevicePerspectives: epuck5:camera;1;1;0;0
renderingDevicePerspectives: epuck3:camera;1;1;0;0
renderingDevicePerspectives: epuck7:camera;1;1;0;0
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