int main() {
// char bufferDebug[80];
int selector;
//init
e_init_port();
e_init_uart1();
e_init_motors();
selector=getselector();
if (selector==0) {
run_breitenberg_follower();
} else if (selector==1) {
finding_light();
} else if (selector==2) {
avoid_light();
} else if (selector==3) {
run_breitenberg_shocker();
} else if (selector==4) {
followHand();
}else{
}
while(1);
}
int main(void)
{
int i;
char event;
e_init_port(); //Configure port pins
e_init_uart1(); //Initialize UART to 115200Kbaud
if(RCONbits.POR) //Reset if Power on (some problem for few robots)
{
RCONbits.POR=0;
__asm__ volatile ("reset");
}
int main() {
e_init_port();
e_init_uart1();
e_led_clear();
e_activate_agenda(e_blink_led0, 2500);
e_activate_agenda(radio, 100);
e_start_agendas_processing();
while(1){
for ( delay = 0 ; delay < 15000; delay++){
__asm__("nop");
}
}
return 0;
}
int main(void)
{
char message[50];
char command[30], response[80];
int c;
int i, version,j;
int nb_bt_device,error;//max 10
e_init_port(); //Configure port pins
e_init_uart1(); //Initialize UART to 115200Kbaud
e_init_uart2(); //Initialize UART to 115200Kbaud
if(RCONbits.POR) //Reset if Power on (some problem for few robots)
{
RCONbits.POR=0;
__asm__ volatile ("reset");
}
// Main program
int main() {
int speedl = 0;
int speedr = 0;
int oldSpeedl = 0;
int oldSpeedr = 0;
int counter = 0;
int step = 0;
int avdStep = 0;
int state = STATE;
int oldState = STATE;
int sensor[8], value, maxSensorValue;
int i;
int trajl[TRAJ_LEN] = {6,9,9,6,
7,6,3,3,
6,6,6,3,
3,3,6,6,
6,3,3};
int trajr[TRAJ_LEN] = {6,3,3,6,
7,6,9,9,
6,6,6,9,
9,9,6,6,
6,9,9};
// Initialize system and sensors
e_init_port();
e_init_uart1();
e_init_motors();
e_init_ad_scan();
// Reset if Power on (some problem for few robots)
if (RCONbits.POR) {
RCONbits.POR = 0;
__asm__ volatile ("reset");
}
int main()
{
e_init_port();
e_init_uart1();
int i;
for (i = 0; i < 14; i++)
{
char c;
while (!e_ischar_uart1());
e_getchar_uart1(&c);
}
while (1)
{
char c;
while (!e_ischar_uart1());
e_getchar_uart1(&c);
e_send_uart1_char(&c, 1);
while (e_uart1_sending());
}
return 0;
}
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)
{
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 main(void)
{
int cam_mode,cam_x1,cam_y1,cam_width,cam_heigth,cam_zx,cam_zy;
if(getSelector() == 0)
return;
char c;
int i;//buff_length;
//int wait_cam;
//defining the position of the several inputs and outputs (motors are outputs) in the respective SFR
//the SFR are programed as structures, so accessing to an input/output implies only acessing to the field of the structure corresponding to the SFR that was
//assigned to that input/output (see epuck_ports.h and p30f6014.h to understand the SFR assignment)
e_init_port();
e_init_motors();
//important to enable uart interface
e_init_uart1();
e_init_ad_scan(ALL_ADC);
e_calibrate_ir();
//initial configuration of the camera
cam_x1=(ARRAY_WIDTH/Z_WIDTH-WIDTH)/2;
cam_y1=(ARRAY_HEIGHT/Z_HEIGHT-HEIGHT)/2;
cam_width=WIDTH;
cam_heigth=HEIGHT;
cam_zx=Z_WIDTH;
cam_zy=Z_HEIGHT;
cam_mode=MODE;
if(cam_mode==GREY_SCALE_MODE)
cam_size=cam_width*cam_heigth;
else
cam_size=cam_width*cam_heigth*2;
//not waiting for camera
wait_cam=0;
e_activate_agenda(updateFlag, 500);//500//1000
e_activate_agenda(readValues, 10);
e_start_agendas_processing();
keepFinding = 0;
e_set_led(4,1);
keepFinding = 1;
int s = getSelector();
while(s==getSelector());
/*while(1){
sprintf(b1,"%i\n",e_get_calibrated_prox(S_FRONT_LEFT));
e_send_uart1_char(b1,10);
while(e_uart1_sending());
}*/
while(1){
while(keepFinding) {
if(!cameraOn) {
startCamera();
correctRobot();
}
while(!captura());
//e_send_uart1_char(buffer,buff_length);
//while(e_uart1_sending());
if(cameraOn > 5)
processaImagem();
cameraOn++;
}
if(cameraOn)
stopCamera();
while(!flag);
flag = 0;
sendInputs();
readOrder();
readOrder();
e_set_speed_left(speedLeft);
e_set_speed_right(speedRight);
e_set_led(4,0);
}
return 0;
}
int main() {
/*system initialization */
e_init_port();
/* Init UART1 for bluetooth */
e_init_uart1();
/* Init UART2 for e-randb */
e_init_uart2();
/* Init IR */
e_init_prox();
//init motors
e_init_motors();
/* Wait for a command comming from bluetooth IRCOMTEST on pc directory*/
btcomWaitForCommand('s');
/* Start agendas processing which will take care of UART interruptions */
e_start_agendas_processing();
/* Init E-RANDB board */
e_init_randb();
/* Range is tunable by software.
* 0 -> Full Range (1m. approx depending on light conditions )
* 255 --> No Range (0cm. approx, depending on light conditions */
e_randb_uart_set_range(0);
/* At some point we tought that the board could just take
* data en leave the calculations for the robot.
* At the moment, it is better to allow the board to do the calculations */
e_randb_uart_set_calculation(ON_BOARD);
/* Store light conditions to use them as offset for the calculation
* of the range and bearing */
e_randb_uart_store_light_conditions();
e_randb_set_uart_communication(1);
finalDataRegister data;
//tabla comunciacion
double comunicacionAngulos[2];
int comunicacionRangos[2];
//subsuncion
int CURRENT_STATE;
int subsuncion[2][2];
int debug_var = 0;
subsuncion[0][0]=SPACING;
subsuncion[1][0]=COHESION;
int i;
for (i=0;i<2;i++){
subsuncion[i][1]=0;
}
//proximity sensors reading
int prox_first_reading[8];
int prox_reading[8];
/* Angles in rad for IRs starting at 0. Left direction. */
const double prox_directions[8] = {5.9865, 5.4105, 4.7124, 3.6652, 2.6180, 1.5708, 0.8727, 0.2967};
/* Get the first reading to take ambient light */
for(i=0; i < 8; i++){
prox_first_reading[i]=e_get_prox(i);
}
/* Print on the bluetooth */
char tmp2[50];
sprintf(tmp2,"-- CHASER --\n");
btcomSendString(tmp2);
while(1) {
//comprobacion proximidad
int maxProx = 0;
/* Get readings and substract the first reading */
for(i=0; i < 8; i++){
prox_reading[i] = e_get_prox(i) - prox_first_reading[i];
if(prox_reading[i] < 0) {prox_reading[i] = 0; }
if ( prox_reading[i]>maxProx){
maxProx = prox_reading[i];
}
}
if(maxProx > PROX_THRES){
subsuncion[0][1]=1; // Collission
}
else{ // Chasing
subsuncion[0][1]=0;
}
CURRENT_STATE = 1; //by default. chasing
for(i=0;i<2;i++){
if(subsuncion[i][1]==1){
CURRENT_STATE = i;
break;
}
}
char tmp[30];
double vector_repelent[2] = {0.0,0.0};
double ang_repelent;
double ang_comunicacion = 0;
if (e_randb_get_data_uart2(&data)){
//actualizar tabla comun
if((data.bearing > -2*PI) && (data.bearing < 2*PI)){
switch(data.data)
{
case 0:
comunicacionAngulos[0]=data.bearing;
comunicacionRangos[0]=data.range;
sprintf(tmp2,"Sigue Lider. Ang: %f, Rango: %f \n", data.bearing, data.range);
btcomSendString(tmp2);
break;
case 1:
comunicacionAngulos[1]=data.bearing;
comunicacionRangos[1]=data.range;
sprintf(tmp2,"Sigue Sucker. Ang: %f, Rango: %f \n", data.bearing, data.range);
btcomSendString(tmp2);
break;
}
}
}
switch(CURRENT_STATE){
case 0: //Collission
sprintf(tmp2,"-- COLISION max= %d --\n",maxProx);
btcomSendString(tmp2);
/* Calc vector Sum */
vector_repelent[0] = 0.0;
vector_repelent[1] = 0.0;
for (i = 0 ; i < 8; i ++ )
{
vector_repelent[0] += prox_reading[i] * cos ( prox_directions[i] );
vector_repelent[1] += prox_reading[i] * sin ( prox_directions[i] );
}
/* Calc pointing angle */
ang_repelent = atan2(vector_repelent[1], vector_repelent[0]);
/* Create repelent angle */
ang_repelent -= PI;
//calculate and set velocity
setAngularVelocity(ang_repelent,1);
break; // Case 0
case 1: // Chasing
sprintf(tmp2,"-- PERSIGUIENDO--\n");
btcomSendString(tmp2);
ang_comunicacion = (comunicacionAngulos[0] + comunicacionAngulos[1])/2;
if(ang_comunicacion>0.6 || ang_comunicacion<-0.6){ // Chasing
sprintf(tmp2,"-- GIRANDO ang= %02f --\n",ang_comunicacion);
btcomSendString(tmp2);
//calculate and set velocity
setAngularVelocity(ang_comunicacion,2);
}
else { // Walk
sprintf(tmp2,"-- RECTO--\n");
btcomSendString(tmp2);
e_set_speed_left(SPEED);
e_set_speed_right(SPEED);
}
break; // Case 1
} // End switch
//if (e_randb_get_data_uart2(&data)){
// sprintf(tmp,"%d %02f %d %2f %d\n",debug_var, data.bearing, data.range);
// btcomSendString(tmp);
/* Send the data through one sensor */
//e_randb_uart_send_all_data(data);
}
return 0;
}
int main()
{
// init robot
e_init_port();
e_init_ad_scan();
e_init_uart1();
e_led_clear();
e_init_motors();
e_start_agendas_processing();
// initialise buffer
int k;
for (k = 0; k < NB_NEIGHBOURS; k++)
neighbours[k].id = -1;
// wait for s to start
/* btcomWaitForCommand('s'); */
btcomSendString("-OK-\n");
e_calibrate_ir();
// initialize ircom, then rng and start listening
ircomStart();
//ircomEnableContinuousListening();
//ircomEnableProximity();
initRandomNumberGenerator();
// after rng init we can disable prox sensors
//ircomDisableProximity();
ircomEnableContinuousListening();
ircomListen();
id = getselector();
ircomResetTime();
lastClock = ircomGetTime();
// activate movement
//e_activate_agenda(move, 2500);
organiseInit();
// advertise current direction
e_led_clear();
e_set_led(0,1);
e_set_led(4,1);
while(1)
{
int messageReceived = 0;
while(((ircomGetTime() - lastClock < COM_CYCLE_SPEED) || (ircomIsReceiving() == 1)))
{
IrcomMessage msg;
ircomPopMessage(&msg);
if (msg.error == 0)
{
processNewMessage(&msg);
messageReceived = 1;
SetRobotSeen(msg);
}
}
e_set_body_led(messageReceived);
SendData();
//sendId();
//sendAngleToNeighbours();
lastClock = ircomGetTime();
e_set_body_led(0);
}
ircomStop();
return 0;
}
