/*! \brief Looking at the selector value
* \return The selector value from 0 to 15
*/
int followgetSelectorValue() {
if(e_get_prox(0) > e_get_prox(7))
{
return 1;
}
return 0;
}
void follow_sensor_calibrate() {
int i, j;
extern char buffer[60];
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++) {
follow_sensorzero[i]=(sensor[i]>>5);
sprintf(buffer, "%d, ", follow_sensorzero[i]);
e_send_uart1_char(buffer, strlen(buffer));
}
sprintf(buffer, " calibration done\r\n");
e_send_uart1_char(buffer, strlen(buffer));
wait(100000);
}
void run_SensDispl() {
int cam_mode,cam_width,cam_heigth,cam_zoom,cam_size;
int i;
unsigned long cam_light;
unsigned char *buf_ptr;
/*Cam default parameter*/
cam_mode=GREY_SCALE_MODE;
cam_width=20;
cam_heigth=20;
cam_zoom=8;
cam_size=cam_width*cam_heigth;
e_poxxxx_init_cam();
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();
while (1) {
e_poxxxx_launch_capture(&buffer[0]); // start camera capture
e_led_clear();
e_set_body_led(0);
e_set_front_led(0);
if (e_get_micro_volume(0)>30 || e_get_micro_volume(1)>30 || e_get_micro_volume(2)>30)
e_set_body_led(1);
if (e_get_prox(0)>400)
e_set_led(0,1);
if (e_get_prox(1)>400)
e_set_led(1,1);
if (e_get_prox(2)>400)
e_set_led(2,1);
if (e_get_prox(3)>400)
e_set_led(3,1);
if (e_get_prox(4)>400)
e_set_led(4,1);
if (e_get_prox(5)>400)
e_set_led(5,1);
if (e_get_prox(6)>400)
e_set_led(6,1);
if (e_get_prox(7)>400)
e_set_led(7,1);
while(!e_poxxxx_is_img_ready()); // wait end of capture
cam_light=0;
buf_ptr=(unsigned char*)&buffer[0];
for (i=0; i<cam_size; i++) {
cam_light+=*buf_ptr;
buf_ptr++;
}
sprintf(buffer, "Cam light %lu\r\n", cam_light);
e_send_uart1_char(buffer, strlen(buffer));
if (cam_light>48000) // 20*20pixels*120grayValue
e_set_front_led(1);
wait(5000);
}
}
int[] set_sensors(void){
sensor[0] = e_get_prox(0);
sensor[1] = e_get_prox(1);
sensor[2] = e_get_prox(2);
sensor[3] = e_get_prox(3);
sensor[4] = e_get_prox(4);
sensor[5] = e_get_prox(5);
sensor[6] = e_get_prox(6);
sensor[7] = e_get_prox(7);
return sensor[8];
}
//check front and left or front and right sensors
void check_in_corner_fw( )
{
int i;
// get one single sample for all front, left and right sensors
corner_prox_data_hl[0]=e_get_prox(1); //right
corner_prox_data_hl[1]=e_get_prox(2); //front-right
corner_prox_data_hl[2]=e_get_prox(0); //front
corner_prox_data_hl[3]=e_get_prox(7); //front
corner_prox_data_hl[4]=e_get_prox(5); //left
corner_prox_data_hl[5]=e_get_prox(6); //front-left
// detect if in corner
corner_present_fw = 0;
//check if in left corner
for (i=2; i<6; i++)
{
if(corner_prox_data_hl[i]>300)
{
++corner_present_fw;
}
}
//check if in left corner
for (i=0; i<4; i++)
{
if(corner_prox_data_hl[i]>400)
{
++corner_present_fw;
}
}
}
void read_ir_sensors()
{
int ir_readings[8];
int i;
for(i=0; i<8; i++)
{
ir_readings[i] = e_get_prox(i);
}
for(i=0; i<8; i++)
{
ve_send_int(ir_readings[i]);
}
}
void initRandomNumberGenerator ()
{
unsigned int seed = 0;
long int i;
for (i = 0; i < 10000; i++)
{
if (i % 500)
{
seed += e_get_prox(0) ^ e_get_prox(1);
seed += e_get_prox(2) ^ e_get_prox(3);
seed += e_get_prox(4) ^ e_get_prox(5);
seed += e_get_prox(6) ^ e_get_prox(7);
}
}
srand(seed);
}
int get_sensor(void){
int event;
int i;
sensor[0] = e_get_prox(0);
sensor[1] = e_get_prox(1);
sensor[2] = e_get_prox(2);
sensor[3] = e_get_prox(3);
sensor[4] = e_get_prox(4);
sensor[5] = e_get_prox(5);
sensor[6] = e_get_prox(6);
sensor[7] = e_get_prox(7);
event=0;
for (i=0; i<8; i++) {//this will cause delay
if(sensor[i]>350) {
event = 1;
}
}
// if sensors detect attacker return 1
return event;
}
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);
}
}
/*! \brief To calibrate your ir sensor
* \warning Call this function one time before calling e_get_calibrated_prox
*/
void e_calibrate_ir()
{
int i=0,j=0;
int long t;
int long tmp[8];
for (;i<8;++i) {
init_value_ir[i]=0;
tmp[i]=0;
}
for (;j<100;++j) {
for (i=0;i<8;++i) {
tmp[i]+=(e_get_prox(i));
for (t=0;t<1000;++t);
}
}
for (i=0;i<8;++i) {
init_value_ir[i]=(int)(tmp[i]/(j*1.0));
}
}
int doesWallExist(int selectionValue)
{
if(selectionValue == 1)
{
if((e_get_prox(1) < 100) && (e_get_prox(2) < 100))
{
if(e_get_prox(0) < 100)
{
return 0;
}
}
}
else
{
if((e_get_prox(5) < 100) && (e_get_prox(6) < 100))
{
if(e_get_prox(7) < 100)
{
return 0;
}
}
}
return 1;
}
/*
read sensor prox and return values in a int array
return values from 0x0000 to 0x00FF (from 0 to 255)
*/
void followGetSensorValues(int *sensorTable) {
unsigned int i;
for (i=0; i < NB_SENSORS; i++) {
sensorTable[i] = e_get_prox(i) - follow_sensorzero[i];
}
}
// *** behaviour insired by a dust cleaner
void run_DustCleaner() {
int i;
int sensor;
int leftwheel, rightwheel;
int spiral=100;
int weightleft[8] = {-10,-10,-5,0,0,5,10,10};
int weightright[8] = {10,10,5,0,0,-5,-10,-10};
while (1) {
e_led_clear();
e_set_body_led(0);
e_set_front_led(0);
if (e_get_prox(0)>600)
e_set_led(0,1);
if (e_get_prox(1)>600)
e_set_led(1,1);
if (e_get_prox(2)>600)
e_set_led(2,1);
if (e_get_prox(3)>600)
e_set_led(3,1);
if (e_get_prox(4)>600)
e_set_led(4,1);
if (e_get_prox(5)>600)
e_set_led(5,1);
if (e_get_prox(6)>600)
e_set_led(6,1);
if (e_get_prox(7)>600)
e_set_led(7,1);
if (e_get_prox(0)>600 || e_get_prox(1)>600 || e_get_prox(2)>600 ||
e_get_prox(5)>600 || e_get_prox(6)>600 || e_get_prox(7)>600 ){ // obstacle
e_set_body_led(1);
// *** avoid
leftwheel=200;
rightwheel=200;
for (i=0; i<8; i++) {
sensor=e_get_prox(i); //-sensorzero[i];
sprintf(buffer, "%d, ", sensor);
e_send_uart1_char(buffer, strlen(buffer));
leftwheel+=weightleft[i]*(sensor>>4);
rightwheel+=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);
} else { // spiral
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;
}
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
}
}