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);
}
/*! \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(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;
}
}
void run_braitenberg() {
int i;
int sensor;
char buffer[80];
int leftwheel, rightwheel;
// Init sensors
e_init_port();
e_init_motors();
e_init_prox();
// Calibrate sensors
e_set_led(8, 1);
braitenberg_sensor_calibrate();
e_set_led(8, 0);
//
while (1) {
leftwheel=200;
rightwheel=200;
for (i=0; i<8; i++) {
sensor=e_get_prox(i)-braitenberg_sensorzero[i];
sprintf(buffer, "%d, ", sensor);
e_send_uart1_char(buffer, strlen(buffer));
leftwheel+=braitenberg_weightleft[i]*(sensor>>4);
rightwheel+=braitenberg_weightright[i]*(sensor>>4);
}
sprintf(buffer, "setspeed %d %d\r\n", leftwheel, rightwheel);
e_send_uart1_char(buffer, strlen(buffer));
if (leftwheel>800) {leftwheel=800;}
if (rightwheel>800) {rightwheel=800;}
if (leftwheel<-800) {leftwheel=-800;}
if (rightwheel<-800) {rightwheel=-800;}
e_set_speed_left(leftwheel);
e_set_speed_right(rightwheel);
wait(100000);
}
}
// 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");
}
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;
e_init_port();
e_init_motors();
//e_init_ad_scan(ALL_ADC);
//e_init_ad_scan();
follow_sensor_calibrate();
loopcount=0;
while (1) {
followGetSensorValues(distances); // read sensor values
gostraight=0;
if ((followgetSelectorValue() & 0x0001) == RIGHT_FOLLOW) {
e_set_led(6,0);
e_set_led(2,1);
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 {
e_set_led(6,1);
e_set_led(2,0);
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);
}
}
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;
}
}
}
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);
