/*! \brief The "main" function of the follower demo */
void run_breitenberg_follower(void)
{
e_init_port();
e_init_motors();
e_init_ad_scan(ALL_ADC);
e_calibrate_ir();
e_activate_agenda(k2000_led, 2500);
e_activate_agenda(follow_neuron, 650);
e_start_agendas_processing();
while(1);
}
/*! \brief The "main" function of the avoider demo */
void run_breitenberg_shocker(void)
{
e_init_port();
e_init_motors();
e_init_ad_scan(ALL_ADC);
e_calibrate_ir();
e_activate_agenda(flow_led, 900);
e_activate_agenda(shock_neuron, 650);
e_start_agendas_processing();
while(1);
}
void robot_off(void)
{
e_init_port();
e_init_motors();
e_init_ad_scan(ALL_ADC);
e_calibrate_ir();
e_start_agendas_processing();
while (1) {
e_led_clear();
wait(10000);
}
}
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;
}
void comm_init(unsigned char seed, unsigned char ID)
{
int i;
e_init_randb(I2C); // IR ring in I2C mode
e_randb_set_range(0); // Transmit IR with full power (0xff is min power)
e_randb_store_light_conditions(); // Calibrate background IR levels
/* Pre-compute random values for communication backoff */
srand(seed + TMR1);
for (i = 0; i < RANDOM_BACKOFF_SIZE; i++)
random_backoff[i] = rand() % MAX_SEND_TIMER_BACKOFF;
tx_buffer.bits.ID = ID;
init_timer3(); // Start broadcasting the contents of tx_buffer
e_start_agendas_processing();
}
void e_init_randb ( unsigned char mode )
{
if ( mode == I2C )
{
/* Init I2C */
e_i2cp_init();
e_i2cp_enable();
}
else
{
/* Init UART2 */
e_init_uart2();
/* Clean UART buffer */
char msg;
while(e_getchar_uart2(&msg));
/* Start Agendas */
e_start_agendas_processing();
/* Locate e-randb task */
e_activate_agenda(e_randb_get_uart2, 2);
/* Tell the board we work on UART mode */
e_randb_set_uart_communication(UART);
}
/* Calculations are made on the BOARD*/
calcOnBoard = TRUE;
/* Init Global variables */
erandbFinished = FALSE;
erandbState = WAITING;
erandbCounter = 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;
}
/*! \brief The "main" function of the program */
void run_wallfollow() {
int leftwheel, rightwheel; // motor speed left and right
int distances[NB_SENSORS]; // array keeping the distance sensor readings
int i; // FOR-loop counters
int gostraight;
int loopcount;
unsigned char selector_change;
e_init_port();
e_init_ad_scan(ALL_ADC);
e_init_motors();
e_start_agendas_processing();
//follow_sensor_calibrate();
e_activate_agenda(left_led, 2500);
e_activate_agenda(right_led, 2500);
e_pause_agenda(left_led);
e_pause_agenda(right_led);
e_calibrate_ir();
loopcount=0;
int selectionValue;
// selector_change = !(followgetSelectorValue() & 0x0001);
e_set_steps_right(0);
selectionValue = followgetSelectorValue();
while (doesWallExist(selectionValue)) {
followGetSensorValues(distances); // read sensor values
gostraight=0;
if (selectionValue == 1) {
//// if(selector_change == LEFT_FOLLOW) {
// selector_change = RIGHT_FOLLOW;
e_led_clear();
e_pause_agenda(left_led);
e_restart_agenda(right_led);
// }
for (i=0; i<8; i++) {
if (distances[i]>50) {break;}
}
if (i==8) {
gostraight=1;
} else {
follow_weightleft[0]=-10;
follow_weightleft[7]=-10;
follow_weightright[0]=10;
follow_weightright[7]=10;
if (distances[2]>300) {
distances[1]-=200;
distances[2]-=600;
distances[3]-=100;
}
}
} else {
// if(selector_change == RIGHT_FOLLOW) {
// selector_change = LEFT_FOLLOW;
e_led_clear();
e_pause_agenda(right_led);
e_restart_agenda(left_led);
// }
for (i=0; i<8; i++) {
if (distances[i]>50) {break;}
}
if (i==8) {
gostraight=1;
} else {
follow_weightleft[0]=10;
follow_weightleft[7]=10;
follow_weightright[0]=-10;
follow_weightright[7]=-10;
if (distances[5]>300) {
distances[4]-=100;
distances[5]-=600;
distances[6]-=200;
}
}
}
leftwheel=BIAS_SPEED;
rightwheel=BIAS_SPEED;
if (gostraight==0) {
for (i=0; i<8; i++) {
leftwheel+=follow_weightleft[i]*(distances[i]>>4);
rightwheel+=follow_weightright[i]*(distances[i]>>4);
}
}
// set robot speed
followsetSpeed(leftwheel, rightwheel);
wait(15000);
//doesWallExist(selectionValue);
}
}
/*! \brief The "main" function of the demo */
void run_accelerometer() {
int accx, accy, accz;
long ampl;
long amplavg;
// char buffer[80];
int state;
int lednum;
double angle;
int soundsel;
// Init sound
e_init_port();
e_init_ad_scan(ALL_ADC);
e_init_sound();
#ifdef MOVE
e_init_motors();
e_start_agendas_processing();
#endif
// Calibrate accelerometers
e_set_led(8, 1);
e_acc_calibr();
e_set_led(8, 0);
#ifdef MOVE
// Move forwards
e_set_speed_left(100);
e_set_speed_right(100);
#endif
// Detect free fall and shocks
state=STATE_NORMAL;
amplavg=1000;
while (1) {
accx = e_get_acc(0);
accy = e_get_acc(1);
accz = e_get_acc(2) + 744; //744 is 1g
if ((accz<0) && (accz>-600)) {accz=0;}
ampl=((long)(accx)*(long)(accx))+((long)(accy)*(long)(accy))+((long)(accz)*(long)(accz));
amplavg=(amplavg>>2)+ampl;
if (! e_dci_unavailable) {
if (state!=STATE_NORMAL) {
state=STATE_NORMAL;
e_set_led(8, 0);
e_set_body_led(0);
}
}
if (amplavg<1000) {
if (state!=STATE_FREEFALL) {
state=STATE_FREEFALL;
e_stop_flag=1;
while (e_dci_unavailable);
// sprintf(buffer, "Free fall: %ld, (%d, %d, %d) -> (%ld)\r\n", amplavg, accx, accy, accz, ampl);
// e_send_uart1_char(buffer, strlen(buffer));
e_play_sound(11028, 8016);
e_set_body_led(1);
e_set_led(8, 0);
}
} else if (amplavg>4000000) {
if (state!=STATE_SHOCK) {
state=STATE_SHOCK;
e_stop_flag=1;
while (e_dci_unavailable);
// sprintf(buffer, "Shock: %ld, (%d, %d, %d) -> (%ld)\r\n", amplavg, accx, accy, accz, ampl);
// e_send_uart1_char(buffer, strlen(buffer));
soundsel=(accx & 3);
if (soundsel==0) {
e_play_sound(0, 2112);
} else if (soundsel==1) {
e_play_sound(2116, 1760);
} else {
e_play_sound(3878, 3412);
}
e_set_body_led(0);
angle=atan2(accy, accx);
lednum=floor(atan2(accy, accx)/PI*4+PI/2+PI/8);
while (lednum>8) {lednum=lednum-8;}
while (lednum<0) {lednum=lednum+8;}
// sprintf(buffer, "(x=%d, y=%d) -> angle=%f, led=%d\r\n", accx, accy, angle, lednum);
// e_send_uart1_char(buffer, strlen(buffer));
e_set_led(lednum, 1);
}
}
}
}
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;
}
void reacttoprox()
{
int proxy0;
int proxy1;
int proxy2;
int proxy3;
int proxy4;
int proxy5;
int proxy6;
int proxy7;
e_init_motors();
e_init_port();
e_init_sound();
e_set_speed_left(0);
e_set_speed_right(0);
e_start_agendas_processing();
e_init_ad_scan(ALL_ADC);
while(1)
{
long i;
proxy0 = e_get_prox(0);
proxy1 = e_get_prox(1);
proxy2 = e_get_prox(2);
proxy3 = e_get_prox(3);
proxy4 = e_get_prox(4);
proxy5 = e_get_prox(5);
proxy6 = e_get_prox(6);
proxy7 = e_get_prox(7);
if(proxy0 > 100){
e_set_speed_left(1000);
e_set_speed_right(800);
LED0 = 1;
LED1 = 0;
LED2 = 0;
LED3 = 0;
LED4 = 0;
LED5 = 0;
LED6 = 0;
LED7 = 0;
}
else if(proxy1 > 100){
LED0 = 1;
LED1 = 1;
LED2 = 1;
LED3 = 1;
LED4 = 1;
LED5 = 1;
LED6 = 1;
LED7 = 1;
e_set_speed_left(0);
e_set_speed_right(0);
long j;
e_play_sound(7294, 3732);
for(j=0; j<4000000; j++) {asm("nop");}
}
else if(proxy2 > 100){
e_set_speed_left(1000);
LED0 = 0;
LED1 = 0;
LED2 = 1;
LED3 = 0;
LED4 = 0;
LED5 = 0;
LED6 = 0;
LED7 = 0;
}
else if(proxy3 > 100){
e_set_speed_left(-1000);
LED0 = 0;
LED1 = 0;
LED2 = 0;
LED3 = 1;
LED4 = 0;
LED5 = 0;
LED6 = 0;
LED7 = 0;
}
else if(proxy4 > 100){
e_set_speed_right(-1000);
LED0 = 0;
LED1 = 0;
LED2 = 0;
LED3 = 0;
LED4 = 1;
LED5 = 0;
LED6 = 0;
LED7 = 0;
}
else if(proxy5 > 100){
e_set_speed_right(1000);
LED0 = 0;
LED1 = 0;
LED2 = 0;
LED3 = 0;
LED4 = 0;
LED5 = 1;
LED6 = 0;
LED7 = 0;
}
else if(proxy6 > 100){
e_set_speed_right(1000);
LED0 = 0;
LED1 = 0;
LED2 = 0;
LED3 = 0;
LED4 = 0;
LED5 = 0;
LED6 = 1;
LED7 = 0;
}
else if(proxy7 > 100){
e_set_speed_right(1000);
e_set_speed_left(800);
LED0 = 0;
LED1 = 0;
LED2 = 0;
LED3 = 0;
LED4 = 0;
LED5 = 0;
LED6 = 0;
LED7 = 1;
}
else {
e_set_speed_left(0);
e_set_speed_right(0);
LED0 = 0;
LED1 = 0;
LED2 = 0;
LED3 = 0;
LED4 = 0;
LED5 = 0;
LED6 = 0;
LED7 = 0;
}
}
}
void aggressive(void)
{
long i;
int left, right;
// e_init_sound();
e_start_agendas_processing();
e_set_speed_left(500);
e_set_speed_right(500);
while(1) {
e_set_led(1,1);
// If robot is stuck, use front 4 sensors to confirm in range, do a degree spin to try get unstuck
//if front left sensors, turn right
if (e_get_prox(0) > 800 || e_get_prox(1) > 800) {
if (e_get_prox(0) > 800) {
e_set_led(0,1);
}
if (e_get_prox(1) > 800) {
e_set_led(1,1);
}
// Do a 90 degree spin,
left=-450;right=450;
e_set_speed_right(right);
e_set_speed_left(left);
for(i=0;i<40000;i++) {asm("nop");}
// set back to normal speed
e_set_speed_left(500);
e_set_speed_right(500);
//clear all LED lights
e_led_clear();
}
//if front right sensors, turn left
if (e_get_prox(6) > 800 || e_get_prox(7) > 800) {
if (e_get_prox(6) > 800) {
e_set_led(7,1);
}
if (e_get_prox(7) > 800) {
e_set_led(7,1);
}
left=450;right=-450;
e_set_speed_right(right);
e_set_speed_left(left);
for(i=0;i<40000;i++) {asm("nop");}
// set back to normal speed
e_set_speed_left(500);
e_set_speed_right(500);
//clear all LED lights
e_led_clear();
}
//if back sensors, turn to face
if (e_get_prox(3) > 800 || e_get_prox(5) > 800) {
/*// Run away fast with fear flash
e_set_led(0,1);
e_set_led(1,1);
e_set_led(2,1);
e_set_led(3,1);
e_set_led(4,1);
e_set_led(5,1);
e_set_led(6,1);
e_set_led(7,1);
e_play_sound(11028, 8016);
*/
e_set_speed_left(1500);
e_set_speed_right(-1500);
for(i=0;i<10000;i++) {asm("nop");}
// Set back to normal speed
//e_led_clear();
e_set_speed_left(500);
e_set_speed_right(500);
}
//camera code
}
}
