Commit da846cd3 authored by Nicolas Peslerbe's avatar Nicolas Peslerbe

Adding motors lib + improvements, compiles and run

parent 504ac673
File added
.idea/
build/
# Copyright 1996-2019 Cyberbotics Ltd.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
### Generic Makefile.include for Webots controllers, physics plugins, robot
### window libraries, remote control libraries and other libraries
### to be used with GNU make
###
### Platforms: Windows, macOS, Linux
### Languages: C, C++
###
### Authors: Olivier Michel, Yvan Bourquin, Fabien Rohrer
### Edmund Ronald, Sergei Poskriakov
###
###-----------------------------------------------------------------------------
###
### This file is meant to be included from the Makefile files located in the
### Webots projects subdirectories. It is possible to set a number of variables
### to customize the build process, i.e., add source files, compilation flags,
### include paths, libraries, etc. These variables should be set in your local
### Makefile just before including this Makefile.include. This Makefile.include
### should never be modified.
###
### Here is a description of the variables you may set in your local Makefile:
###
### ---- C Sources ----
### if your program uses several C source files:
### C_SOURCES = my_plugin.c my_clever_algo.c my_graphics.c
###
### ---- C++ Sources ----
### if your program uses several C++ source files:
### CXX_SOURCES = my_plugin.cc my_clever_algo.cpp my_graphics.c++
###
### ---- Compilation options ----
### if special compilation flags are necessary:
### CFLAGS = -Wno-unused-result
###
### ---- Linked libraries ----
### if your program needs additional libraries:
### INCLUDE = -I"/my_library_path/include"
### LIBRARIES = -L"/path/to/my/library" -lmy_library -lmy_other_library
###
### ---- Linking options ----
### if special linking flags are needed:
### LFLAGS = -s
###
### ---- Webots included libraries ----
### if you want to use the Webots C API in your C++ controller program:
### USE_C_API = true
### if you want to link with the Qt framework embedded in Webots:
### QT = core gui widgets network
###
### ---- Debug mode ----
### if you want to display the gcc command line for compilation and link, as
### well as the rm command details used for cleaning:
### VERBOSE = 1
###
###-----------------------------------------------------------------------------
C_SOURCES = ctrl1.c laplace.c infrared.c rangeAndBearing.c motors.c
### Do not modify: this includes Webots global Makefile.include
space :=
space +=
WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME))))
include $(WEBOTS_HOME_PATH)/resources/Makefile.include
/********************************************************************************/
/* File: ctrl1.c */
/* Version: 1.0 */
/* Date: 1-Dec-19 */
/* Description: Controller for epucks */
/* */
/* Author: Hugo Aguettaz, Marie-Joe Stoeri & Nicolas Peslerbe */
/* Last revision:12-Oct-15 by Florian Maushart */
/********************************************************************************/
#include "inc/laplace.h"
#include "inc/infrared.h"
#include "inc/motors.h"
#include "inc/rangeAndBearing.h"
#include <webots/robot.h>
#include <webots/motor.h>
#include <webots/differential_wheels.h>
#include <webots/distance_sensor.h>
#include <webots/emitter.h>
#include <webots/receiver.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
// ******** SETTINGS PARAMETERS
#define USE_SUPERVISOR false
#define FLOCK_SIZE 5
#define NEIGHBOR_BOUND
#define TIME_STEP 64 // [ms] Length of time step
// ********
#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;
static void reset(){
wb_robot_init();
robot_name=(char*) wb_robot_get_name();
sscanf(robot_name,"epuck%d",&robot_id_u);
robot_id = robot_id_u%FLOCK_SIZE;
printf("Reset: robot %d\n",robot_id_u);
rangeAndBearing_init();
infrared_init();
motors_init();
}
int main(){
reset(); // Resetting the robot
float new_speed[2] = {0,0};
for(;;){
Measurement* list = rangeAndBearing_getRelativeRobotPositions(robot_name);
infrared_updateList(list);
laplace_compute(list, new_speed);
motors_applySpeed(new_speed);
wb_robot_step(TIME_STEP);
}
}
#ifndef __INFRARED_H__
#define __INFRARED_H__
#include "measurements.h"
void infrared_init();
void infrared_updateList(Measurement* measurementList);
#endif
\ No newline at end of file
#ifndef __LAPLACE_H__
#define __LAPLACE_H__
#include "measurements.h"
/**
* Compute laplacian value for GraphBased method
* Parameters:
* - objectsInSurround: Measurements of objects to take in account in while computing the graph (neighborhood only)
* - velTot: Velocity value for x and y
* Returns:
* --
*/
void laplace_compute(Measurement* objectsInSurround, float* velTot);
#endif
#ifndef MEASUREMENTS
#define MEASUREMENTS
#define X 0
#define Y 1
typedef struct Measurement_t Measurement;
struct Measurement_t{
int robotID; // -1 if not a robot
int relativePosition[2];
Measurement* next;
};
#endif
\ No newline at end of file
#ifndef __MOTORS_H__
#define __MOTORS_H__
#include "measurements.h"
void motors_init();
void motors_applySpeed();
#endif
\ No newline at end of file
#ifndef __RANGE_AND_BEARING_H__
#define __RANGE_AND_BEARING_H__
#include "measurements.h"
/**
* Manage communication with other robots
* Parameters:
* - The robot name
* Returns:
* - The mesurments list with robot IDs
*/
Measurement* rangeAndBearing_getRelativeRobotPositions();
void rangeAndBearing_init();
#endif
\ No newline at end of file
/********************************************************************************/
/* File: infrared.c */
/* Date: Dec-19 */
/* Description: Infrared lib - DIS Project */
/* */
/* Author: Hugo Aguettaz, Marie-Joe Stoeri & Nicolas Peslerbe */
/********************************************************************************/
/***********************
* Inclusions
*/
#include "inc/infrared.h"
#include <webots/robot.h>
#include <webots/distance_sensor.h>
#include <stdlib.h>
#include <stdio.h>
#define NB_SENSORS 8
static WbDeviceTag ds[NB_SENSORS];
void infrared_init(){
char s[4]="ps0";
for(int i=0; i<NB_SENSORS;i++) {
ds[i] = wb_robot_get_device(s); // the device name is specified in the world file
s[2]++; // increases the device number
}
for(int i=0;i<NB_SENSORS;i++)
wb_distance_sensor_enable(ds[i],64);
}
void infrared_updateList(Measurement* measurementList){
int distances[NB_SENSORS];
for(int i=0;i<NB_SENSORS;i++)
{
distances[i] = wb_distance_sensor_get_value(ds[i]);
}
}
\ No newline at end of file
/********************************************************************************/
/* File: laplace.c */
/* Date: Dec-19 */
/* Description: Fopr graph based technique use - DIS Project */
/* */
/* Author: Hugo Aguettaz, Marie-Joe Stoeri & Nicolas Peslerbe */
/********************************************************************************/
/***********************
* Inclusions
*/
#include "inc/laplace.h"
#include <stdlib.h>
#include <stdio.h>
/***********************
* Local constants definition
*/
#define PRINT_MATRIX 0
//#define IS_TEST
#define ALPHA_FREE 0.5
#define ALPHA_OBST 0.7
/***********************
* Local prototype
*/
/**
* Function to transpose a matrix
*
* Input pramaters:
* - input: A memory emplacement of size n*m
* - output: A memory emplacement of size m*n
* - n: Number of rows
* - m: Number of colums
*/
void transpose(float* input, float* output, int m, int n);
/**
* Function to do the product of two matrix
*
* Input pramaters:
* - mat1: A memory emplacement of size n*m
* - mat2: A memory emplacement of size m*p
* - res: A memory emplacement of size n*p
* - m: Number of rows mat1
* - n: Number of colums mat1 and rows mat2
* - p: Number of colums mat2
*/
void multiply(float* mat1, float* mat2, float* res, int m, int n, int p);
/**
* Print a matrix on stdo
*
* Input pramaters:
* - name: A name for the matrix
* - matrix: The matrix to print
* - m: Number of rows
* - n: Number of columns
*/
void printMatrix(char* name, float* matrix, int m, int n);
/***********************
* Methods implementation
*/
void transpose(float* input, float* output, int m, int n)
{
for(int i=0; i<m; i++)
for(int j=0; j<n; j++)
{
output[j*m+i] = input[i*n+j];
}
}
void multiply(float* mat1, float* mat2, float* res, int m, int n, int p)
{
int i, j, k;
for (i = 0; i < m; i++)
{
for (j = 0; j < p; j++)
{
res[i*p+j] = 0;
for (k = 0; k < n; k++)
res[i*p+j] += mat1[i*n+k]*mat2[k*p+j];
}
}
}
void printMatrix(char* name, float* matrix, int m, int n){
if(!PRINT_MATRIX) return;
printf("Matrix %s (%d * %d)\n", name, m, n);
for(int i= 0; i<m; i++){
for(int j= 0; j<n; j++){
printf("%f\t", matrix[i*n+j]);
}
printf("\n");
}
}
void laplace_compute(Measurement* objectsInSurround, float* velTot){
// 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;
Measurement* element = objectsInSurround;
while(element != NULL){
numberOfObjects++;
element = element->next;
}
float incidenceMatrix[(numberOfObjects+1) * numberOfObjects];
for(int i = 0; i < (numberOfObjects+1) * numberOfObjects; i++)
incidenceMatrix[i] = 0.;
float weightsMatrix[numberOfObjects*numberOfObjects];
for(int i = 0; i < numberOfObjects * numberOfObjects; i++)
weightsMatrix[i] = 0.;
// Compute incidence matrix of size (num_neighbors+1 x num_neighbors)
for (int j=0; j<numberOfObjects; j++)
{
incidenceMatrix[j] = -1;
incidenceMatrix[(j+1)*numberOfObjects+j] = 1;
}
printMatrix("incidenceMatrix", incidenceMatrix, numberOfObjects+1, numberOfObjects);
int obstacleInSurround = 0;
{
Measurement* nextObject = objectsInSurround;
int j = 0;
while(nextObject){
if (nextObject->robotID != -1)
weightsMatrix[j*numberOfObjects+j] = 1;
else{
weightsMatrix[j*numberOfObjects+j] = -2; // more importance to avoid obstacle
obstacleInSurround = 1;
}
nextObject = nextObject->next;
j++;
}
}
printMatrix("weightsMatrix", weightsMatrix, numberOfObjects, numberOfObjects);
float alpha = (obstacleInSurround ? ALPHA_OBST : ALPHA_FREE);
float incidenceMatrixT[numberOfObjects * (numberOfObjects+1)];
transpose(incidenceMatrix, incidenceMatrixT, numberOfObjects+1, numberOfObjects);
printMatrix("incidenceMatrixT", incidenceMatrixT, numberOfObjects, (numberOfObjects+1));
float IW[(numberOfObjects+1) * numberOfObjects];
multiply(incidenceMatrix, weightsMatrix, IW, numberOfObjects+1, numberOfObjects, numberOfObjects);
printMatrix("IW", IW, numberOfObjects+1, numberOfObjects);
float L[(numberOfObjects+1) * (numberOfObjects+1)];
multiply(IW, incidenceMatrixT, L, numberOfObjects+1, numberOfObjects, numberOfObjects+1);
printMatrix("L", L, numberOfObjects+1, numberOfObjects+1);
float vel_graph[2] = {0, 0};
// Compute velocity of current robot with v = -L(x-B)
{
Measurement* nextObject = objectsInSurround;
int i = 0;
while(nextObject){
vel_graph[X] += -L[0] * nextObject->relativePosition[X];
vel_graph[Y] += -L[0] * nextObject->relativePosition[Y];
nextObject = nextObject->next;
i++;
}
}
float vel_migr[2] = {5, 0};
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];
}
/***********************
* Test mathods - Usable using IS_TEST constant
*/
#ifdef IS_TEST
int main(){
float velTot[2];
Measurement* robotsMeasurements = malloc(sizeof(Measurement));
robotsMeasurements[0] = malloc(sizeof(Measurement));
robotsMeasurements[0].robotID = 1;
robotsMeasurements[0].relativePosition[X] = 0.;
robotsMeasurements[0].relativePosition[Y] = 0.;
robotsMeasurements[1].robotID = 2;
robotsMeasurements[1].relativePosition[X] = 0.;
robotsMeasurements[1].relativePosition[Y] = 1.;
robotsMeasurements[2].robotID = 3;
robotsMeasurements[2].relativePosition[X] = 1.;
robotsMeasurements[2].relativePosition[Y] = 1.;
robotsMeasurements[3].robotID = -1;
robotsMeasurements[3].relativePosition[X] = 1.;
robotsMeasurements[3].relativePosition[Y] = 0.;
robotsMeasurements[4].robotID = -1;
robotsMeasurements[4].relativePosition[X] = 1.;
robotsMeasurements[4].relativePosition[Y] = 2.;
laplacian(robotsMeasurements, 5, velTot);
printf("Output: %f %f\n", velTot[X], velTot[Y]);
return 0;
}
#endif
\ No newline at end of file
/********************************************************************************/
/* 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>
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);
}
void motors_applySpeed(float* speed){
wb_motor_set_position(left_motor, speed[X]);
wb_motor_set_position(right_motor, speed[Y]);
}
\ No newline at end of file
/********************************************************************************/
/* File: rangeAndBearing.c */
/* Date: Dec-19 */
/* Description: Range and bearing lib - DIS Project */
/* */
/* Author: Hugo Aguettaz, Marie-Joe Stoeri & Nicolas Peslerbe */
/********************************************************************************/
/***********************
* Inclusions
*/
#include "inc/rangeAndBearing.h"
#include <stdlib.h>
#include <webots/emitter.h>
#include <webots/receiver.h>
#include <webots/robot.h>
#include <string.h>
#include <stdio.h>
#include <math.h>
/***********************
* Local prototype
*/
/**
* Send a ping to other robots
* Parameters:
* - The robot name
* Returns:
* --
*/
static void send_ping(char* robot_name);
/**
* Compute relative distant of other robots and add it to a linked list
* Parameters:
* --
* Returns:
* The measurements linkedlist.
*/
static Measurement* process_received_ping_messages();