...
 
Commits (2)
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.
libIrcom is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with libIrcom. If not, see <http://www.gnu.org/licenses/>.
*/
// simple test : send or receive numbers, and avoid obstacles in the same time.
#include <ircom/e_ad_conv.h>
#include <epfl/e_init_port.h>
#include <epfl/e_epuck_ports.h>
#include <epfl/e_uart_char.h>
#include <epfl/e_led.h>
#include <epfl/e_led.h>
#include <epfl/e_motors.h>
#include <epfl/e_agenda.h>
#include <stdio.h>
#include <ircom/ircom.h>
#include <btcom/btcom.h>
#include <math.h>
float sensorDir[NB_IR_SENSORS] = {0.2967, 0.8727, 1.5708, 2.6180, 3.6652, 4.7124, 5.4105, 5.9865};
int getselector()
{
return SELECTOR0 + 2*SELECTOR1 + 4*SELECTOR2 + 8*SELECTOR3;
}
void obstacleAvoidance();
int main()
{
// init robot
e_init_port();
e_init_ad_scan();
e_init_uart1();
e_led_clear();
e_init_motors();
e_start_agendas_processing();
// wait for s to start
//btcomWaitForCommand('s');
//btcomSendString("==== READY - IR TESTING ====\n\n");
e_calibrate_ir();
// initialize ircom and start reading
ircomStart();
ircomEnableContinuousListening();
ircomListen();
// rely on selector to define the role
int selector = getselector();
// show selector choosen
int i;
long int j;
for (i = 0; i < selector; i++)
{
e_led_clear();
for(j = 0; j < 200000; j++)
asm("nop");
e_set_led(i%8, 1);
for(j = 0; j < 300000; j++)
asm("nop");
e_led_clear();
for(j = 0; j < 300000; j++)
asm("nop");
}
// activate obstacle avoidance
//e_activate_agenda(obstacleAvoidance, 10000);
// acting as sender
if (selector == 1)
{
btcomSendString("==== EMITTER ====\n\n");
int i;
for (i = 0; i < 10000; i++)
{
// takes ~15knops for a 32window, avoid putting messages too close...
for(j = 0; j < 200000; j++) asm("nop");
ircomSend(i % 256);
while (ircomSendDone() == 0);
btcomSendString(".");
}
}
// acting as receiver
else if (selector == 2)
{
btcomSendString("==== RECEIVER ====\n\n");
int i = 0;
while (i < 200)
{
// ircomListen();
IrcomMessage imsg;
ircomPopMessage(&imsg);
if (imsg.error == 0)
{
e_set_led(1, 2);
int val = (int) imsg.value;
/* Send Value*/
char tmp[128];
sprintf(tmp, "Receive successful : %d - distance=%f \t direction=%f \n", val, (double)imsg.distance, (double)imsg.direction);
btcomSendString(tmp);
}
else if (imsg.error > 0)
{
btcomSendString("Receive failed \n");
}
// else imsg.error == -1 -> no message available in the queue
if (imsg.error != -1) i++;
}
}
// no proper role defined...
else
{
int i = 0;
long int j;
while(1)
{
e_led_clear();
for(j = 0; j < 200000; j++)
asm("nop");
e_set_led(i, 1);
for(j = 0; j < 300000; j++)
asm("nop");
i++;
i = i%8;
}
}
ircomStop();
return 0;
}
int obstacleAvoidanceThreshold = 30.0;
int obstacleAvoidanceSpeed = 500.0;
void obstacleAvoidance()
{
// check if an obstacle is perceived
double reading = 0.0;
int obstaclePerceived = 0;
int i=0;
double x = 0.0, y = 0.0;
for (i = 0; i < 8; i++)
{
reading = e_get_calibrated_prox(i);
// if signal above noise
if(reading >= obstacleAvoidanceThreshold)
{
obstaclePerceived = 1;
// compute direction to escape
double signal = reading - obstacleAvoidanceThreshold;
x += -cos(sensorDir[i]) * signal / 8.0;
y += sin(sensorDir[i]) * signal / 8.0;
}
}
// no obstacles to avoid, return immediately
if (obstaclePerceived == 0)
{
// go straight forward
// change movement direction
e_set_speed_left(obstacleAvoidanceSpeed);
e_set_speed_right(obstacleAvoidanceSpeed);
// return obstaclePerceived;
return;
}
double desiredAngle = atan2 (y, x);
double leftSpeed = 0.0;
double rightSpeed = 0.0;
// turn left
if (desiredAngle >= 0.0)
{
leftSpeed = cos(desiredAngle);
rightSpeed = 1;
}
// turn right
else
{
leftSpeed = 1;
rightSpeed = cos(desiredAngle);
}
// rescale values
leftSpeed *= obstacleAvoidanceSpeed;
rightSpeed *= obstacleAvoidanceSpeed;
// change movement direction
e_set_speed_left(leftSpeed);
e_set_speed_right(rightSpeed);
// advertise obstacle avoidance in progress
// return 1;
}
Mmain main.c /^int main()$/
getselector main.c /^int getselector()$/
obstacleAvoidance main.c /^void obstacleAvoidance()$/
obstacleAvoidanceSpeed main.c 169
obstacleAvoidanceThreshold main.c 168
sensorDir main.c 37
!_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 //
getselector main.c /^int getselector()$/;" f
main main.c /^int main()$/;" f
obstacleAvoidance main.c /^void obstacleAvoidance()$/;" f
obstacleAvoidanceSpeed main.c /^int obstacleAvoidanceSpeed = 500.0;$/;" v
obstacleAvoidanceThreshold main.c /^int obstacleAvoidanceThreshold = 30.0;$/;" v
sensorDir main.c /^float sensorDir[NB_IR_SENSORS] = {0.2967, 0.8727, 1.5708, 2.6180, 3.6652, 4.7124, 5.4105, 5.9865};$/;" v
This source diff could not be displayed because it is too large. You can view the blob instead.
This diff is collapsed.
/********************************************************************
* Advance control motor of e-puck *
* December 2004: first version *
* Lucas Meier & Francesco Mondada *
* *
********************************************************************/
/********************************************************************************
Control motor of e-puck with timer 4 and 5
December 2004: first version
Lucas Meier & Francesco Mondada
Version 1.0 november 2005
Michael Bonani
Additions by Alexander Bahr (February 2010)
This file is part of the e-puck library license.
See http://www.e-puck.org/index.php?option=com_content&task=view&id=18&Itemid=45
(c) 2005-2007 Michael Bonani, Francesco Mondada, Lucas Meier
(c) 2010 Alexander Bahr
Robotics system laboratory http://lsro.epfl.ch
Laboratory of intelligent systems http://lis.epfl.ch
Distributed intelligent systems and algorithms laboratory http://disal.epfl.ch
EPFL Ecole polytechnique federale de Lausanne http://www.epfl.ch
**********************************************************************************/
/*! \file
* \ingroup motor_LED
* \brief Manage the motors (with timer2)
* \brief Manage the motors (with timer 4 and 5).
*
* This module manage the motors with the agenda solution (timer2).
* This module manage the motors with two timers: timer4 (motor left)
* and timer5 (motor right).
* \warning You can't use this module to control the motors if you are
* using the camera, because the camera's module also use timer4 and
* timer5.
*
* A little exemple to use the motors with agenda (e-puck turn on himself)
* A little exemple for the motors (e-puck turn on himself)
* \code
* #include <p30f6014A.h>
* #include <motor_led/e_epuck_ports.h>
* #include <motor_led/e_init_port.h>
* #include <motor_led/advance_one_timer/e_motors.h>
* #include <motor_led/advance_one_timer/e_agenda.h>
* #include <motor_led/e_motors.h>
*
* int main(void)
* {
* e_init_port();
* e_init_motors();
* e_set_speed(0, 500);
* e_start_agendas_processing();
* e_set_speed_left(500); //go forward on half speed
* e_set_speed_right(-500); //go backward on half speed
* while(1) {}
* }
* \endcode
* \sa e_agenda.h
* \author Jonathan Besuchet
* \author Code: Michael Bonani, Francesco Mondada, Lucas Meier, Alexander Bahr \n Doc: Jonathan Besuchet
*/
#ifndef _MOTORS
#define _MOTORS
/* internal functions */
//void run_left_motor(void);
//void run_right_motor(void);
// robot geometry // added by bahr
#define STEPS_PER_M 4443 // in [steps per meters] added by bahr NOTE: this is NOT the correct parameter
#define WHEEL_DISTANCE 0.060 // in [meters] added by bahr NOTE: this is NOT the correct parameter
#define DR_NOISE 0 // noise level in percent
#define DR_NOISE_BIAS 0 // noise bias in percent
#define DR_BIAS 0 // dead-reckoning bias in percent
/* functions */
void e_init_motors(void); // init to be done before using the other calls
/* user called function */
void e_init_motors(void); // init to be done before using the other calls
void e_set_speed_left(int motor_speed); // motor speed in steps/s
void e_set_speed_right(int motor_speed); // motor speed in steps/s
long int e_get_steps_left(void); // motors steps done left // modified by bahr
long int e_get_steps_right(void); // motors steps done right // modified by bahr
void e_set_steps_left(long int set_steps); // set motor steps counter // modified by bahr
void e_set_steps_right(long int set_steps); // set motor steps counter // modified by bahr
void e_set_speed_left(int motor_speed); // motor speed: from -1000 to 1000
void e_set_speed_right(int motor_speed); // motor speed: from -1000 to 1000
void e_set_speed(int linear_speed, int angular_speed);
// open loop motor control
int e_get_ol_ctrl_status(void); // added by bahr
void e_set_distance(long int set_distance, int motor_speed); // added by bahr
void e_set_turn(double turn_angle, int motor_speed); // added by bahr
void e_set_steps_left(int steps_left);
void e_set_steps_right(int steps_right);
// dead reckoning
void e_do_dr(void); // added by bahr
void e_set_dr_position(double set_dr_x,double set_dr_y,double set_dr_theta); // added by bahr
void e_get_dr_position(double *get_dr_x,double *get_dr_y,double *get_dr_theta); // added by bahr
int e_get_steps_left();
int e_get_steps_right();
// auxilary routines to convert from a compass heading (0 degrees is y-axis) in degrees to a
// mathematical angle 0 radians is x-axis
double e_deg2rad(double angle_deg); // added by bahr
double e_rad2deg(double angle_rad); // added by bahr
#endif
# Makefile
# ==============================================================================
# 07.02.2002 cmc : Created based on sample from Kevin Nickels.
# 07.15.2004 cmc : Updated and applied to sn_sim project.
# 08.05.2004 cmc : Added tags to removed files in distclean, added "doc" target
# 12.16.2004 cmc : Remove output files with distclean.
# ------------------------------------------------------------------------------
# TODO: Figure out why LD won't link C++ object files with standard args.
# ==============================================================================
# ==============================================================================
# Local variables
# ------------------------------------------------------------------------------
# first, find all the pertinent filenames
ASMSRCS = $(wildcard *.s)
CSRCS = $(wildcard *.c)
SRCS = $(CSRCS) $(ASMSRCS)
HDRS = $(wildcard *.h) $(wildcard *.inc)
OBJS = $(addsuffix .o,$(basename $(SRCS)))
CTAGS = ctags
CC = pic30-elf-gcc
LD = pic30-elf-ld
ASM = pic30-elf-as
CFLAGS = -g -Wall -mcpu=30F6014A
CPPFLAGS = -I../ -I../std_microchip/support/h -I../std_microchip/include -I../epfl
# ==============================================================================
# Dependencies & rules
# ------------------------------------------------------------------------------
all: $(OBJS)
%.o: %.s
$(ASM) -g -I../std_microchip/support/inc -p=30F6014A $< -o $@
# ==============================================================================
# Clean up directory
# ------------------------------------------------------------------------------
.PHONY: clean
clean:
- $(RM) $(OBJS) *~ core.* *.rpo
.PHONY: distclean
distclean: clean
- $(RM) $(DEPS) tags *.a *.so $(OUTPUT)
# ==============================================================================
# ==============================================================================
# make tags files for vim
# ------------------------------------------------------------------------------
tags: $(SRCS) $(HDRS)
$(CTAGS) $(SRCS) $(HDRS)
# ==============================================================================
# ==============================================================================
# end of Makefile
Webots Project File version R2019b
perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000008700ffffff000000010000011c00000245fc0200000001fb0000001400540065007800740045006400690074006f00720100000000000002450000008b00ffffff0000000300000780000001e9fc0100000001fb0000000e0043006f006e0073006f006c00650100000000000007800000008700ffffff000006620000024500000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff00000001000000020000016b000007750100000002010000000101
sceneTreePerspectives: 000000ff0000000100000002000000c0000000fa0100000002010000000201
perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff000000010000027a00000182fc0200000001fb0000001400540065007800740045006400690074006f0072010000001600000182000000a200ffffff00000003000005000000008cfc0100000001fb0000000e0043006f006e0073006f006c00650100000000000005000000005400ffffff000002800000018200000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff00000001000000020000016b000007750100000006010000000101
sceneTreePerspectives: 000000ff0000000100000002000000c0000000fc0100000006010000000201
maximizedDockId: -1
centralWidgetVisible: 1
selectionDisabled: 0
viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 2 "controllers/ctrl1/ctrl1.c" "../../../../../Applications/Webots.app/projects/default/controllers/braitenberg/braitenberg.c" "controllers/ctrl1/inc/utilities.h"
renderingDevicePerspectives: epuck0:camera;1;1;0;0
renderingDevicePerspectives: epuck2:camera;1;1;0;0
renderingDevicePerspectives: epuck4:camera;1;1;0;0
renderingDevicePerspectives: epuck3:camera;1;1;0;0
renderingDevicePerspectives: epuck1:camera;1;1;0;0
textFiles: 0 "controllers/ctrl1/ctrl1.c" "controllers/ctrl1/inc/utilities.h"
renderingDevicePerspectives: epuck0:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck1:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck2:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck4:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck3:camera;1;0.820513;0;0