Commit c57ada93 authored by Nicolas Peslerbe's avatar Nicolas Peslerbe

Inclusion of real epuck and code adaptations

parent b724b467
[submodule "epuck"]
path = epuck
url = https://disalgitn.epfl.ch/epuck/epuck.git
# ==============================================================================
# Configuration
# ------------------------------------------------------------------------------
PROG = ctrl1
EPUCKLIBROOT = ../../epuck/EpuckDevelopmentTree/library
# ==============================================================================
# Potential Epuck Libraries
# ------------------------------------------------------------------------------
USE_EPUCK_A_D = 0
USE_EPUCK_A_D_ADVANCED_AD_SCAN = 1
USE_EPUCK_CODEC = 0
USE_EPUCK_MOTOR_LED = 1
USE_EPUCK_UART = 1
USE_EPUCK_I2C = 1
EXTERNAL_OBJS = #blank
ifeq ($(USE_EPUCK_A_D),1)
EXTERNAL_OBJS += $(EPUCKLIBROOT)/a_d/e_accelerometer.o $(EPUCKLIBROOT)/a_d/e_ad_conv.o $(EPUCKLIBROOT)/a_d/e_prox_timer2.o
endif
ifeq ($(USE_EPUCK_A_D_ADVANCED_AD_SCAN),1)
EXTERNAL_OBJS += $(EPUCKLIBROOT)/a_d/advance_ad_scan/*.o
endif
ifeq ($(USE_EPUCK_CODEC),1)
EXTERNAL_OBJS += $(EPUCKLIBROOT)/codec/e_sub_dci_kickoff.o $(EPUCKLIBROOT)/codec/e_isr_dci.o $(EPUCKLIBROOT)/codec/e_init_dci_master.o $(EPUCKLIBROOT)/codec/e_init_codec_slave.o $(EPUCKLIBROOT)/codec/e_const_sound.o $(EPUCKLIBROOT)/codec/e_sound.o
endif
ifeq ($(USE_EPUCK_MOTOR_LED),1)
EXTERNAL_OBJS += $(EPUCKLIBROOT)/motor_led/e_init_port.o $(EPUCKLIBROOT)/motor_led/e_led.o $(EPUCKLIBROOT)/motor_led/e_motors.o
endif
ifeq ($(USE_EPUCK_UART),1)
EXTERNAL_OBJS += $(EPUCKLIBROOT)/uart/e_init_uart1.o $(EPUCKLIBROOT)/uart/e_uart1_rx_char.o $(EPUCKLIBROOT)/uart/e_uart1_tx_char.o
endif
ifeq ($(USE_EPUCK_I2C),1)
EXTERNAL_OBJS += $(EPUCKLIBROOT)/I2C/e_I2C_protocol.o $(EPUCKLIBROOT)/I2C/e_I2C_master_module.o
endif
# ==============================================================================
# Local variables
# ------------------------------------------------------------------------------
# first, find all the pertinent filenames
ASMSRCS = $(wildcard *.s)
CSRCS = ctrl1.c laplace.c infrared.c rangeAndBearing.c motors.c deadReckoning.c utilities.c sensorcalibrate.c
CXXSRCS = $(wildcard *.cc) $(wildcard *.cpp) $(wildcard *.C)
SRCS = $(CSRCS) $(CXXSRCS) $(ASMSRCS)
HDRS = $(wildcard *.h) $(wildcard *.hh)
OBJS = $(addsuffix .o,$(basename $(SRCS)))
DEPS = $(addsuffix .d,$(basename $(SRCS)))
# program aliases
CTAGS = ctags
ASM = pic30-elf-as
CC = pic30-elf-gcc
LD = pic30-elf-ld
# common options
DEPCFLAGS = -Wall -I. -I$(EPUCKLIBROOT)/std_microchip/support/h -I$(EPUCKLIBROOT)/std_microchip/include -I$(EPUCKLIBROOT) -I../std_microchip/support/h
CFLAGS = $(DEPCFLAGS) -mcpu=30F6014A -DIS_REAL_ROBOT
LDFLAGS = -L$(EPUCKLIBROOT)/std_microchip/lib --defsym=__ICD2RAM=1 --script=$(EPUCKLIBROOT)/std_microchip/support/gld/p30f6014a.gld -mpic30_elf32
LDLIBS = -lpic30-elf -lm-elf -lc-elf
#LDLIBS = -lpic30-coff -lm-coff -lc-coff
#LDLIBS = --start-group -lpic30-coff -lm-coff -lc-coff --end-group
#LDLIBS = --start-group -lpic30-elf -lm-elf -lc-elf --end-group
ifeq ($(MAKECMDGOALS),debug)
CFLAGS += -g -DDEBUG -DVERBOSITY=0
endif
# ==============================================================================
# Dependencies & rules
# ------------------------------------------------------------------------------
all: $(PROG).hex tags $(DEPS)
.PHONY: debug
debug: all
%.hex: %.cof
pic30-elf-bin2hex $(PROG).cof
%.cof: $(OBJS)
$(LD) $(LDFLAGS) --start-group $(LDLIBS) $(OBJS) $(EXTERNAL_OBJS) --end-group -o $@
%.o: %.s
$(ASM) -I../../../library/std_microchip/support/inc -p=30F6014A $< -o $@
# ==============================================================================
# Clean up directory
# ------------------------------------------------------------------------------
.PHONY: clean
clean:
- $(RM) $(OBJS) *~ core.* *.rpo $(PROG).cof $(PROG).hex
.PHONY: distclean
distclean: clean
- $(RM) $(DEPS) tags *.a *.so $(OUTPUT)
# ==============================================================================
# ==============================================================================
# make tags files for vim
# ------------------------------------------------------------------------------
tags: $(SRCS) $(HDRS)
$(CTAGS) $(SRCS) $(HDRS)
# ==============================================================================
# ==============================================================================
# a default rule for building dependency files
# ------------------------------------------------------------------------------
%.d: %.c
@ $(SHELL) -ec '$(CXX) -MM $(DEPCFLAGS) $< > $@'
%.d: %.cc
@ $(SHELL) -ec '$(CXX) -MM $(DEPCFLAGS) $< > $@'
%.d: %.C
@ $(SHELL) -ec '$(CXX) -MM $(DEPCFLAGS) $< > $@'
%.d: %.cpp
@ $(SHELL) -ec '$(CXX) -MM $(DEPCFLAGS) $< > $@'
%.d: %.s
cat /dev/null > ./$@
# ==============================================================================
# ==============================================================================
# include the source code dependencies
# ------------------------------------------------------------------------------
ifneq ($(MAKECMDGOALS),clean)
include $(DEPS)
endif
# ==============================================================================
# ==============================================================================
# end of Makefile
No preview for this file type
......@@ -16,116 +16,116 @@
#include "inc/deadReckoning.h"
#include "inc/utilities.h"
#ifdef IS_REAL_ROBOT
#else
#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>
#endif
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
// ******** SETTINGS PARAMETERS
#define USE_SUPERVISOR false
// ******** SETTINGS PARAMETERS
#define USE_SUPERVISOR 0
#define FLOCK_SIZE 5
#define NEIGHBOR_BOUND
#define ROBOT_TO_MONITOR 0
#define TIME_STEP 64 // [ms] Length of time step
#define EPS 0.1
#define MIGR_SPEED 0.05
#define MIGR_SPEED 0.06
#define SPEED_CORR 0.02
// ********
#define NB_SENSORS 8 // Number of distance sensors
static char* robot_name;
static int robot_id_u, robot_id;
bool state = 0 ; // state for the fsm : 0 -> free & 1 -> obstacle avoidance
int state = 0 ; // state for the fsm : 0 -> free & 1 -> obstacle avoidance
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;
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();
#ifdef IS_REAL_ROBOT
#else
wb_robot_init();
if(getRobotId() == ROBOT_TO_MONITOR)
utilities_changeDebugState(1);
printf("Reset: robot %d\n",getRobotId());
#endif
deadReckoning_init();
rangeAndBearing_init();
infrared_init();
motors_init();
}
void migratory(float* my_speed, float* my_position){
// realign migration
if (my_position[Y] > EPS) my_speed[Y] -= SPEED_CORR;
if (my_position[Y] < -EPS) my_speed[Y] += SPEED_CORR;
// realign migration
//if (my_position[Y] > EPS) my_speed[Y] -= SPEED_CORR;
//if (my_position[Y] < -EPS) my_speed[Y] += SPEED_CORR;
// add constant migration to the other side
my_speed[X] += MIGR_SPEED;
// add constant migration to the other side
my_speed[X] += MIGR_SPEED;
}
int main(){
reset(); // Resetting the robot
float my_speed[2] = {0,0};
float weights[2] = {1,-20};
float my_position[3] = {0,0,0};
int applied_speed[2] = {0,0};
reset(); // Resetting the robot
float my_speed[2] = {0,0};
float weights[2] = {1,-20};
float my_position[3] = {0,0,0};
int applied_speed[2] = {0,0};
for(;;){
numberPrint++;
int obstacle = 0;
if(utilities_debug()) printf("___________________\n");
if(utilities_debug()) printf("Robot %d\n",robot_id);
Measurement* list = rangeAndBearing_getRelativeRobotPositions(robot_name);
list = infrared_updateList(list, &obstacle);
for(;;){
numberPrint++;
int obstacle = 0;
if(utilities_debug()) printf("___________________\n");
if(utilities_debug()) printf("Robot %d\n",utilities_getRobotId());
Measurement* list = rangeAndBearing_getRelativeRobotPositions();
list = infrared_updateList(list, &obstacle);
deadReckoning_updateSelfPosition(my_position, applied_speed);
float my_speed_tmp[2];
// compute new speed according to laplace method in initial referential
laplace_compute(list, my_speed, weights, my_position);
deadReckoning_updateSelfPosition(my_position, applied_speed);
float my_speed_tmp[2];
// compute new speed according to laplace method in initial referential
laplace_compute(list, my_speed, weights, my_position);
if(utilities_debug()) printf("Before migr x = %f, y = %f\n", my_speed[X], my_speed[Y]);
if(utilities_debug()) printf("Before migr x = %f, y = %f\n", my_speed[X], my_speed[Y]);
utilities_transformReferential(my_speed, my_speed_tmp, -my_position[2]); //from Rt to R0
utilities_transformReferential(my_speed, my_speed_tmp, -my_position[2]); //from Rt to R0
// add migration urge and realignment in intial referential
// add migration urge and realignment in intial referential
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
// change speed in inital referential to current referential
utilities_transformReferential(my_speed_tmp, my_speed, my_position[2]); //from R0 to Rt
if(utilities_debug()) printf("After migr x = %f, y = %f\n", my_speed[X], my_speed[Y]);
if(utilities_debug()) printf("After migr x = %f, y = %f\n", my_speed[X], my_speed[Y]);
// apply speed to motors
motors_applySpeed(my_speed, applied_speed);
// apply speed to motors
motors_applySpeed(my_speed, applied_speed);
// Emptying data
Measurement* element = NULL;
while(list != NULL){
element = list->next;
free(list);
list = element;
}
// Emptying data
Measurement* element = NULL;
while(list != NULL){
element = list->next;
free(list);
list = element;
}
wb_robot_step(TIME_STEP);
}
#ifndef IS_REAL_ROBOT
wb_robot_step(TIME_STEP);
#else
int i;
for(i=0;i<20000;i++); //wait 10ms;
#endif
}
}
ctrl1.o: ctrl1.c inc/laplace.h inc/measurements.h inc/infrared.h \
inc/motors.h inc/rangeAndBearing.h inc/deadReckoning.h inc/utilities.h \
../../epuck/EpuckDevelopmentTree/library/std_microchip/include/stdio.h \
../../epuck/EpuckDevelopmentTree/library/std_microchip/include/yvals.h \
../../epuck/EpuckDevelopmentTree/library/std_microchip/include/math.h \
../../epuck/EpuckDevelopmentTree/library/std_microchip/include/stdlib.h \
../../epuck/EpuckDevelopmentTree/library/std_microchip/include/string.h
......@@ -7,15 +7,29 @@
#include <time.h>
#ifdef IS_REAL_ROBOT
#include "./motor_led/e_motors.h"
void deadReckoning_init(){
e_set_dr_position(dr_x,dr_y,dr_theta);
}
void deadReckoning_updateSelfPosition(float* my_position, int* applied_speed){
(void) applied_speed;
e_do_dr();
e_get_dr_position(&my_position[0],&my_position[1],&my_position[2]);
}
#else
#define V0 0.5 // Migratory urge speed
#define AXLE_LENGTH 0.052 // Distance between wheels of robot (meters)
#define SPEED_UNIT_RADS 0.00628 // Conversion factor from speed unit to radian per second
#define WHEEL_RADIUS 0.0205 // Wheel radius (meters)
#define DELTA_T 0.064 // Timestep (seconds)
void deadReckoning_init(){
}
void deadReckoning_init(){}
void deadReckoning_updateSelfPosition(float* my_position, int* applied_speed){
// Compute deltas of the robot
......@@ -41,3 +55,5 @@ void deadReckoning_updateSelfPosition(float* my_position, int* applied_speed){
if(utilities_debug()) printf("deadReck location: x = %f, y= %f, theta= %f\n", my_position[0], my_position[1], my_position[2]);
}
#endif
deadReckoning.o: deadReckoning.c inc/deadReckoning.h inc/measurements.h \
inc/utilities.h \
../../epuck/EpuckDevelopmentTree/library/std_microchip/include/math.h \
../../epuck/EpuckDevelopmentTree/library/std_microchip/include/stdlib.h \
../../epuck/EpuckDevelopmentTree/library/std_microchip/include/yvals.h \
../../epuck/EpuckDevelopmentTree/library/std_microchip/include/stdio.h \
../../epuck/EpuckDevelopmentTree/library/std_microchip/include/time.h
......@@ -4,7 +4,6 @@
#include "measurements.h"
void deadReckoning_init();
void deadReckoning(float* vel_mig);
void deadReckoning_updateSelfPosition(float* my_position, int* applied_speed);
#endif
\ No newline at end of file
#endif
......@@ -6,4 +6,4 @@
void infrared_init();
Measurement* infrared_updateList(Measurement* measurementList, int* obstacle);
#endif
\ No newline at end of file
#endif
......@@ -17,4 +17,4 @@ struct Measurement_t{
Measurement* next;
};
#endif
\ No newline at end of file
#endif
......@@ -7,4 +7,4 @@ void motors_init();
void motors_applySpeed(float* speed, int* appliedSpeed);
#endif
\ No newline at end of file
#endif
......@@ -14,4 +14,4 @@ Measurement* rangeAndBearing_getRelativeRobotPositions();
void rangeAndBearing_init();
#endif
\ No newline at end of file
#endif
#ifndef SENSORCALIBRATE_H
#define SENSORCALIBRATE_H
extern int sensorzero[8];
void sensor_calibrate();
#endif
......@@ -10,5 +10,13 @@ extern int numberPrint;
void utilities_changeDebugState(int enableDebugTemp);
int utilities_debug();
void utilities_transformReferential(float* speed, float* new_speed, float theta);
int utilities_getRobotId();
int utilities_isInMyTeam(int robot);
int utilities_getTeamID(int robot);
#endif
\ No newline at end of file
#ifdef IS_REAL_ROBOT
void wait(long num);
int getselector() ;
#endif
#endif
......@@ -8,28 +8,39 @@
/***********************
* Inclusions
*/
* Inclusions
*/
#include "inc/infrared.h"
#include "inc/utilities.h"
#include <webots/robot.h>
#include <webots/distance_sensor.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#define NB_SENSORS 8
#define DISTANCE_THRESHOLD 200
#define EPS 0.78
#ifdef IS_REAL_ROBOT
#include "inc/sensorcalibrate.h"
#include "./a_d/e_prox.h"
#define NB_SAMPLES 5
#else
#include <webots/robot.h>
#include <webots/distance_sensor.h>
static WbDeviceTag ds[NB_SENSORS];
#endif
float ps_orientations[8] = {-0.3,-0.8,-1.57,3.64,2.64,1.57,0.8,0.3}; //proximity sensors orientations
float ps_weights[8] = {10, 8, 6, 3, 3,10, 14, 18};
static WbDeviceTag ds[NB_SENSORS];
bool mustPrint = false;
void infrared_init(){
#ifdef IS_REAL_ROBOT
sensor_calibrate();
#else
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
......@@ -37,65 +48,91 @@ void infrared_init(){
}
for(int i=0;i<NB_SENSORS;i++)
wb_distance_sensor_enable(ds[i],64);
wb_distance_sensor_enable(ds[i],64);
#endif
}
Measurement* infrared_updateList(Measurement* measurementList, int* obstacle){
double intensity;
double intensity[NB_SENSORS];
int i;
#ifdef IS_REAL_ROBOT
double sensorMean[NB_SENSORS];
for (i=0; i<NB_SENSORS; i++)
sensorMean[i]=0;
int n;
for(n=0;n<NB_SAMPLES; n++)
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(i == 3 || i == 4) continue;
if (intensity > DISTANCE_THRESHOLD){
float distance = sqrt(1/intensity);
float theta = ps_orientations[i];
/*if(measurementList){ // BUG ifno element in list
Measurement* element = measurementList;
do{
// Check if element overlaps
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;*/
*obstacle = 1;
Measurement* temp = malloc(sizeof(Measurement));
temp->next = measurementList;
temp->robotID = -1;
temp->biasPosition[X] = 0;
temp->biasPosition[Y] = 0;
temp->relativePosition[DISTANCE] = distance; // relative x pos
temp->relativePosition[THETA] = theta; // relative y pos
temp->weight = ps_weights[i]*2;
/*if(temp->relativePosition[THETA] > 0 && temp->relativePosition[THETA] < 3.1415926 && my_position[Y] > 0 )
temp->weight = 15;
else
temp->weight = 10;
// Get sensor values
for (i=0; i < NB_SENSORS; i++) {
// Use the sensorzero[i] value generated in sensor_calibrate() to zero sensorvalues
intensity[i] = e_get_prox(i)-sensorzero[i];
//linearize the sensor output and compute the average
sensorMean[i]+=12.1514*log((double)intensity[i])/(double)NB_SAMPLES;
}
}
#else
for(i=0;i<NB_SENSORS;i++)
{
intensity[i] = wb_distance_sensor_get_value(ds[i]);
}
#endif
for(i=0;i<NB_SENSORS;i++)
{
if(utilities_debug()) printf("sensor %d intensity at orientation %f = %f\n", i, ps_orientations[i], intensity);
//if(i == 3 || i == 4) continue;
if (intensity[i] > DISTANCE_THRESHOLD){
float distance = sqrt(1/intensity[i]);
float theta = ps_orientations[i];
/*if(measurementList){ // BUG ifno element in list
Measurement* element = measurementList;
do{
// Check if element overlaps
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;*/
*obstacle = 1;
Measurement* temp = malloc(sizeof(Measurement));
temp->next = measurementList;
temp->robotID = -1;
temp->biasPosition[X] = 0;
temp->biasPosition[Y] = 0;
temp->relativePosition[DISTANCE] = distance; // relative x pos
temp->relativePosition[THETA] = theta; // relative y pos
temp->weight = ps_weights[i]*2;
/*if(temp->relativePosition[THETA] > 0 && temp->relativePosition[THETA] < 3.1415926 && my_position[Y] > 0 )
temp->weight = 15;
else
temp->weight = 10;
*/
if(utilities_debug()) printf("Object detected: distance = %f, theta = %f\n", temp->relativePosition[DISTANCE], temp->relativePosition[THETA]);
if(utilities_debug()) printf("Object detected: distance = %f, theta = %f\n", temp->relativePosition[DISTANCE], temp->relativePosition[THETA]);
measurementList = temp;
}
}
return measurementList;
measurementList = temp;
}
}
return measurementList;
}
\ No newline at end of file
}
infrared.o: infrared.c inc/infrared.h inc/measurements.h inc/utilities.