Commit 504ac673 authored by Nicolas Peslerbe's avatar Nicolas Peslerbe

Adding motors lib + improvements, compiles and run

parent a11811be
File deleted
<?xml version="1.0" encoding="UTF-8"?>
<module type="JAVA_MODULE" version="4">
<component name="NewModuleRootManager" inherit-compiler-output="true">
<exclude-output />
<content url="file://$MODULE_DIR$" />
<orderEntry type="inheritedJdk" />
<orderEntry type="sourceFolder" forTests="false" />
</component>
</module>
\ No newline at end of file
<component name="ProjectCodeStyleConfiguration">
<state>
<option name="PREFERRED_PROJECT_CODE_STYLE" value="Default" />
</state>
</component>
\ No newline at end of file
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="JavaScriptSettings">
<option name="languageLevel" value="ES6" />
</component>
</project>
\ No newline at end of file
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectModuleManager">
<modules>
<module fileurl="file://$PROJECT_DIR$/.idea/Project.iml" filepath="$PROJECT_DIR$/.idea/Project.iml" />
</modules>
</component>
</project>
\ No newline at end of file
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="VcsDirectoryMappings">
<mapping directory="" vcs="Git" />
</component>
</project>
\ No newline at end of file
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ChangeListManager">
<list default="true" id="d9bcd0a7-f75a-4f20-942a-8a93013b7509" name="Default Changelist" comment="">
<change afterPath="$PROJECT_DIR$/controllers/ctrl1/inc/infrared.h" afterDir="false" />
<change afterPath="$PROJECT_DIR$/controllers/ctrl1/inc/motors.h" afterDir="false" />
<change afterPath="$PROJECT_DIR$/controllers/ctrl1/motors.c" afterDir="false" />
<change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
<change beforePath="$PROJECT_DIR$/controllers/ctrl1/Makefile" beforeDir="false" afterPath="$PROJECT_DIR$/controllers/ctrl1/Makefile" afterDir="false" />
<change beforePath="$PROJECT_DIR$/controllers/ctrl1/build/release/ctrl1.d" beforeDir="false" afterPath="$PROJECT_DIR$/controllers/ctrl1/build/release/ctrl1.d" afterDir="false" />
<change beforePath="$PROJECT_DIR$/controllers/ctrl1/ctrl1.c" beforeDir="false" afterPath="$PROJECT_DIR$/controllers/ctrl1/ctrl1.c" afterDir="false" />
<change beforePath="$PROJECT_DIR$/controllers/ctrl1/infrared.c" beforeDir="false" afterPath="$PROJECT_DIR$/controllers/ctrl1/infrared.c" afterDir="false" />
<change beforePath="$PROJECT_DIR$/controllers/ctrl1/infrared.h" beforeDir="false" />
<change beforePath="$PROJECT_DIR$/controllers/ctrl1/laplace.c" beforeDir="false" afterPath="$PROJECT_DIR$/controllers/ctrl1/laplace.c" afterDir="false" />
<change beforePath="$PROJECT_DIR$/controllers/ctrl1/laplace.h" beforeDir="false" afterPath="$PROJECT_DIR$/controllers/ctrl1/inc/laplace.h" afterDir="false" />
<change beforePath="$PROJECT_DIR$/controllers/ctrl1/measurements.h" beforeDir="false" afterPath="$PROJECT_DIR$/controllers/ctrl1/inc/measurements.h" afterDir="false" />
<change beforePath="$PROJECT_DIR$/controllers/ctrl1/rangeAndBearing.c" beforeDir="false" afterPath="$PROJECT_DIR$/controllers/ctrl1/rangeAndBearing.c" afterDir="false" />
<change beforePath="$PROJECT_DIR$/controllers/ctrl1/rangeAndBearing.h" beforeDir="false" afterPath="$PROJECT_DIR$/controllers/ctrl1/inc/rangeAndBearing.h" afterDir="false" />
</list>
<option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
<option name="SHOW_DIALOG" value="false" />
<option name="HIGHLIGHT_CONFLICTS" value="true" />
<option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
<option name="LAST_RESOLUTION" value="IGNORE" />
</component>
<component name="CodeStyleSettingsInfer">
<option name="done" value="true" />
</component>
<component name="Git.Settings">
<option name="RECENT_GIT_ROOT_PATH" value="$PROJECT_DIR$" />
</component>
<component name="ProjectCodeStyleSettingsMigration">
<option name="version" value="1" />
</component>
<component name="ProjectId" id="1UQv71DgzkdiOxmtUnr0nTA47QM" />
<component name="PropertiesComponent">
<property name="SHARE_PROJECT_CONFIGURATION_FILES" value="true" />
<property name="WebServerToolWindowFactoryState" value="false" />
<property name="aspect.path.notification.shown" value="true" />
<property name="last_opened_file_path" value="$PROJECT_DIR$/controllers/commonSrc" />
</component>
<component name="RecentsManager">
<key name="CopyFile.RECENT_KEYS">
<recent name="$PROJECT_DIR$/controllers/commonSrc" />
</key>
<key name="MoveFile.RECENT_KEYS">
<recent name="$PROJECT_DIR$/controllers/ctrl1/inc" />
<recent name="$PROJECT_DIR$/controllers/ctrl1" />
<recent name="$PROJECT_DIR$/controllers/commonSrc" />
<recent name="$PROJECT_DIR$/controllers" />
</key>
</component>
<component name="RunDashboard">
<option name="ruleStates">
<list>
<RuleState>
<option name="name" value="ConfigurationTypeDashboardGroupingRule" />
</RuleState>
<RuleState>
<option name="name" value="StatusDashboardGroupingRule" />
</RuleState>
</list>
</option>
</component>
<component name="SvnConfiguration">
<configuration />
</component>
<component name="TaskManager">
<task active="true" id="Default" summary="Default task">
<changelist id="d9bcd0a7-f75a-4f20-942a-8a93013b7509" name="Default Changelist" comment="" />
<created>1575301462899</created>
<option name="number" value="Default" />
<option name="presentableId" value="Default" />
<updated>1575301462899</updated>
<workItem from="1575301466330" duration="12211000" />
</task>
<servers />
</component>
<component name="TypeScriptGeneratedFilesManager">
<option name="version" value="1" />
</component>
<component name="Vcs.Log.Tabs.Properties">
<option name="TAB_STATES">
<map>
<entry key="MAIN">
<value>
<State />
</value>
</entry>
</map>
</option>
</component>
</project>
\ No newline at end of file
# 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
build/release/ctrl1.o: ctrl1.c inc/laplace.h inc/measurements.h \
inc/infrared.h inc/motors.h inc/rangeAndBearing.h \
/Applications/Webots.app/include/controller/c/webots/robot.h \
/Applications/Webots.app/include/controller/c/webots/types.h \
/Applications/Webots.app/include/controller/c/webots/nodes.h \
/Applications/Webots.app/include/controller/c/webots/motor.h \
/Applications/Webots.app/include/controller/c/webots/differential_wheels.h \
/Applications/Webots.app/include/controller/c/webots/distance_sensor.h \
/Applications/Webots.app/include/controller/c/webots/emitter.h \
/Applications/Webots.app/include/controller/c/webots/receiver.h
build/release/infrared.o: infrared.c inc/infrared.h inc/measurements.h \
/Applications/Webots.app/include/controller/c/webots/robot.h \
/Applications/Webots.app/include/controller/c/webots/types.h \
/Applications/Webots.app/include/controller/c/webots/nodes.h \
/Applications/Webots.app/include/controller/c/webots/distance_sensor.h
build/release/laplace.o: laplace.c inc/laplace.h inc/measurements.h
build/release/motors.o: motors.c inc/motors.h inc/measurements.h \
/Applications/Webots.app/include/controller/c/webots/robot.h \
/Applications/Webots.app/include/controller/c/webots/types.h \
/Applications/Webots.app/include/controller/c/webots/nodes.h \
/Applications/Webots.app/include/controller/c/webots/motor.h \
/Applications/Webots.app/include/controller/c/webots/differential_wheels.h
build/release/rangeAndBearing.o: rangeAndBearing.c inc/rangeAndBearing.h \
inc/measurements.h \
/Applications/Webots.app/include/controller/c/webots/emitter.h \
/Applications/Webots.app/include/controller/c/webots/types.h \
/Applications/Webots.app/include/controller/c/webots/receiver.h \
/Applications/Webots.app/include/controller/c/webots/robot.h \
/Applications/Webots.app/include/controller/c/webots/nodes.h
/********************************************************************************/
/* 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)