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 turn_direction(float direction)
{
int end_turn;
if (direction < PI) // turn clockwise
{
e_set_steps_left(0);
e_set_speed_left(TURN_SPEED); // motor speed in steps/s
e_set_speed_right(-TURN_SPEED); // motor speed in steps/s
// calculate how many steps the robot needs to turn
end_turn = (int)(STEPS_FOR_2PI*(direction/(2*PI)));
while(e_get_steps_left() < end_turn){
flick_led();
} // turn until done
}
else // turn counterclockwise
{
e_set_steps_right(0);
e_set_speed_left(-TURN_SPEED); // motor speed in steps/s
e_set_speed_right(TURN_SPEED); // motor speed in steps/s
// calculate how many steps the robot needs to turn
end_turn = (int)(STEPS_FOR_2PI*((2*PI-direction)/(2*PI)));
while(e_get_steps_right() < end_turn){
flick_led();
} // turn until done
}
// stop motors
e_set_speed_left(0); // motor speed in steps/s
e_set_speed_right(0); // motor speed in steps/s
}
void processaImagem() {
long avg = 0;
int hits = 0;
int i;
for(i = 0 ; i < buff_length ; i+=2) {
int r = buffer[i]&0xF8;
int g = (buffer[i]&0x07)<<5 | (buffer[i+1]&0xE0)>>3;
int b = (buffer[i+1]&0x1F)<<3;
if(r >= 90 && g <= 70 && b <= 70) {
avg+= i;
hits++;
}
}
if(hits > 0) {
avg = avg/hits/2;
if(hits > 0 && avg < 640/2)
e_set_led(5,1);
else
e_set_led(5,0);
if(hits > 0 && avg >= 640/2)
e_set_led(3,1);
else
e_set_led(3,0);
int distance = 640/2-avg;
if(distance > 15 || distance < -15) {
if(distance > 0) {
e_set_speed_left(-10);
e_set_speed_right(10);
} else {
e_set_speed_left(10);
e_set_speed_right(-10);
}
}else {
approachButton();
}
}else{
if(correctRight) {
e_set_speed_left(70);
e_set_speed_right(-70);
}else {
e_set_speed_left(-70);
e_set_speed_right(70);
}
}
}
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);
}
}
/*! \brief Turn a preset angle in [degrees] clockwise or counter-clockwise
* \param turn_angle The angle in [degrees] which is to be traveled
* CW (-) or CCW (+)
* \param motor_speed The speed at which the robot turns. The speed is always positive.
*/
void e_set_turn(double turn_angle, int motor_speed) // added by bahr
{
INTERRUPT_OFF();
double delta_s;
nbr_pas_left = 0;
nbr_pas_right = 0;
// set open loop control status to active
ol_ctrl_status = 1;
// compute the distance which each wheels has to travel (in opposite direction)
delta_s=((double)WHEEL_DISTANCE*(fabs(turn_angle)*(PI/(double)180.0)))/(double)2.0;
// compute the number of necessary steps for the turn
nbr_pas_ol_ctrl = (long int)(((long int)STEPS_PER_M*delta_s));
// ensure that the motor speed is positive
motor_speed=abs(motor_speed);
// turn clock-wise
if (turn_angle < 0)
{
nbr_pas_ol_ctrl_left = nbr_pas_ol_ctrl;
nbr_pas_ol_ctrl_right = -nbr_pas_ol_ctrl;
e_set_speed_left(motor_speed);
e_set_speed_right(-motor_speed);
}
// turn counter clock-wise
if (turn_angle > 0)
{
nbr_pas_ol_ctrl_left = -nbr_pas_ol_ctrl;
nbr_pas_ol_ctrl_right = nbr_pas_ol_ctrl;
e_set_speed_left(-motor_speed);
e_set_speed_right(motor_speed);
}
// stop
if (turn_angle == 0)
{
ol_ctrl_status = 4;
e_set_speed_left(0);
e_set_speed_right(0);
}
INTERRUPT_ON();
}
/*! \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]);
}
/*
set robot speed
speeds: from -MAXSPEED to MAXSPEED
*/
void followsetSpeed(int LeftSpeed, int RightSpeed) {
if (LeftSpeed < -MAXSPEED) {LeftSpeed = -MAXSPEED;}
if (LeftSpeed > MAXSPEED) {LeftSpeed = MAXSPEED;}
if (RightSpeed < -MAXSPEED) {RightSpeed = -MAXSPEED;}
if (RightSpeed > MAXSPEED) {RightSpeed = MAXSPEED;}
e_set_speed_left(LeftSpeed);
e_set_speed_right(RightSpeed);
}
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);
}
/*! \brief Manage linear/angular speed
*
* This function manage the speed of the motors according to the
* desired linear and angular speed.
* \param linear_speed the speed in the axis of e-puck
* \param angular_speed the rotation speed (trigonometric)
*/
void e_set_speed(int linear_speed, int angular_speed)
{
if(abs(linear_speed) + abs(angular_speed) > 1000)
return;
else
{
e_set_speed_left (linear_speed - angular_speed);
e_set_speed_right(linear_speed + angular_speed);
}
}
/** Stop the motor activities
*
* Set the speed for zero and clean the step counter
*/
void stop_motors()
{
e_set_speed_right(0);
e_set_speed_left(0);
e_set_steps_right(0);
e_set_steps_left(0);
send_char(1);/* It signalize the end of operation. */
}
// *** 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
void move()
{
perception();
// compute final direction and motor speed
speed = 0.0;
float desiredAngle = direction;
double leftSpeed = 0.0;
double rightSpeed = 0.0;
// if angle is not big ... don't turn
if (fabs(desiredAngle) < MOVE_THRESOLD * M_PI / 180.0)
{
// direction does not change...
leftSpeed = speed;
rightSpeed = speed;
}
// turn left
else if (desiredAngle >= 0.0)
{
leftSpeed = -turnspeed;
rightSpeed = turnspeed;
// update internal direction according to movement
float timestep = 0.25;
float angle = 2.0 * M_PI * turnspeed * timestep / NUMBER_STEPS_FULL_TURN;
updateDirections(-angle);
}
// turn right
else
{
leftSpeed = turnspeed;
rightSpeed = -turnspeed;
// update internal direction according to movement
float timestep = 0.25;
float angle = 2.0 * M_PI * turnspeed * timestep / NUMBER_STEPS_FULL_TURN;
updateDirections(angle);
}
// change movement direction
e_set_speed_left(leftSpeed);
e_set_speed_right(rightSpeed);
}
void recv_vel()
{
int stepsR,stepsL;
stepsR = ve_recv_int();
stepsL = ve_recv_int();
if((stepsR > 1000)||(stepsR < -1000)||(stepsL > 1000)||(stepsL < -1000))
{
send_char(1);/* It signalize the end of operation. */
return;
}
e_set_speed_right(stepsR);
e_set_speed_left(stepsL);
send_char(1);/* It signalize the end of operation. */
}
void setAngularVelocity(double ang,double force){
double fCLinear, fCAngular, fC1,fVLinear,fVAngular;
while ( ang > PI ) {ang -= 2 * PI;}
while ( ang < -PI ) {ang += 2 * PI;}
fCLinear = 1.0;
fCAngular = 1.0;
fC1 = force * SPEED / PI;
/* Calc Linear Speed */
fVLinear = SPEED * fCLinear * ( cos ( ang / 2) );
/*Calc Angular Speed */
fVAngular = ang;
e_set_speed_left(fVLinear - fC1 * fVAngular);
e_set_speed_right(fVLinear + fC1 * fVAngular);
}
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 Travel a preset distance in [mm] forward or backward
* \param set_distance The distance in [mm] which is to be traveled
* forward (+) or backward (-)
* \param motor_speed The speed at which the robot travels. The speed is always positive.
*/
void e_set_distance(long int set_distance, int motor_speed) // added by bahr
{
INTERRUPT_OFF();
nbr_pas_left = 0;
nbr_pas_right = 0;
// set open loop control status to active
ol_ctrl_status = 1;
// computebthe number of steps for each motor
nbr_pas_ol_ctrl_left = (long int)(((long int)STEPS_PER_M*set_distance)/1000);
nbr_pas_ol_ctrl_right = (long int)(((long int)STEPS_PER_M*set_distance)/1000);
// go backwards
if (set_distance < 0)
{
motor_speed=-abs(motor_speed);
}
// go forward
if (set_distance > 0)
{
motor_speed=abs(motor_speed);
}
// stop
if (set_distance == 0)
{
ol_ctrl_status = 4;
motor_speed = 0;
}
// set motor speed
e_set_speed_left(motor_speed);
e_set_speed_right(motor_speed);
INTERRUPT_ON();
}
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;
}
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 run_square() {
unsigned char useGyroFlag = 0; // 0=use odometry, 1=use gyroscope
unsigned char squareState = 0;
signed int tempZ = 0;
float turnAngle = 0;
float diffTime = 0;
float dpmsValue = 0;
//e_init_uart2(BAUD230400); // for debug
while(1) {
if(isEpuckVersion1_1() && useGyroFlag==1) {
switch(squareState) {
case 0: // set speed to go forward (square side)
e_set_speed_left(150);
e_set_speed_right(150);
e_set_steps_left(0);
squareState = 1;
break;
case 1: // go forward for a while
if(e_get_steps_left() >= 800) {
squareState = 2;
}
break;
case 2: // set speed to rotate on the square angle
e_set_steps_left(0);
e_set_speed_left(-100);
e_set_speed_right(100);
resetTime();
turnAngle = 0;
squareState = 3;
break;
case 3: // rotate 90 degrees
tempZ = getZAxisGyro();
dpmsValue = rawToDpms(tempZ);
diffTime = getDiffTimeMsAndReset();
turnAngle += diffTime*dpmsValue; // integrate the angular rate to get the rotation angle
//sprintf(buffer, "%f,%d,%f\r\n", turnAngle,tempZ,diffTime);
//e_send_uart2_char(buffer, strlen(buffer));
if(turnAngle >= 90) {
e_set_speed_left(0);
e_set_speed_right(0);
squareState = 0;
}
break;
}
} else {
switch(squareState) {
case 0: // set speed to go forward (square side)
e_set_speed_left(150);
e_set_speed_right(150);
e_set_steps_left(0);
squareState = 1;
break;
case 1: // go forward for a while
if(e_get_steps_left() >= 800) {
squareState = 2;
}
break;
case 2: // set speed to rotate on the square angle
e_set_steps_left(0);
e_set_speed_left(-100);
e_set_speed_right(100);
resetTime();
turnAngle = 0;
squareState = 3;
break;
case 3: // rotate 90 degrees
turnAngle = e_get_steps_left();
//sprintf(buffer, "%f,%d,%f\r\n", turnAngle,tempZ,diffTime);
//e_send_uart2_char(buffer, strlen(buffer));
if(turnAngle <= -330) { // from field test 330 steps correspond to about 90 degrees
e_set_speed_left(0);
e_set_speed_right(0);
squareState = 0;
}
break;
}
}
}
}
void run_CameraTurn() {
int cam_mode,cam_width,cam_heigth,cam_zoom,cam_size;
int i;
unsigned char *buf_ptr, pixel, lightest;
unsigned int left, right, lightPos;
#include "DataEEPROM.h"
/*read HW version from the eeprom (last word)*/
int HWversion=0xFFFF;
int temp = 0;
temp = ReadEE(0x7F,0xFFFE,&HWversion, 1);
temp = temp & 0x03; // get the camera rotation from the HWversion byte
/*Cam default parameter*/
cam_mode=GREY_SCALE_MODE;
if ((temp==3)||(temp==0)) { // 0' and 180' camera rotation
cam_width=1;
cam_heigth=60;
} else {
cam_width=60;
cam_heigth=1;
}
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();
while (1) {
e_poxxxx_launch_capture(&buffer[0]); // start camera capture
e_led_clear();
e_set_body_led(0);
e_set_front_led(0);
while(!e_poxxxx_is_img_ready()); // wait end of capture
buf_ptr=(unsigned char*)&buffer[0];
left=0; right=0; lightPos=0; lightest=0;
for (i=0; i<30; i++) { //left
pixel=*buf_ptr;
buf_ptr++;
left+=pixel;
if (pixel>lightest) {
lightest=pixel;
lightPos=i;
}
}
for (; i<cam_heigth; i++) { //right
pixel=*buf_ptr;
buf_ptr++;
right+=pixel;
if (pixel>lightest) {
lightest=pixel;
lightPos=i;
}
}
if (lightPos<20) { //led on at lightest position
e_set_led(7,1); }
else if (lightPos<40) {
e_set_led(0,1); }
else {
e_set_led(1,1); }
if ((temp==3)||(temp==2)) { // 0' and 90' camera rotation
e_set_speed_left(10*(lightPos-30)); // motor speed in steps/s
e_set_speed_right(-10*(lightPos-30));
} else {
e_set_speed_left(-10*(lightPos-30)); // motor speed in steps/s
e_set_speed_right(10*(lightPos-30));
}
sprintf(buffer, "left %u, right %u, lightest %u, lightPos %u\r\n", left, right, lightest, lightPos);
e_send_uart1_char(buffer, strlen(buffer));
wait(5000);
}
}
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);
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;
}
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;
}
/*! \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);
}
}
}
}
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
}
}
