Commit 01bb7c1a authored by Nicolas Peslerbe's avatar Nicolas Peslerbe

Adding motors lib + improvements, compiles and run

parent 97e04e29
......@@ -2,23 +2,20 @@
<project version="4">
<component name="ChangeListManager">
<list default="true" id="d9bcd0a7-f75a-4f20-942a-8a93013b7509" name="Default Changelist" comment="">
<change afterPath="$PROJECT_DIR$/.idea/Project.iml" afterDir="false" />
<change afterPath="$PROJECT_DIR$/.idea/codeStyles/codeStyleConfig.xml" afterDir="false" />
<change afterPath="$PROJECT_DIR$/.idea/misc.xml" afterDir="false" />
<change afterPath="$PROJECT_DIR$/.idea/modules.xml" afterDir="false" />
<change afterPath="$PROJECT_DIR$/.idea/vcs.xml" afterDir="false" />
<change afterPath="$PROJECT_DIR$/controllers/ctrl1/infrared.c" afterDir="false" />
<change afterPath="$PROJECT_DIR$/controllers/ctrl1/infrared.h" afterDir="false" />
<change afterPath="$PROJECT_DIR$/controllers/ctrl1/laplace.h" afterDir="false" />
<change afterPath="$PROJECT_DIR$/controllers/ctrl1/measurements.h" afterDir="false" />
<change afterPath="$PROJECT_DIR$/controllers/ctrl1/rangeAndBearing.c" afterDir="false" />
<change afterPath="$PROJECT_DIR$/controllers/ctrl1/rangeAndBearing.h" afterDir="false" />
<change beforePath="$PROJECT_DIR$/README.md" beforeDir="false" />
<change beforePath="$PROJECT_DIR$/worlds/.collision.wbproj" beforeDir="false" />
<change beforePath="$PROJECT_DIR$/worlds/.obstacle.wbproj" beforeDir="false" />
<change beforePath="$PROJECT_DIR$/worlds/collision.wbt" beforeDir="false" />
<change beforePath="$PROJECT_DIR$/worlds/textures/ague.jpg" beforeDir="false" />
<change beforePath="$PROJECT_DIR$/worlds/textures/stone.jpg" beforeDir="false" />
<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" />
......@@ -47,6 +44,7 @@
<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" />
......@@ -74,7 +72,7 @@
<option name="number" value="Default" />
<option name="presentableId" value="Default" />
<updated>1575301462899</updated>
<workItem from="1575301466330" duration="7502000" />
<workItem from="1575301466330" duration="12211000" />
</task>
<servers />
</component>
......
......@@ -66,7 +66,7 @@
### 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 +=
......
build/release/ctrl1.o: ctrl1.c \
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 \
......
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
......@@ -9,120 +9,73 @@
/********************************************************************************/
#include <stdio.h>
#include <math.h>
#include <string.h>
#include "../commonSrc/laplace.h"
#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 ROBOT_NAME "ROBOT"
#define TIME_STEP 64 // [ms] Length of time step
// ********
#define NB_SENSORS 8 // Number of distance sensors
/* Harware elements declaration */
WbDeviceTag left_motor; //handler for left wheel of the robot
WbDeviceTag right_motor; //handler for the right wheel of the robot
static WbDeviceTag left_motor;
static WbDeviceTag right_motor;
WbDeviceTag ds[NB_SENSORS]; // Handle for the infrared distance sensors
static WbDeviceTag ds[NB_SENSORS];
WbDeviceTag receiver; // Handle for the receiver node
WbDeviceTag emitter; // Handle for the emitter node
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);
int main(){
rangeAndBearing_init();
infrared_init();
motors_init();
}
char* robot_name=(char*) wb_robot_get_name();
int main(){
int msl = 0, msr = 0; // Wheel speeds
float msl_w, msr_w;
int bmsl, bmsr, sum_sensors; // Braitenberg parameters
int i;
int distances[NB_SENSORS]; // Array for the distance sensor readings
int max_sens = 0; // Store highest sensor value
reset(); // Resetting the robot
float new_speed[2] = {0,0};
for(;;){
bmsl = 0; bmsr = 0;
sum_sensors = 0;
max_sens = 0;
Measurement* list = getRelativePositions(ROBOT_NAME);
speed[robot_id][0] = (1/DELTA_T)*(my_position[0]-prev_my_position[0]);
speed[robot_id][1] = (1/DELTA_T)*(my_position[1]-prev_my_position[1]);
// Compute wheels speed from reynold's speed
compute_wheel_speeds(&msl, &msr);
// Adapt speed instinct to distance sensor values
if (sum_sensors > NB_SENSORS*MIN_SENS) {
msl -= msl*max_sens/(2*MAX_SENS);
msr -= msr*max_sens/(2*MAX_SENS);
}
// Add Braitenberg
msl += bmsl;
msr += bmsr;
/*Webots 2018b*/
// Set speed
msl_w = msl*MAX_SPEED_WEB/1000;
msr_w = msr*MAX_SPEED_WEB/1000;
wb_motor_set_velocity(left_motor, msl_w);
wb_motor_set_velocity(right_motor, msr_w);
//wb_differential_wheels_set_speed(msl,msr);
/*Webots 2018b*/
// Continue one step
Measurement* list = rangeAndBearing_getRelativeRobotPositions(robot_name);
infrared_updateList(list);
laplace_compute(list, new_speed);
motors_applySpeed(new_speed);
wb_robot_step(TIME_STEP);
}
}
void initialDirection(){
}
void prepareData(){
for (k=0;k<FLOCK_SIZE;k++) {
if (k != robot_id) { // Loop on flockmates only
// If neighbor k is too close (Euclidean distance)
if (pow(loc[robot_id][0]-loc[k][0],2)+pow(loc[robot_id][1]-loc[k][1],2) < NEIGHBOR_BOUND)
{
}
}
}
}
\ No newline at end of file
#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
#define LAPLACE
#ifndef __LAPLACE_H__
#define __LAPLACE_H__
#include "measurements.h"
......@@ -11,7 +11,7 @@
* Returns:
* --
*/
void laplacian(Measurement* objectsInSurround, float* velTot);
void laplace_compute(Measurement* objectsInSurround, float* velTot);
#endif
#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
#define RANGE_AND_BEARING
#ifndef __RANGE_AND_BEARING_H__
#define __RANGE_AND_BEARING_H__
#include "measurements.h"
......@@ -10,6 +10,8 @@
* Returns:
* - The mesurments list with robot IDs
*/
Measurement* getRelativeRobotPositions();
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 "infrared.h"
#include <stdlib.h>
#include <stdio.h>
#define NB_SENSORS 8
WbDeviceTag ds[NB_SENSORS];
static WbDeviceTag ds[NB_SENSORS];
void init(){
int i;
void infrared_init(){
char s[4]="ps0";
for(i=0; i<NB_SENSORS;i++) {
ds[i]=wb_robot_get_device(s); // the device name is specified in the world file
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(i=0;i<NB_SENSORS;i++)
for(int i=0;i<NB_SENSORS;i++)
wb_distance_sensor_enable(ds[i],64);
}
void mergeListWithProxValues(Measurement* measurementList){
void infrared_updateList(Measurement* measurementList){
int distances[NB_SENSORS];
for(i=0;i<NB_SENSORS;i++)
for(int i=0;i<NB_SENSORS;i++)
{
distances[i] = wb_distance_sensor_get_value(ds[i]);
}
......
#ifndef INFRARED
#define INFRARED
#include "measurements.h"
#endif
\ 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 "laplace.h"
#include "inc/laplace.h"
#include <stdlib.h>
#include <stdio.h>
......@@ -10,7 +19,7 @@
* Local constants definition
*/
#define PRINT_MATRIX 0
#define IS_TEST 0
//#define IS_TEST
#define ALPHA_FREE 0.5
#define ALPHA_OBST 0.7
......@@ -51,7 +60,7 @@ void multiply(float* mat1, float* mat2, float* res, int m, int n, int p);
* - m: Number of rows
* - n: Number of columns
*/
void printMatrix(char* name, float* matrix, int m, int n)
void printMatrix(char* name, float* matrix, int m, int n);
/***********************
......@@ -92,11 +101,18 @@ void printMatrix(char* name, float* matrix, int m, int n){
}
}
void laplacian(Measurement* objectsInSurround, int numberOfObjects, float* velTot){
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.;
......@@ -130,16 +146,6 @@ void laplacian(Measurement* objectsInSurround, int numberOfObjects, float* velTo
j++;
}
}
for (int j=0; j<numberOfObjects; j++)
{
if (objectLocation[j].robotID != -1)
weightsMatrix[j*numberOfObjects+j] = 1;
else{
weightsMatrix[j*numberOfObjects+j] = -2; // more importance to avoid obstacle
obstacleInSurround = 1;
}
}
printMatrix("weightsMatrix", weightsMatrix, numberOfObjects, numberOfObjects);
float alpha = (obstacleInSurround ? ALPHA_OBST : ALPHA_FREE);
......
/********************************************************************************/
/* 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 "rangeAndBearing.h"
#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
......@@ -33,16 +51,16 @@ static Measurement* process_received_ping_messages();
* Methods implementation
*/
WbDeviceTag receiver2;
WbDeviceTag emitter2;
static WbDeviceTag receiver;
static WbDeviceTag emitter;
void init(){
void rangeAndBearing_init(){
receiver2 = wb_robot_get_device("receiver2");
emitter2 = wb_robot_get_device("emitter2");
receiver = wb_robot_get_device("receiver");
emitter = wb_robot_get_device("emitter");
wb_receiver_enable(receiver2,64);
wb_receiver_enable(receiver,64);
}
......@@ -50,7 +68,7 @@ static void send_ping(char* robot_name)
{
char out[10];
strcpy(out,robot_name);
wb_emitter_send(emitter2,out,strlen(out)+1);
wb_emitter_send(emitter,out,strlen(out)+1);
}
static Measurement* process_received_ping_messages()
......@@ -65,15 +83,14 @@ static Measurement* process_received_ping_messages()
double theta;
double range;
char *inbuffer; // Buffer for the receiver node
int other_robot_id;
while (wb_receiver_get_queue_length(receiver2) > 0) {
while (wb_receiver_get_queue_length(receiver) > 0) {
numberOfElements++;
inbuffer = (char*) wb_receiver_get_data(receiver2);
message_direction = wb_receiver_get_emitter_direction(receiver2);
message_rssi = wb_receiver_get_signal_strength(receiver2);
inbuffer = (char*) wb_receiver_get_data(receiver);
message_direction = wb_receiver_get_emitter_direction(receiver);
message_rssi = wb_receiver_get_signal_strength(receiver);
double y = message_direction[2];
double x = message_direction[1];
......@@ -93,12 +110,12 @@ static Measurement* process_received_ping_messages()
element->relativePosition[Y] = -1.0 * range*sin(theta); // relative y pos
element->next = NULL;
wb_receiver_next_packet(receiver2);
wb_receiver_next_packet(receiver);
}
return listStart;
}
Measurement* getRelativeRobotPositions(char* robotName){
Measurement* rangeAndBearing_getRelativeRobotPositions(char* robotName){
send_ping(robotName);
return process_received_ping_messages(list, size);
return process_received_ping_messages();
}
\ No newline at end of file
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