Commit e596245a authored by Nicolas Peslerbe's avatar Nicolas Peslerbe

Working program with real e-pucks

parent f0a22f7e
This diff is collapsed.
......@@ -57,6 +57,12 @@ static void reset(){
utilities_changeDebugState(1);
printf("Reset: robot %d\n",getRobotId());
#endif
e_init_port();
e_init_ad_scan();
e_init_uart1();
e_led_clear();
deadReckoning_init();
rangeAndBearing_init();
infrared_init();
......@@ -77,9 +83,9 @@ void migratory(float* my_speed, float* my_position){
int main(){
reset(); // Resetting the robot
double my_position[3] = {0,0,0};
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(;;){
......
This source diff could not be displayed because it is too large. You can view the blob instead.
......@@ -9,13 +9,13 @@
#ifdef IS_REAL_ROBOT
#include "./motor_led/e_motors.h"
#include "./epfl/e_motors.h"
void deadReckoning_init(){
e_set_dr_position(dr_x,dr_y,dr_theta);
e_set_dr_position(0.,0.,0.);
}
void deadReckoning_updateSelfPosition(float* my_position, int* applied_speed){
void deadReckoning_updateSelfPosition(double* my_position, int* applied_speed){
(void) applied_speed;
e_do_dr();
e_get_dr_position(&my_position[0],&my_position[1],&my_position[2]);
......@@ -31,7 +31,7 @@ void deadReckoning_updateSelfPosition(float* my_position, int* applied_speed){
void deadReckoning_init(){}
void deadReckoning_updateSelfPosition(float* my_position, int* applied_speed){
void deadReckoning_updateSelfPosition(double* my_position, int* applied_speed){
// Compute deltas of the robot
float dr = (float) applied_speed[RIGHT] * SPEED_UNIT_RADS * WHEEL_RADIUS * DELTA_T;
float dl = (float) applied_speed[LEFT] * SPEED_UNIT_RADS * WHEEL_RADIUS * DELTA_T;
......@@ -44,9 +44,9 @@ void deadReckoning_updateSelfPosition(float* my_position, int* applied_speed){
// Update position ??? maybe add time step???
my_position[0] += dx;
my_position[1] += dy;
my_position[2] += dtheta;
my_position[0] += (double) dx;
my_position[1] += (double) dy;
my_position[2] += (double) dtheta;
// Keep orientation within 0, 2pi
while (my_position[2] > 2*M_PI) my_position[2] -= 2.0*M_PI;
......
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
* e_robot_id.c
*
* Interface to robot's unique id number stored in the EEPROM.
*
* Adapted from initial version by X. Raemy.
*
* $Author$
* $Date: 2007-09-20 14:21:06 +0200 (Thu, 20 Sep 2007) $
* $Revision$
* $HeadURL: https://grmapc10.epfl.ch/svn/students/Epuck/EpuckDevelopmentTree/library/contrib/robot_id/e_robot_id.c $
*
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
#include "inc/e_robot_id.h"
#include "epfl/e_epuck_ports.h"
static int robotid;
int readrobotidfromeeprom();
void e_init_robot_id() {
robotid = readrobotidfromeeprom();
}
int e_get_robot_id() {
return robotid;
}
int e_get_selector() {
return SELECTOR0 + 2*SELECTOR1 + 4*SELECTOR2 + 8*SELECTOR3;
}
.include "p30f6014a.inc"
.global _readrobotidfromeeprom
.section eeprom, eedata
EE_number:
.section .text
_readrobotidfromeeprom:
; Setup pointer to EEPROM memory
MOV #tblpage(EE_number),W1
MOV W1,TBLPAG
MOV #tbloffset(EE_number),W1
; Read the EEPROM data
TBLRDL [W1],W0
return
.end
......@@ -4,6 +4,6 @@
#include "measurements.h"
void deadReckoning_init();
void deadReckoning_updateSelfPosition(float* my_position, int* applied_speed);
void deadReckoning_updateSelfPosition(double* my_position, int* applied_speed);
#endif
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
* e_robot_id.h
*
* Interface to robot's unique id number stored in the EEPROM.
*
* Adapted from initial version by X. Raemy.
*
* $Author$
* $Date: 2007-09-20 14:21:06 +0200 (Thu, 20 Sep 2007) $
* $Revision$
* $HeadURL: https://grmapc10.epfl.ch/svn/students/Epuck/EpuckDevelopmentTree/library/contrib/robot_id/e_robot_id.h $
*
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
#ifndef _ROBOT_ID_H
#define _ROBOT_ID_H
void e_init_robot_id();
int e_get_robot_id();
int e_get_selector();
#endif // _ROBOT_ID_H
#ifndef SENSORCALIBRATE_H
#define SENSORCALIBRATE_H
extern int sensorzero[8];
void sensor_calibrate();
#endif
......@@ -21,8 +21,7 @@
#define EPS 0.78
#ifdef IS_REAL_ROBOT
#include "inc/sensorcalibrate.h"
#include "./a_d/e_prox.h"
#include "./ircom/e_ad_conv.h"
#define NB_SAMPLES 5
#else
#include <webots/robot.h>
......@@ -39,7 +38,7 @@ float ps_weights[8] = {10, 8, 6, 3, 3,10, 14, 18};
void infrared_init(){
#ifdef IS_REAL_ROBOT
sensor_calibrate();
e_calibrate_ir();
#else
char s[4]="ps0";
for(int i=0; i<NB_SENSORS;i++) {
......@@ -69,7 +68,7 @@ Measurement* infrared_updateList(Measurement* measurementList, int* obstacle){
// 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];
intensity[i] = e_get_calibrated_prox(i);
//linearize the sensor output and compute the average
sensorMean[i]+=12.1514*log((double)intensity[i])/(double)NB_SAMPLES;
}
......
......@@ -13,7 +13,7 @@
#include <math.h>
#ifdef IS_REAL_ROBOT
#include "./motor_led/e_motors.h"
#include "./epfl/e_motors.h"
#else
#include <webots/robot.h>
#include <webots/motor.h>
......@@ -87,7 +87,7 @@ void motors_applySpeed(float* speed, int* appliedSpeed){
if(utilities_debug()) printf("Motor speed left = %d, right = %d\n", appliedSpeed[LEFT], appliedSpeed[RIGHT]);
#ifdef IS_REAL_ROBOT
e_set_speed_left(appliedSpeed[LEFT]);
e_set_speed_right(appliedSpeed[IGHT]);
e_set_speed_right(appliedSpeed[RIGHT]);
#else
wb_motor_set_velocity(left_motor, ((double) appliedSpeed[LEFT])*MAX_SPEED_WEB/MAX_SPEED_EPUCK);
wb_motor_set_velocity(right_motor, ((double) appliedSpeed[RIGHT])*MAX_SPEED_WEB/MAX_SPEED_EPUCK);
......
......@@ -8,14 +8,15 @@
/***********************
* Inclusions
*/
* Inclusions
*/
#include "inc/rangeAndBearing.h"
#include "inc/utilities.h"
#include <stdlib.h>
#ifdef IS_REAL_ROBOT
#include <ircom/ircom.h>
#else
#include <webots/emitter.h>
......@@ -23,6 +24,9 @@
#include <webots/robot.h>
#include <webots/differential_wheels.h>
#include <webots/distance_sensor.h>
static WbDeviceTag receiver;
static WbDeviceTag emitter;
#endif
#include <string.h>
......@@ -33,117 +37,143 @@
// define bias matrix with at most 9 robots
float bias[10][2] = {{4,0},{3,1},{3,-1},{2,-2},
{2,2},{2,0},{1,1},{1,-1},{0,0}};
{2,2},{2,0},{1,1},{1,-1},{0,0}};
/***********************
* Local prototype
*/
* Local prototype
*/
/**
* Send a ping to other robots
* Parameters:
* - The robot name
* Returns:
* --
*/
* Send a ping to other robots
* Parameters:
* - The robot name
* Returns:
* --
*/
static void send_ping();
/**
* Compute relative distant of other robots and add it to a linked list
* Parameters:
* --
* Returns:
* The measurements linkedlist.
*/
* Compute relative distant of other robots and add it to a linked list
* Parameters:
* --
* Returns:
* The measurements linkedlist.
*/
static Measurement* process_received_ping_messages();
/***********************
* Methods implementation
*/
* Methods implementation
*/
static WbDeviceTag receiver;
static WbDeviceTag emitter;
void rangeAndBearing_init(){
receiver = wb_robot_get_device("receiver2");
emitter = wb_robot_get_device("emitter2");
void rangeAndBearing_init(){
#ifdef IS_REAL_ROBOT
ircomStart();
ircomEnableContinuousListening();
ircomListen();
#else
receiver = wb_robot_get_device("receiver2");
emitter = wb_robot_get_device("emitter2");
wb_receiver_enable(receiver,64);
#endif
wb_receiver_enable(receiver,64);
}
#ifdef IS_REAL_ROBOT
static void send_ping()
{
ircomSend(utilities_getRobotId());
while (ircomSendDone() == 0);
}
#else
static void send_ping()
{
char out[2] = {(char) utilities_getRobotId(), '\0'};
wb_emitter_send(emitter,out,strlen(out)+1);
wb_emitter_send(emitter,out,strlen(out)+1);
}
#endif
static Measurement* process_received_ping_messages()
{
int numberOfElements = 0;
Measurement* listStart = NULL;
Measurement* element = NULL;
Measurement* listStart = NULL;
Measurement* element = NULL;
const double *message_direction;
double message_rssi; // Received Signal Strength indicator
double theta;
double range;
#ifdef IS_REAL_ROBOT
IrcomMessage imsg;
while(1){
ircomPopMessage(&imsg);
if (imsg.error != 0)
{
break;
}
int receivedId = (int) imsg.value;
range = (double)imsg.distance;
theta = (double)imsg.direction;
const double *message_direction;
double message_rssi; // Received Signal Strength indicator
double theta;
double range;
char *inbuffer; // Buffer for the receiver node
while (wb_receiver_get_queue_length(receiver) > 0) {
numberOfElements++;
#else
char *inbuffer; // Buffer for the receiver node
while (wb_receiver_get_queue_length(receiver) > 0) {
inbuffer = (char*) wb_receiver_get_data(receiver);
message_direction = wb_receiver_get_emitter_direction(receiver);
message_rssi = wb_receiver_get_signal_strength(receiver);
inbuffer = (char*) wb_receiver_get_data(receiver);
message_direction = wb_receiver_get_emitter_direction(receiver);
message_rssi = wb_receiver_get_signal_strength(receiver);
int receivedId = (int)inbuffer[0];
#endif
double y = message_direction[2];
double x = message_direction[1];
double y = message_direction[2];
double x = message_direction[1];
theta = -atan2(y,x) - 1.5707;
range = sqrt((1/message_rssi));
theta = -atan2(y,x) - 1.5707;
range = sqrt((1/message_rssi));
int receivedId = (int)inbuffer[0];
if(utilities_isInMyTeam(receivedId)){
if(utilities_isInMyTeam(receivedId)){
int friendID = utilities_getTeamID(receivedId);
int myID = utilities_getTeamID(utilities_getRobotId());
int friendID = utilities_getTeamID(receivedId);
int myID = utilities_getTeamID(utilities_getRobotId());
if(listStart == NULL){
element = malloc(sizeof(Measurement));
listStart = element;
} else{
element->next = malloc(sizeof(Measurement));
element = element->next;
}
if(listStart == NULL){
element = malloc(sizeof(Measurement));
listStart = element;
} else{
element->next = malloc(sizeof(Measurement));
element = element->next;
}
element->robotID = friendID;
element->robotID = friendID;
element->biasPosition[X] = bias[friendID][0] - bias[myID][0];
element->biasPosition[Y] = bias[friendID][1] - bias[myID][1];
element->biasPosition[X] = bias[friendID][0] - bias[myID][0];
element->biasPosition[Y] = bias[friendID][1] - bias[myID][1];
element->relativePosition[THETA] = theta; // relative x pos
element->relativePosition[DISTANCE] = range; // relative y pos
element->weight = 4;
element->relativePosition[THETA] = theta; // relative x pos
element->relativePosition[DISTANCE] = range; // relative y pos
element->weight = 4;
if(utilities_debug()) printf("%d: ",receivedId);
if(utilities_debug()) printf("%d: ",receivedId);
element->next = NULL;
if(utilities_debug()) printf("r&b %d: distance: %f, theta:%f\n", receivedId, element->relativePosition[DISTANCE], element->relativePosition[THETA] );
if(utilities_debug()) printf("bias: x: %f, y:%f\n", element->biasPosition[X], element->biasPosition[Y]);
}
wb_receiver_next_packet(receiver);
element->next = NULL;
if(utilities_debug()) printf("r&b %d: distance: %f, theta:%f\n", receivedId, element->relativePosition[DISTANCE], element->relativePosition[THETA] );
if(utilities_debug()) printf("bias: x: %f, y:%f\n", element->biasPosition[X], element->biasPosition[Y]);
}
#ifndef IS_REAL_ROBOT
wb_receiver_next_packet(receiver);
#endif
}
return listStart;
}
}
Measurement* rangeAndBearing_getRelativeRobotPositions(){
send_ping();
return process_received_ping_messages();
}
Measurement* rangeAndBearing_getRelativeRobotPositions(){
send_ping();
return process_received_ping_messages();
}
//******************************************************************************
// Name: sensorcalibrate.c
// Author:
// Date:
// Rev: October 5, 2015 by Florian Maushart
// Descr: Take the average of 32 samples of the e-puck distance sensors for
// calibration and output them to sensorzero[i]
//******************************************************************************
#include "p30f6014a.h"
#include "stdio.h"
#include "string.h"
#include "uart/e_uart_char.h"
#include "a_d/e_prox.h"
#include "inc/utilities.h"
#include "inc/sensorcalibrate.h"
int sensorzero[8];
void sensor_calibrate() {
int i, j;
char buffer[80];
long sensor[8];
for (i=0; i<8; i++) {
sensor[i]=0;
}
for (j=0; j<32; j++) {
for (i=0; i<8; i++) {
sensor[i]+=e_get_prox(i);
}
wait(10000);
}
for (i=0; i<8; i++) {
sensorzero[i]=(sensor[i]>>5);
sprintf(buffer, "%d, ", sensorzero[i]);
e_send_uart1_char(buffer, strlen(buffer));
}
sprintf(buffer, " calibration done\r\n");
e_send_uart1_char(buffer, strlen(buffer));
wait(100000);
}
!_TAG_FILE_FORMAT 2 /extended format; --format=1 will not append ;" to lines/
!_TAG_FILE_SORTED 1 /0=unsorted, 1=sorted, 2=foldcase/
!_TAG_PROGRAM_AUTHOR Darren Hiebert /dhiebert@users.sourceforge.net/
!_TAG_PROGRAM_NAME Exuberant Ctags //
!_TAG_PROGRAM_URL http://ctags.sourceforge.net /official site/
!_TAG_PROGRAM_VERSION 5.9~svn20110310 //
ALPHA_FREE laplace.c 25;" d file:
ALPHA_OBST laplace.c 26;" d file:
AXLE_LENGTH deadReckoning.c 27;" d file:
AXLE_LENGTH motors.c 28;" d file:
DELTA_T deadReckoning.c 30;" d file:
DISTANCE_THRESHOLD infrared.c 20;" d file:
EE_number e_robot_id_asm.s /^EE_number:$/;" l
EPS ctrl1.c 42;" d file:
EPS infrared.c 21;" d file:
FIRST_GROUP_SIZE utilities.c 61;" d file:
FLOCK_SIZE ctrl1.c 37;" d file:
KU motors.c 26;" d file:
KW motors.c 27;" d file:
MAX_BOUNDARY rangeAndBearing.c 36;" d file:
MAX_ROBOTS laplace.c 27;" d file:
MAX_SPEED_EPUCK motors.c 31;" d file:
MAX_SPEED_WEB motors.c 30;" d file:
MIGR_SPEED ctrl1.c 43;" d file:
NB_SAMPLES infrared.c 25;" d file:
NB_SENSORS ctrl1.c 47;" d file:
NB_SENSORS infrared.c 19;" d file:
NEIGHBOR_BOUND ctrl1.c 38;" d file:
PRINT_MATRIX laplace.c 23;" d file:
ROBOT_TO_MONITOR ctrl1.c 39;" d file:
SPEED_CORR ctrl1.c 44;" d file:
SPEED_UNIT_RADS deadReckoning.c 28;" d file:
TIME_STEP ctrl1.c 41;" d file:
USE_SUPERVISOR ctrl1.c 36;" d file:
V0 deadReckoning.c 26;" d file:
WHEEL_RADIUS deadReckoning.c 29;" d file:
WHEEL_RADIUS motors.c 29;" d file:
_readrobotidfromeeprom e_robot_id_asm.s /^_readrobotidfromeeprom:$/;" l
bias rangeAndBearing.c /^float bias[10][2] = {{4,0},{3,1},{3,-1},{2,-2},$/;" v
deadReckoning_init deadReckoning.c /^void deadReckoning_init(){$/;" f
deadReckoning_init deadReckoning.c /^void deadReckoning_init(){}$/;" f
deadReckoning_updateSelfPosition deadReckoning.c /^void deadReckoning_updateSelfPosition(double* my_position, int* applied_speed){$/;" f
ds infrared.c /^static WbDeviceTag ds[NB_SENSORS];$/;" v file:
e_get_robot_id e_robot_id.c /^int e_get_robot_id() {$/;" f
e_get_selector e_robot_id.c /^int e_get_selector() {$/;" f
e_init_robot_id e_robot_id.c /^void e_init_robot_id() {$/;" f
emitter rangeAndBearing.c /^static WbDeviceTag emitter;$/;" v file:
enabledDebug utilities.c /^int enabledDebug = 0;$/;" v
getselector utilities.c /^int getselector() {$/;" f
id_set_up utilities.c /^int id_set_up = 0;$/;" v
infrared_init infrared.c /^void infrared_init(){$/;" f
infrared_updateList infrared.c /^Measurement* infrared_updateList(Measurement* measurementList, int* obstacle){$/;" f
laplace_compute laplace.c /^void laplace_compute(Measurement* objectsInSurround, float* newSpeed, float* weights, float* my_position){$/;" f
left_motor motors.c /^static WbDeviceTag left_motor;$/;" v file:
main ctrl1.c /^int main(){$/;" f
migratory ctrl1.c /^void migratory(float* my_speed, float* my_position){$/;" f
motors_applySpeed motors.c /^void motors_applySpeed(float* speed, int* appliedSpeed){$/;" f
motors_init motors.c /^void motors_init(){$/;" f
motors_limit motors.c /^void motors_limit(int *number, int limit) {$/;" f
motors_transformSpeed motors.c /^void motors_transformSpeed(float* speed, int *speed_for_motors){$/;" f
multiply laplace.c /^void multiply(float* mat1, float* mat2, float* res, int m, int n, int p)$/;" f
numberPrint utilities.c /^int numberPrint = 0;$/;" v
printMatrix laplace.c /^void printMatrix(char* name, float* matrix, int m, int n){$/;" f
process_received_ping_messages rangeAndBearing.c /^static Measurement* process_received_ping_messages()$/;" f file:
ps_orientations infrared.c /^float ps_orientations[8] = {-0.3,-0.8,-1.57,3.64,2.64,1.57,0.8,0.3}; \/\/proximity sensors orientations$/;" v
ps_weights infrared.c /^float ps_weights[8] = {10, 8, 6, 3, 3,10, 14, 18};$/;" v
rangeAndBearing_getRelativeRobotPositions rangeAndBearing.c /^ Measurement* rangeAndBearing_getRelativeRobotPositions(){$/;" f
rangeAndBearing_init rangeAndBearing.c /^void rangeAndBearing_init(){$/;" f
receiver rangeAndBearing.c /^static WbDeviceTag receiver;$/;" v file:
reset ctrl1.c /^static void reset(){$/;" f file:
right_motor motors.c /^static WbDeviceTag right_motor;$/;" v file:
robot_id utilities.c /^int robot_id;$/;" v
robotid e_robot_id.c /^static int robotid;$/;" v file:
send_ping rangeAndBearing.c /^static void send_ping()$/;" f file:
state ctrl1.c /^int state = 0 ; \/\/ state for the fsm : 0 -> free & 1 -> obstacle avoidance$/;" v
transpose laplace.c /^void transpose(float* input, float* output, int m, int n)$/;" f
utilities_changeDebugState utilities.c /^void utilities_changeDebugState(int enabledDebugTemp){$/;" f
utilities_debug utilities.c /^int utilities_debug(){$/;" f
utilities_getRobotId utilities.c /^int utilities_getRobotId(){$/;" f
utilities_getTeamID utilities.c /^int utilities_getTeamID(int robot){$/;" f
utilities_isInMyTeam utilities.c /^int utilities_isInMyTeam(int robot){$/;" f
utilities_transformReferential utilities.c /^void utilities_transformReferential(float* speed, float* new_speed, float theta){$/;" f
wait utilities.c /^void wait(long num) {$/;" f
......@@ -24,21 +24,23 @@ void utilities_transformReferential(float* speed, float* new_speed, float theta)
int robot_id;
int id_set_up = false;
int id_set_up = 0;
#ifdef IS_REAL_ROBOT
#include "./contrib/robot_id/e_robot_id.h"
#include "./motor_led/e_epuck_ports.h"
#include "inc/e_robot_id.h"
#include "./epfl/e_epuck_ports.h"
int utilities_getRobotId(){
if(!id_set_up)
if(!id_set_up){
robot_id = e_get_robot_id()%10;
id_set_up = 1;
}
return robot_id;
}
int utilities_isInMyTeam(int robot){
return robot%2 == getRobotId()%2;
return robot%2 == utilities_getRobotId()%2;
}
int utilities_getTeamID(int robot){
......
/*
Copyright 2007 Alexandre Campo, Alvaro Gutierrez, Valentin Longchamp.
This file is part of libIrcom.
libIrcom is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License.