void readValues() {
if(!lockReading) {
values[0]+= e_get_calibrated_prox(S_FRONT_RIGHT);
values[1]+= e_get_calibrated_prox(S_RIGHT);
values[2]+= e_get_calibrated_prox(S_LEFT);
values[3]+= e_get_calibrated_prox(S_FRONT_LEFT);
readTimes++;
}
}
/*! \brief Calcul the speed to set on each wheel for avoiding
*
* Here we do a level-headed sum to take advantage of each captor
* depending of there position. For exemple if the captor number 0
* detect something, he has to set the right speed high and set
* the left speed low.
*/
void shock_neuron(void)
{
for (m = 0; m < 2; m++)
{
potential[m] = 0;
for (s = 0; s < 8; s++)
potential[m] += (matrix_prox[m][s]*e_get_calibrated_prox(s)); // get values from proximity sensors
speed[m] = (potential[m]/PROXSCALING_SHOCK + BASICSPEED);
}
if((speed[1] < 50 && speed[1] > -50)
&& (speed[0] < 50 && speed[0] > -50)) {
speed[1] = speed[1] * 20;
speed[0] = speed[0] * 20;
}
if (speed[1] > 1000)
speed[1] = 1000;
else if (speed[1] < -1000 )
speed[1] = -1000;
if (speed[0] > 1000)
speed[0] = 1000;
else if (speed[0] < -1000 )
speed[0] = -1000;
e_set_speed_left(speed[1]);
e_set_speed_right(speed[0]);
}
int approachButton() {
e_set_led(5,0);
e_set_led(3,0);
e_set_speed_left(300);
e_set_speed_right(300);
while(e_get_calibrated_prox(S_FRONT_LEFT) < 3000 && e_get_calibrated_prox(S_FRONT_RIGHT) < 3000) {
if(e_get_calibrated_prox(S_RIGHT) > 200 && e_get_calibrated_prox(S_FRONT_RIGHT) < 200){
correctRobot();
return 0;
}
if(e_get_calibrated_prox(S_LEFT) > 200 && e_get_calibrated_prox(S_FRONT_LEFT) < 200){
correctRobot();
return 0;
}
}
unsigned int i;
unsigned int m;
for(i = 0 ; i < 10000 ; i++)
m = sqrt(i*100);
e_set_speed_left(-200);
e_set_speed_right(-200);
for(i = 0 ; i < 10000 ; i++)
m = sqrt(i*100);
e_set_speed_left(-200);
e_set_speed_right(200);
while(e_get_calibrated_prox(S_FRONT_RIGHT) > 0);
for(i = 0 ; i < 4000 ; i++)
m = sqrt(i*100);
e_set_speed_left(0);
e_set_speed_right(0);
keepFinding = 0;
}
void correctRobot() {
if(e_get_calibrated_prox(S_LEFT) > 10) {
correctRight = 0;
e_set_speed_left(100);
e_set_speed_right(-100);
while(e_get_calibrated_prox(S_FRONT_LEFT) > 20);
waitABit();
e_set_speed_left(700);
e_set_speed_right(700);
waitABit();
e_set_speed_left(0);
e_set_speed_right(0);
}else if(e_get_calibrated_prox(S_RIGHT) > 10) {
correctRight = 1;
e_set_speed_left(-100);
e_set_speed_right(100);
while(e_get_calibrated_prox(S_FRONT_RIGHT) > 20);
waitABit();
e_set_speed_left(700);
e_set_speed_right(700);
waitABit();
e_set_speed_left(0);
e_set_speed_right(0);
}else if(e_get_calibrated_prox(S_FRONT_LEFT) > 100 || e_get_calibrated_prox(S_FRONT_RIGHT) > 100) {
e_set_speed_left(-300);
e_set_speed_right(-300);
waitABit();
e_set_speed_left(0);
e_set_speed_right(0);
}
}
void follow_neuron(void)
{
int lin_speed = lin_speed_calc((e_get_calibrated_prox(7)+e_get_calibrated_prox(0))/2, 6);
int angle_speed = angle_speed_calc((e_get_calibrated_prox(0)+e_get_calibrated_prox(1)) -
(e_get_calibrated_prox(7)+e_get_calibrated_prox(6)), 4);
e_set_speed_left (lin_speed - angle_speed);
e_set_speed_right(lin_speed + angle_speed);
}
void obstacleAvoidance()
{
// check if an obstacle is perceived
double reading = 0.0;
int obstaclePerceived = 0;
int i=0;
double x = 0.0, y = 0.0;
for (i = 0; i < 8; i++)
{
reading = e_get_calibrated_prox(i);
// if signal above noise
if(reading >= obstacleAvoidanceThreshold)
{
obstaclePerceived = 1;
// compute direction to escape
double signal = reading - obstacleAvoidanceThreshold;
x += -cos(sensorDir[i]) * signal / 8.0;
y += sin(sensorDir[i]) * signal / 8.0;
}
}
// no obstacles to avoid, return immediately
if (obstaclePerceived == 0)
{
// go straight forward
// change movement direction
e_set_speed_left(obstacleAvoidanceSpeed);
e_set_speed_right(obstacleAvoidanceSpeed);
// return obstaclePerceived;
return;
}
double desiredAngle = atan2 (y, x);
double leftSpeed = 0.0;
double rightSpeed = 0.0;
// turn left
if (desiredAngle >= 0.0)
{
leftSpeed = cos(desiredAngle);
rightSpeed = 1;
}
// turn right
else
{
leftSpeed = 1;
rightSpeed = cos(desiredAngle);
}
// rescale values
leftSpeed *= obstacleAvoidanceSpeed;
rightSpeed *= obstacleAvoidanceSpeed;
// change movement direction
e_set_speed_left(leftSpeed);
e_set_speed_right(rightSpeed);
// advertise obstacle avoidance in progress
// return 1;
}
/*! \breif Read the sensors proxymities
* \param sensorTable Where the value must be stocked
*/
void followGetSensorValues(int *sensorTable) {
unsigned int i;
for (i=0; i < NB_SENSORS; i++) {
sensorTable[i] = e_get_calibrated_prox(i); //e_get_prox(i) - follow_sensorzero[i];
}
}
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);