Commit 56da1b07 authored by Nicolas Peslerbe's avatar Nicolas Peslerbe

Modifications

parent 7f2e2079
No preview for this file type
......@@ -66,7 +66,7 @@
### VERBOSE = 1
###
###-----------------------------------------------------------------------------
C_SOURCES = ctrl1.c laplace.c infrared.c rangeAndBearing.c motors.c
C_SOURCES = ctrl1.c laplace.c infrared.c rangeAndBearing.c motors.c deadReckoning.c utilities.c
### Do not modify: this includes Webots global Makefile.include
space :=
space +=
......
No preview for this file type
......@@ -13,6 +13,8 @@
#include "inc/infrared.h"
#include "inc/motors.h"
#include "inc/rangeAndBearing.h"
#include "inc/deadReckoning.h"
#include "inc/utilities.h"
#include <webots/robot.h>
#include <webots/motor.h>
......@@ -28,10 +30,11 @@
// ******** SETTINGS PARAMETERS
// ******** SETTINGS PARAMETERS
#define USE_SUPERVISOR false
#define FLOCK_SIZE 5
#define NEIGHBOR_BOUND
#define ROBOT_TO_MONITOR 0
#define TIME_STEP 64 // [ms] Length of time step
......@@ -39,14 +42,6 @@
// ********
#define NB_SENSORS 8 // Number of distance sensors
/* Harware elements declaration */
static WbDeviceTag left_motor;
static WbDeviceTag right_motor;
static WbDeviceTag ds[NB_SENSORS];
static WbDeviceTag receiver;
static WbDeviceTag emitter;
static char* robot_name;
static int robot_id_u, robot_id;
......@@ -54,16 +49,15 @@ static int robot_id_u, robot_id;
static void reset(){
wb_robot_init();
// Initialize dead reckoning variables at (0,0,0)
double dr_x = 0;
double dr_y = 0;
double dr_theta = 0;
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);
printf("Reset: robot %d\n",robot_id_u);
deadReckoning_init();
rangeAndBearing_init();
infrared_init();
motors_init();
......@@ -74,16 +68,21 @@ int main(){
reset(); // Resetting the robot
float new_speed[2] = {0,0};
float vel_migr[2] = {5,0};
float vel_migr[2] = {0.5,0};
float weights[2] = {1,-2};
for(;;){
numberPrint++;
if(utilities_debug()) printf("___________________\n");
Measurement* list = rangeAndBearing_getRelativeRobotPositions(robot_name);
infrared_updateList(list);
// realign robot in direction of the migratory urge
dead_reckoning(&dr_x, &dr_y, &dr_theta, vel_migr);
deadReckoning(vel_migr);
// compute new speed
laplace_compute(list, new_speed, vel_migr);
laplace_compute(list, new_speed, vel_migr, weights);
// apply speed to motors
motors_applySpeed(new_speed);
......
#include "inc/deadReckoning.h"
#include "inc/utilities.h"
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <time.h>
#define V0 0.5
void deadReckoning_init(){
}
/*
void deadReckoning(float* vel_mig){
double x;
double y;
double theta;
// update dead reckoning via motor steps since last call
deadReckoning_do_dr();
// retrieve new position
deadReckoning_get_dr_position(&x, &y, &theta);
// compute new velocity to realign according to the migratory urge
vel_mig[X] = V0 * cos(dr_theta);
vel_mig[Y] = V0 * sin(dr_theta);
if(utilities_debug()) printf("Dear reckoning: %f, %f, %f\n", x, y, theta );
}
*/
void deadReckoning(float* vel_mig){
// compute new velocity to realign according to the migratory urge
vel_mig[X] = 0.5;
vel_mig[Y] = 0;
}
void deadReckoning_updateSteps(float* speed, double time){
}
\ No newline at end of file
void dead_reckoning(double* dr_x, double* dr_y, double* dr_theta, double* vel_mig){
// update dead reckoning via motor steps since last call
e_do_dr();
// retrieve new position
e_get_dr_position(&dr_x, &dr_y, &dr_theta);
// compute new velocity to realign according to the migratory urge
vel_mig -> V0 + dr_theta
}
#ifndef __DEADRECKONING_H__
#define __DEADRECKONING_H__
#include "measurements.h"
void deadReckoning_init();
void deadReckoning(float* vel_mig);
void deadReckoning_updateSteps(float* speed, double time);
#endif
\ No newline at end of file
......@@ -11,7 +11,7 @@
* Returns:
* --
*/
void laplace_compute(Measurement* objectsInSurround, float* velTot);
void laplace_compute(Measurement* objectsInSurround, float* newSpeed, float* velMigration, float* weights);
#endif
......@@ -8,7 +8,7 @@ typedef struct Measurement_t Measurement;
struct Measurement_t{
int robotID; // -1 if not a robot
int relativePosition[2];
float relativePosition[2];
Measurement* next;
};
......
#ifndef __UTILITIES_H__
#define __UTILITIES_H__
extern int enabledDebug;
extern int numberPrint;
void utilities_changeDebugState(int enableDebugTemp);
int utilities_debug();
#endif
\ No newline at end of file
......@@ -22,6 +22,8 @@
static WbDeviceTag ds[NB_SENSORS];
bool mustPrint = false;
void infrared_init(){
char s[4]="ps0";
for(int i=0; i<NB_SENSORS;i++) {
......
......@@ -11,17 +11,19 @@
*/
#include "inc/laplace.h"
#include "inc/utilities.h"
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
/***********************
* Local constants definition
*/
#define PRINT_MATRIX 0
#define PRINT_MATRIX 1
//#define IS_TEST
#define ALPHA_FREE 0.5
#define ALPHA_OBST 0.7
#define ALPHA_FREE 0.7
#define ALPHA_OBST 0.5
/***********************
* Local prototype
......@@ -91,7 +93,7 @@ void multiply(float* mat1, float* mat2, float* res, int m, int n, int p)
}
void printMatrix(char* name, float* matrix, int m, int n){
if(!PRINT_MATRIX) return;
if(!PRINT_MATRIX || !utilities_debug()) return;
printf("Matrix %s (%d * %d)\n", name, m, n);
for(int i= 0; i<m; i++){
for(int j= 0; j<n; j++){
......@@ -101,8 +103,7 @@ void printMatrix(char* name, float* matrix, int m, int n){
}
}
void laplace_compute(Measurement* objectsInSurround, float* velTot, float* weights){
void laplace_compute(Measurement* objectsInSurround, float* newSpeed, float* velMigration, float* weights){
// Compute neighborhood by range and bearing (contains neighbors robots and obstacles)
// X contains relative positions of each neighbors (size = (num_neighbors * 2)
int numberOfObjects = 0;
......@@ -171,19 +172,20 @@ void laplace_compute(Measurement* objectsInSurround, float* velTot, float* weigh
Measurement* nextObject = objectsInSurround;
int i = 0;
while(nextObject){
vel_graph[X] += -L[0] * nextObject->relativePosition[X];
vel_graph[Y] += -L[0] * nextObject->relativePosition[Y];
vel_graph[X] += -L[i] * nextObject->relativePosition[X];
vel_graph[Y] += -L[i] * nextObject->relativePosition[Y];
nextObject = nextObject->next;
i++;
}
}
if(utilities_debug()) printf("Vel graph x = %f, y = %f\n", vel_graph[X], vel_graph[Y]);
if(utilities_debug()) printf("Vel migration x = %f, y = %f\n", velMigration[X], velMigration[Y]);
newSpeed[X] = alpha * vel_graph[X] + (1-alpha) * velMigration[X]; // tot velocity is a weighted sum of graph and migration velocities
newSpeed[Y] = alpha * vel_graph[Y] + (1-alpha) * velMigration[Y];
float vel_migr[2] = {5, 0};
if(utilities_debug()) printf("Speed x = %f, y = %f\n", newSpeed[X], newSpeed[Y]);
velTot[X] = alpha * vel_graph[X] + (1-alpha) * vel_migr[X]; // tot velocity is a weighted sum of graph and migration velocities
velTot[Y] = alpha * vel_graph[Y] + (1-alpha) * vel_migr[Y];
}
......
/********************************************************************************/
/* File: motors.c */
/* Date: Dec-19 */
/* Description: Motors lib - DIS Project */
/* */
/* Author: Hugo Aguettaz, Marie-Joe Stoeri & Nicolas Peslerbe */
/********************************************************************************/
#include "inc/motors.h"
#include <webots/robot.h>
#include <webots/motor.h>
#include <webots/differential_wheels.h>
#include <math.h>
#define KU 4
#define KW 4
#define R 0.041/2 //radius of a wheel
#define L 0.053/2 //half distance between wheels
static WbDeviceTag left_motor;
static WbDeviceTag right_motor;
void motors_init(){
left_motor = wb_robot_get_device("left wheel motor");
right_motor = wb_robot_get_device("right wheel motor");
wb_motor_set_position(left_motor, INFINITY);
wb_motor_set_position(right_motor, INFINITY);
}
// function to transform speed of the center of mass of the robot to left and right wheels speeds
void motors_transformSpeed(float* speed, float* v_right, float* v_left){
linearSpeed = KU * sqrt(pow(speed[X],2)+pow(speed[Y],2)) * cos(atan2(speed[Y],speed[X]));
rotationalSpeed = KW * atan2(speed[Y],speed[X]);
v_right* = (linearSpeed + L * rotationalSpeed)/R;
v_left* = (linearSpeed - L * rotationalSpeed)/R;
}
void motors_applySpeed(float* speed){
float* v_right;
float* v_left;
motors_transformSpeed(speed, v_right, v_left);
wb_motor_set_position(left_motor, v_left);
wb_motor_set_position(right_motor, v_right);
}
\ No newline at end of file
......@@ -7,26 +7,70 @@
/********************************************************************************/
#include "inc/motors.h"
#include "inc/utilities.h"
#include "inc/deadReckoning.h"
#include <webots/robot.h>
#include <webots/motor.h>
#include <webots/differential_wheels.h>
#include <stdio.h>
#include <math.h>
#define KU 0.2
#define KW 1
#define AXLE_LENGTH 0.052
#define WHEEL_RADIUS 0.0205
#define MAX_SPEED 1000
static WbDeviceTag left_motor;
static WbDeviceTag right_motor;
void motors_limit(int *number, int limit) {
if (*number > limit)
*number = limit;
if (*number < -limit)
*number = -limit;
}
void motors_init(){
left_motor = wb_robot_get_device("left wheel motor");
right_motor = wb_robot_get_device("right wheel motor");
wb_motor_set_position(left_motor, INFINITY);
wb_motor_set_position(right_motor, INFINITY);
wb_motor_set_position(left_motor, 0);
wb_motor_set_position(right_motor, 0);
}
void motors_transformSpeed(float* speed, int *msl, int *msr){
float linearSpeed = KU * sqrt(pow(speed[X],2)+pow(speed[Y],2)) * cos(atan2(speed[Y],speed[X]));
float rotationalSpeed = KW * atan2(speed[Y],speed[X]);
*msl = (linearSpeed - AXLE_LENGTH * rotationalSpeed/2.0) * (50 / WHEEL_RADIUS);
*msr = (linearSpeed + AXLE_LENGTH * rotationalSpeed/2.0) * (50 / WHEEL_RADIUS);
motors_limit(msl,MAX_SPEED);
motors_limit(msr,MAX_SPEED);
if(utilities_debug()) printf("Motor speed left = %d, right = %d\n", *msl, *msr);
}
int last_v_left = 0;
int last_v_right = 0;
double start_time = 0;
void motors_applySpeed(float* speed){
wb_motor_set_position(left_motor, speed[X]);
wb_motor_set_position(right_motor, speed[Y]);
int v_right;
int v_left;
motors_transformSpeed(speed, &v_right, &v_left);
deadReckoning_updateSteps(speed, wb_robot_get_time()-start_time);
last_v_left = v_left;
last_v_right = v_right;
start_time = wb_robot_get_time();
wb_motor_set_position(left_motor, v_left);
wb_motor_set_position(right_motor, v_right);
}
\ No newline at end of file
......@@ -12,17 +12,20 @@
*/
#include "inc/rangeAndBearing.h"
#include "inc/utilities.h"
#include <stdlib.h>
#include <webots/emitter.h>
#include <webots/receiver.h>
#include <webots/robot.h>
#include <webots/differential_wheels.h>
#include <webots/distance_sensor.h>
#include <string.h>
#include <stdio.h>
#include <math.h>
#define MAX_BOUNDARY 0.3
/***********************
* Local prototype
......@@ -57,13 +60,14 @@ static WbDeviceTag emitter;
void rangeAndBearing_init(){
receiver = wb_robot_get_device("receiver");
emitter = wb_robot_get_device("emitter");
receiver = wb_robot_get_device("receiver2");
emitter = wb_robot_get_device("emitter2");
wb_receiver_enable(receiver,64);
}
static void send_ping(char* robot_name)
{
char out[10];
......@@ -71,8 +75,10 @@ static void send_ping(char* robot_name)
wb_emitter_send(emitter,out,strlen(out)+1);
}
static Measurement* process_received_ping_messages()
static Measurement* process_received_ping_messages(char* robot_name)
{
int myId = (int)(robot_name[5]-'0');
int numberOfElements = 0;
Measurement* listStart = NULL;
......@@ -86,8 +92,6 @@ static Measurement* process_received_ping_messages()
while (wb_receiver_get_queue_length(receiver) > 0) {
numberOfElements++;
inbuffer = (char*) wb_receiver_get_data(receiver);
message_direction = wb_receiver_get_emitter_direction(receiver);
message_rssi = wb_receiver_get_signal_strength(receiver);
......@@ -98,24 +102,40 @@ static Measurement* process_received_ping_messages()
theta = -atan2(y,x);
range = sqrt((1/message_rssi));
if(listStart == NULL){
element = malloc(sizeof(Measurement));
listStart = element;
} else{
element->next = malloc(sizeof(Measurement));
element = element->next;
double x_rel = range*cos(theta);
double y_rel = -1.0 * range*sin(theta);
if(sqrt(x_rel*x_rel + y_rel*y_rel) < MAX_BOUNDARY){
if(listStart == NULL){
element = malloc(sizeof(Measurement));
listStart = element;
} else{
element->next = malloc(sizeof(Measurement));
element = element->next;
}
int receivedId = (int)(inbuffer[5]-'0');
if((myId < 5 && receivedId < 5) || (myId >= 5 && receivedId >= 5) ){
element->robotID = receivedId;
if(utilities_debug()) printf("%d: ",receivedId);
}
else{
element->robotID = -1;
if(utilities_debug()) printf("%d (ennemy): ",receivedId);
}
element->relativePosition[X] = x_rel; // relative x pos
element->relativePosition[Y] = y_rel; // relative y pos
element->next = NULL;
if(utilities_debug()) printf("%f, %f\n", element->relativePosition[X], element->relativePosition[Y] );
}
element->robotID = (int)(inbuffer[5]-'0');
element->relativePosition[X] = range*cos(theta); // relative x pos
element->relativePosition[Y] = -1.0 * range*sin(theta); // relative y pos
element->next = NULL;
wb_receiver_next_packet(receiver);
}
return listStart;
}
Measurement* rangeAndBearing_getRelativeRobotPositions(char* robotName){
send_ping(robotName);
return process_received_ping_messages();
return process_received_ping_messages(robotName);
}
\ No newline at end of file
#include "inc/utilities.h"
int enabledDebug = 0;
int numberPrint = 0;
void utilities_changeDebugState(int enabledDebugTemp){
enabledDebug = enabledDebugTemp;
}
int utilities_debug(){
return ((numberPrint%50 == 45) && enabledDebug);
}
\ No newline at end of file
Webots Project File version R2019b
perspectives: 000000ff00000000fd0000000300000000000001de00000433fc0100000002fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e006500540072006500650100000019000003f10000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e02000003cd0000007d000001de00000433000000010000029b000002cbfc0200000001fb0000001400540065007800740045006400690074006f00720100000000000002cb0000008b00ffffff0000000300000a0000000253fc0100000001fb0000000e0043006f006e0073006f006c0065010000000000000a000000008700ffffff00000763000002cb00000004000000040000000100000008fc00000000
simulationViewPerspectives: 000000ff000000010000000200000100000006530100000002010000000101
sceneTreePerspectives: 000000ff0000000100000002000002f3000000000100000002010000000201
perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000008700ffffff0000000100000236000002e8fc0200000001fb0000001400540065007800740045006400690074006f00720100000000000002e80000008b00ffffff0000000300000780000001abfc0100000001fb0000000e0043006f006e0073006f006c00650100000000000007800000008700ffffff00000548000002e800000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff00000001000000020000016b000007750100000002010000000101
sceneTreePerspectives: 000000ff0000000100000002000000c0000000fa0100000002010000000201
maximizedDockId: -1
centralWidgetVisible: 1
projectionMode: PERSPECTIVE
......@@ -9,14 +9,14 @@ renderingMode: PLAIN
selectionDisabled: 0
viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 2 "../TP/DIS_19-20_lab02/controllers/braiten_sln/braiten_sln.c" "controllers/ctrl1/ctrl1.c" "controllers/ctrl1/infrared.c"
renderingDevicePerspectives: epuck6:camera;1;1;0;0
textFiles: 0 "controllers/ctrl1/ctrl1.c"
renderingDevicePerspectives: epuck0:camera;1;1;0;0
renderingDevicePerspectives: epuck5:camera;1;1;0;0
renderingDevicePerspectives: epuck1: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: epuck6:camera;1;1;0;0
renderingDevicePerspectives: epuck1:camera;1;1;0;0
renderingDevicePerspectives: epuck3:camera;1;1;0;0
renderingDevicePerspectives: epuck0:camera;1;1;0;0
renderingDevicePerspectives: epuck8:camera;1;1;0;0
renderingDevicePerspectives: epuck4:camera;1;1;0;0
renderingDevicePerspectives: epuck7:camera;1;1;0;0
renderingDevicePerspectives: epuck2:camera;1;1;0;0
Webots Project File version R2019b
perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000008700ffffff000000010000011c00000245fc0200000001fb0000001400540065007800740045006400690074006f00720100000000000002450000008b00ffffff0000000300000780000001e9fc0100000001fb0000000e0043006f006e0073006f006c00650100000000000007800000008700ffffff000006620000024500000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff00000001000000020000016b000007750100000002010000000101
sceneTreePerspectives: 000000ff0000000100000002000000c0000000fa0100000002010000000201
maximizedDockId: -1
centralWidgetVisible: 1
selectionDisabled: 0
viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 1 "controllers/ctrl1/ctrl1.c" "../../../../../Applications/Webots.app/projects/default/controllers/braitenberg/braitenberg.c"
renderingDevicePerspectives: epuck0:camera;1;1;0;0
renderingDevicePerspectives: epuck1:camera;1;1;0;0
renderingDevicePerspectives: epuck3:camera;1;1;0;0
renderingDevicePerspectives: epuck4:camera;1;1;0;0
renderingDevicePerspectives: epuck2:camera;1;1;0;0
#VRML_SIM R2019b utf8
WorldInfo {
info [
"Description"
"Author: first name last name <e-mail>"
"Date: DD MMM YYYY"
]
basicTimeStep 16
}
Viewpoint {
orientation -0.9998684634686608 0.016207307512567284 0.0006155842853415571 1.15569
position -0.8941668423407272 5.032331617062606 2.470769152915888
}
PointLight {
intensity 0.5
location -1.06 0.58 -0.55
castShadows TRUE
}
PointLight {
intensity 0.5
location -1.63 0.53 -0.05
}
Background {
skyColor [
0.4 0.7 1
]
}
DEF ground Solid {
translation -1.5 0 0
children [
Shape {
appearance Appearance {
material Material {
ambientIntensity 1
diffuseColor 1 1 1
emissiveColor 0.823529 0.6 0.34902
shininess 0
specularColor 0.196078 0.196078 0
}
texture ImageTexture {
url [
"textures/lightwood.png"
]
}
textureTransform TextureTransform {
center 0.46 0
rotation -0.32
translation -1.01 0
}
}
geometry DEF ground_geo IndexedFaceSet {
coord Coordinate {
point [
-1.6 0 -1
-1.6 0 1
1.6 0 1
1.6 0 -1
]
}
texCoord TextureCoordinate {
point [
0 0
0 5
5 5
5 0
]
}
coordIndex [
0, 1, 2, 3, -1
]
texCoordIndex [
0, 1, 2, 3, -1
]
}