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)
{
e_init_port();
e_init_motors();
e_init_ad_scan(ALL_ADC);
e_calibrate_ir();
int selector=getselector();
switch (selector) {
case 0:robot_off();break;
case 1:aggressive();break;
case 2:fear();break;
case 3:curious();break;
case 4:love();break;
case 5:robot_off();break;
default:/*own high level behaviour*/break;
}
}
/*! \brief Initialize all the A/D register needed
*
* Set up the different ADC register to process the AD conversion
* by scanning the used AD channels. Each value of the channels will
* be stored in a different AD buffer register and an inturrupt will
* occure at the end of the scan.
* \param only_micro Put MICRO_ONLY to use only the three microphones
* at 33kHz. Put ALL_ADC to use all the stuff using the ADC.
*/
void e_init_ad_scan(unsigned char only_micro)
{
selector = getselector();
if(only_micro == MICRO_ONLY)
micro_only = MICRO_ONLY;
else
micro_only = ALL_ADC;
ADCON1 = 0; //reset to default value
ADCON2 = 0; //reset to default value
ADCON3 = 0; //reset to default value
ADCHS = 0; //reset to default value
// ADPCFGbits.PCFGx
// = 0 for Analog input mode,
// = 1 for digital input mode (default)
if(selector == 10) {
ADPCFGbits.PCFG0 = 0; // ir8, right
ADPCFGbits.PCFG1 = 0; // ir9, left
} else {
ADPCFGbits.PCFG0 = 1; // front and body leds
ADPCFGbits.PCFG1 = 1;
}
ADPCFGbits.PCFG2 = 0; // micro 0
ADPCFGbits.PCFG3 = 0; // micro 1
ADPCFGbits.PCFG4 = 0; // micro 2
ADPCFGbits.PCFG5 = 0; // axe x acc
ADPCFGbits.PCFG6 = 0; // axe y acc
ADPCFGbits.PCFG7 = 0; // axe z acc
ADPCFGbits.PCFG8 = 0; // ir0
ADPCFGbits.PCFG9 = 0; // ir1
ADPCFGbits.PCFG10 = 0; // ir2
ADPCFGbits.PCFG11 = 0; // ir3
ADPCFGbits.PCFG12 = 0; // ir4
ADPCFGbits.PCFG13 = 0; // ir5
ADPCFGbits.PCFG14 = 0; // ir6
ADPCFGbits.PCFG15 = 0; // ir7
//specifie the channels to be scanned
ADCSSLbits.CSSL0 = 0; // ir8 on gumstix ext.
ADCSSLbits.CSSL1 = 0; // ir9 on gumstix ext.
ADCSSLbits.CSSL2 = 1; // micro 0
ADCSSLbits.CSSL3 = 1; // micro 1
ADCSSLbits.CSSL4 = 1; // micro 2
ADCSSLbits.CSSL5 = 0; // axe x acc
ADCSSLbits.CSSL6 = 0; // axe y acc
ADCSSLbits.CSSL7 = 0; // axe z acc
ADCSSLbits.CSSL8 = 0; // ir0
ADCSSLbits.CSSL9 = 0; // ir1
ADCSSLbits.CSSL10 = 0; // ir2
ADCSSLbits.CSSL11 = 0; // ir3
ADCSSLbits.CSSL12 = 0; // ir4
ADCSSLbits.CSSL13 = 0; // ir5
ADCSSLbits.CSSL14 = 0; // ir6
ADCSSLbits.CSSL15 = 0; // ir7
ADCON1bits.FORM = 0; //output = unsigned int
ADCON1bits.ASAM = 1; //automatic sampling on
ADCON1bits.SSRC = 7; //automatic convertion mode
ADCON2bits.SMPI = 3-1; //interupt on 14th sample
ADCON2bits.CSCNA = 1; //scan channel input mode on
ADCON3bits.SAMC = 1; //number of cycle between acquisition and conversion (need 2 for the prox)
if(micro_only)
ADCON3bits.ADCS = 19; //acquisition only for micro = 33kHz
else
ADCON3bits.ADCS = ADCS_3_CHAN; //Tad = (ADCS + SAMC) * Tcy/2 = 2170[ns],
//WARNING: Tad min must be 667 [ns]
IFS0bits.ADIF = 0; //Clear the A/D interrupt flag bit
IEC0bits.ADIE = 1; //Set the A/D interrupt enable bit
ADCON1bits.ADON = 1; //enable AD conversion
//wait for the array to be filled one time
if(!micro_only)
do
{
NOP();
}
while (e_last_acc_scan_id < ACC_SAMP_NB-1);
}
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()
{
// 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;
}
__asm__ volatile ("reset");
}
//system initialization
e_init_port(); // configure port pins
e_init_ad_scan(ALL_ADC);
// sound
e_init_sound();
e_init_motors();
e_calibrate_ir();
e_start_agendas_processing();
// Decide upon program
selector=getselector();
switch (selector) {
case 0:robot_off();break;
case 1:aggressive_c();break;
case 2:fear();break;
case 3:curious();break;
case 4:love();break;
case 5:goal_seeking();break;
case 6:high_level(0);break; //searcher
case 7:high_level(1);break; //hider
default:robot_off();break;
}
return 0;
}
int run_asercom(void) {
static char c1,c2,wait_cam=0;
static int i,j,n,speedr,speedl,positionr,positionl,LED_nbr,LED_action,accx,accy,accz,sound;
static int cam_mode,cam_width,cam_heigth,cam_zoom,cam_size,cam_x1,cam_y1;
static char first=0;
char *ptr;
static int mod, reg, val;
#ifdef IR_RECEIVER
char ir_move = 0,ir_address= 0, ir_last_move = 0;
#endif
static TypeAccSpheric accelero;
//static TypeAccRaw accelero_raw;
int use_bt=0;
//e_init_port(); // configure port pins
//e_start_agendas_processing();
e_init_motors();
//e_init_uart1(); // initialize UART to 115200 Kbaud
//e_init_ad_scan();
selector = getselector(); //SELECTOR0 + 2*SELECTOR1 + 4*SELECTOR2 + 8*SELECTOR3;
if(selector==10) {
use_bt=0;
} else {
use_bt=1;
}
#ifdef FLOOR_SENSORS
if(use_bt) { // the I2C must remain disabled when using the gumstix extension
e_i2cp_init();
}
#endif
#ifdef IR_RECEIVER
e_init_remote_control();
#endif
if(RCONbits.POR) { // reset if power on (some problem for few robots)
RCONbits.POR=0;
RESET();
}
/*read HW version from the eeprom (last word)*/
static int HWversion=0xFFFF;
ReadEE(0x7F,0xFFFE,&HWversion, 1);
/*Cam default parameter*/
cam_mode=RGB_565_MODE;
cam_width=40; // DEFAULT_WIDTH;
cam_heigth=40; // DEFAULT_HEIGHT;
cam_zoom=8;
cam_size=cam_width*cam_heigth*2;
if(use_bt) {
e_poxxxx_init_cam();
//e_po6030k_set_sketch_mode(E_PO6030K_SKETCH_COLOR);
e_poxxxx_config_cam((ARRAY_WIDTH -cam_width*cam_zoom)/2,(ARRAY_HEIGHT-cam_heigth*cam_zoom)/2,cam_width*cam_zoom,cam_heigth*cam_zoom,cam_zoom,cam_zoom,cam_mode);
e_poxxxx_set_mirror(1,1);
e_poxxxx_write_cam_registers();
}
e_acc_calibr();
if(use_bt) {
uart1_send_static_text("\f\a"
"WELCOME to the SerCom protocol on e-Puck\r\n"
"the EPFL education robot type \"H\" for help\r\n");
} else {
uart2_send_static_text("\f\a"
"WELCOME to the SerCom protocol on e-Puck\r\n"
"the EPFL education robot type \"H\" for help\r\n");
}
while(1) {
if(use_bt) {
while (e_getchar_uart1(&c)==0)
#ifdef IR_RECEIVER
{
ir_move = e_get_data();
ir_address = e_get_address();
if (((ir_address == 0)||(ir_address == 8))&&(ir_move!=ir_last_move)){
switch(ir_move) {
case 1:
speedr = SPEED_IR;
speedl = SPEED_IR/2;
break;
case 2:
speedr = SPEED_IR;
speedl = SPEED_IR;
break;
case 3:
speedr = SPEED_IR/2;
speedl = SPEED_IR;
break;
case 4:
speedr = SPEED_IR;
speedl = -SPEED_IR;
break;
case 5:
speedr = 0;
speedl = 0;
break;
case 6:
speedr = -SPEED_IR;
speedl = SPEED_IR;
break;
case 7:
speedr = -SPEED_IR;
speedl = -SPEED_IR/2;
break;
case 8:
speedr = -SPEED_IR;
speedl = -SPEED_IR;
break;
case 9:
speedr = -SPEED_IR/2;
speedl = -SPEED_IR;
break;
case 0:
if(first==0){
e_init_sound();
first=1;
}
e_play_sound(11028,8016);
break;
default:
speedr = speedl = 0;
}
ir_last_move = ir_move;
e_set_speed_left(speedl);
e_set_speed_right(speedr);
}
}
#else
;
#endif
} else {
while (e_getchar_uart2(&c)==0)
#ifdef IR_RECEIVER
{
ir_move = e_get_data();
ir_address = e_get_address();
if (((ir_address == 0)||(ir_address == 8))&&(ir_move!=ir_last_move)){
switch(ir_move) {
case 1:
speedr = SPEED_IR;
speedl = SPEED_IR/2;
break;
case 2:
speedr = SPEED_IR;
speedl = SPEED_IR;
break;
case 3:
speedr = SPEED_IR/2;
speedl = SPEED_IR;
break;
case 4:
speedr = SPEED_IR;
speedl = -SPEED_IR;
break;
case 5:
speedr = 0;
speedl = 0;
break;
case 6:
speedr = -SPEED_IR;
speedl = SPEED_IR;
break;
case 7:
speedr = -SPEED_IR;
speedl = -SPEED_IR/2;
break;
case 8:
speedr = -SPEED_IR;
speedl = -SPEED_IR;
break;
case 9:
speedr = -SPEED_IR/2;
speedl = -SPEED_IR;
break;
case 0:
if(first==0){
e_init_sound();
first=1;
}
e_play_sound(11028,8016);
break;
default:
speedr = speedl = 0;
}
ir_last_move = ir_move;
e_set_speed_left(speedl);
e_set_speed_right(speedr);
}
}
#else
;
#endif
}
if (c<0) { // binary mode (big endian)
i=0;
do {
switch(-c) {
case 'a': // Read acceleration sensors in a non
// filtered way, some as ASCII
accx = e_get_acc_filtered(0, 1);
accy = e_get_acc_filtered(1, 1);
accz = e_get_acc_filtered(2, 1);
//accx = e_get_acc(0); //too much noisy
//accy = e_get_acc(1);
//accz = e_get_acc(2);
buffer[i++] = accx & 0xff;
buffer[i++] = accx >> 8;
buffer[i++] = accy & 0xff;
buffer[i++] = accy >> 8;
buffer[i++] = accz & 0xff;
buffer[i++] = accz >> 8;
/*
accelero_raw=e_read_acc_xyz();
ptr=(char *)&accelero_raw.acc_x;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr++;
ptr=(char *)&accelero_raw.acc_y;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr++;
ptr=(char *)&accelero_raw.acc_z;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr++;
*/
break;
case 'A': // read acceleration sensors
accelero=e_read_acc_spheric();
ptr=(char *)&accelero.acceleration;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr=(char *)&accelero.orientation;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr=(char *)&accelero.inclination;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
break;
case 'b': // battery ok?
buffer[i++] = BATT_LOW;
break;
case 'D': // set motor speed
if(use_bt) {
while (e_getchar_uart1(&c1)==0);
while (e_getchar_uart1(&c2)==0);
} else {
while (e_getchar_uart2(&c1)==0);
while (e_getchar_uart2(&c2)==0);
}
speedl=(unsigned char)c1+((unsigned int)c2<<8);
if(use_bt) {
while (e_getchar_uart1(&c1)==0);
while (e_getchar_uart1(&c2)==0);
} else {
while (e_getchar_uart2(&c1)==0);
while (e_getchar_uart2(&c2)==0);
}
speedr=(unsigned char)c1+((unsigned int)c2<<8);
e_set_speed_left(speedl);
e_set_speed_right(speedr);
break;
case 'E': // get motor speed
buffer[i++] = speedl & 0xff;
buffer[i++] = speedl >> 8;
buffer[i++] = speedr & 0xff;
buffer[i++] = speedr >> 8;
break;
case 'I': // get camera image
if(use_bt) {
e_poxxxx_launch_capture(&buffer[i+3]);
wait_cam=1;
buffer[i++]=(char)cam_mode&0xff;//send image parameter
buffer[i++]=(char)cam_width&0xff;
buffer[i++]=(char)cam_heigth&0xff;
i+=cam_size;
}
break;
case 'L': // set LED
if(use_bt) {
while (e_getchar_uart1(&c1)==0);
while (e_getchar_uart1(&c2)==0);
} else {
while (e_getchar_uart2(&c1)==0);
while (e_getchar_uart2(&c2)==0);
}
switch(c1) {
case 8:
if(use_bt) {
e_set_body_led(c2);
}
break;
case 9:
if(use_bt) {
e_set_front_led(c2);
}
break;
default:
e_set_led(c1,c2);
break;
}
break;
case 'M': // optional floor sensors
#ifdef FLOOR_SENSORS
if(use_bt) {
e_i2cp_init();
e_i2cp_enable();
e_i2cp_read(0xC0, 0);
for(j = 0; j < 6; j++) {
if (j % 2 == 0) buffer[i++] = e_i2cp_read(0xC0, j + 1);
else buffer[i++] = e_i2cp_read(0xC0, j - 1);
}
#ifdef CLIFF_SENSORS
for(j=13; j<17; j++) {
if (j % 2 == 0) buffer[i++] = e_i2cp_read(0xC0, j - 1);
else buffer[i++] = e_i2cp_read(0xC0, j + 1);
}
#endif
e_i2cp_disable();
}
#else
for(j=0;j<6;j++) buffer[i++]=0;
#endif
break;
case 'N': // read proximity sensors
if(use_bt) {
for(j=0;j<8;j++) {
n=e_get_calibrated_prox(j); // or ? n=e_get_prox(j);
buffer[i++]=n&0xff;
buffer[i++]=n>>8;
}
} else {
for(j=0;j<10;j++) {
n=e_get_calibrated_prox(j); // or ? n=e_get_prox(j);
buffer[i++]=n&0xff;
buffer[i++]=n>>8;
}
}
break;
case 'O': // read light sensors
if(use_bt) {
for(j=0;j<8;j++) {
n=e_get_ambient_light(j);
buffer[i++]=n&0xff;
buffer[i++]=n>>8;
}
} else {
for(j=0;j<10;j++) {
n=e_get_ambient_light(j);
buffer[i++]=n&0xff;
buffer[i++]=n>>8;
}
}
break;
case 'Q': // read encoders
n=e_get_steps_left();
buffer[i++]=n&0xff;
buffer[i++]=n>>8;
n=e_get_steps_right();
buffer[i++]=n&0xff;
buffer[i++]=n>>8;
break;
case 'u': // get last micro volumes
n = e_get_micro_volume(0);
buffer[i++] = n & 0xff;
buffer[i++] = n >> 8;
n = e_get_micro_volume(1);
buffer[i++] = n & 0xff;
buffer[i++] = n >> 8;
n = e_get_micro_volume(2);
buffer[i++] = n & 0xff;
buffer[i++] = n >> 8;
break;
case 'U': // get micro buffer
ptr=(char *)e_mic_scan;
if(use_bt) {
e_send_uart1_char(ptr,600);//send sound buffer
} else {
e_send_uart2_char(ptr,600);//send sound buffer
}
n=e_last_mic_scan_id;//send last scan
buffer[i++]=n&0xff;
break;
default: // silently ignored
break;
}
if(use_bt) {
while (e_getchar_uart1(&c)==0); // get next command
} else {
while (e_getchar_uart2(&c)==0); // get next command
}
} while(c);
