/* Segue lateralmente a parede */
void follow_wall() {
short dist1, dist14, dist3, dist4;
/* Se o sonar 1 estiver lendo uma distância muito alta vira à esquerda */
read_sonar(1, &dist1);
if(dist1 > THRESHOLD ) {
set_motors_speed(10, 2);
busy_wait(300000);
set_motors_speed(0, 0);
}
/* Se o sonar 1 estiver lendo uma distância muito baixa vira à direita */
else if(dist1 < THRESHOLD - ACCEPTABLE_DIFFERENCE) {
set_motors_speed(2, 10);
busy_wait(300000);
set_motors_speed(0, 0);
}
/* Está a uma boa distância da parede */
else {
set_motors_speed(10, 10);
busy_wait(300000);
}
/* Se houver uma parede à frente rodar até evitar a parede */
read_sonar(3, &dist3);
while(dist3 < THRESHOLD) {
set_motors_speed(0, 10);
busy_wait(1000);
set_motors_speed(0, 0);
read_sonar(3, &dist3);
}
}
/* Anda em linha reta até encontrar uma parede */
int find_wall() {
short dist3, dist4;
/* Verifica as distancias dos sonares às paredes */
read_sonar(3, &dist3);
read_sonar(4, &dist4);
/* Se a distância do s3 ou s4 for menor que o limiar, encontrou parede */
if(dist3 > THRESHOLD && dist4 > THRESHOLD)
return 0;
else
return 1;
}
/* O robo anda em linha reta ate encontrar uma parede e
entao passa a seguir a parede estando com ela sempre
em seu lado direito */
int main() {
unsigned short *dist[16];
/* inicia no modo busca-parede */
mode = 0;
/* cria proximity call back para quando estiver proximo da parede */
register_proximity_callback(3, MIM_DIST, found);
register_proximity_callback(4, MIM_DIST, found);
/* comeca andando em linha reta */
set_motors_speed(SPEED, SPEED);
/* enquanto estiver no modo busca-parede */
while(mode == 0);
/* passa a andar para esquerda */
set_motors_speed(HSPEED, 0);
/* enquanto nao estiver de lado para parede */
do {
read_sonar(3, dist[3]);
read_sonar(4, dist[4]);
} while(*(dist[3]) <= MIM_DIST && *(dist[4]) <= MIM_DIST);
/* volta a andar para frente */
set_motors_speed(SPEED, SPEED);
dir = 0;
/* fica seguindo a parede */
while(mode) {
read_sonar(7, dist[7]);
read_sonar(8, dist[8]);
if ( *(dist[7]) <= MIM_DIST || *(dist[8]) <= MIM_DIST ) {
if(dir != 2) {
set_motor_speed(1, HSPEED);
}
} else if ( *(dist[7]) >= MAX_DIST || *(dist[8]) >= MAX_DIST ) {
if(dir != 1) {
set_motor_speed(0, HSPEED);
}
} else {
if(dir != 0) {
set_motors_speed(SPEED, SPEED);
}
}
}
return 0;
}
/* main function */
void _start(void)
{
unsigned int distances[16];
/* While not close to anything. */
do {
if(read_sonar(3) < 1200)
set_speed_motors(30, 0);
else if(read_sonar(4) < 1200)
set_speed_motors(0, 30);
else
set_speed_motors(30,30);
} while (1);
}
void right() {
set_motor_speed(0,0);
set_motor_speed(1,SPEED);
do {
read_sonar(3,&(dist[3]));
} while (dist[3] <= LIMIAR);
set_motor_speed(1,SPEED);
set_motor_speed(0,SPEED);
}
void left() {
set_motor_speed(1,0);
set_motor_speed(0,SPEED);
do {
read_sonar(4,&(dist[4]));
} while (dist[4] <= LIMIAR);
set_motor_speed(1,SPEED);
set_motor_speed(0,SPEED);
}
/* Posiciona a lateral esquerda do robô paralelamente à parede */
int set_position_to_left() {
short dist0, dist14;
/* Roda o robo até encontrar uma parede à esquerda */
set_motors_speed(0, 10);
/* Se a distância do sonar da esquerda foi menor que o limiar + erro */
/* é porque encontrou uma parede. */
read_sonar(0, &dist0);
read_sonar(14, &dist14);
if(dist0 < THRESHOLD + ACCEPTABLE_ERROR && dist14 < THRESHOLD + ACCEPTABLE_ERROR) {
/* O robo está lado a lado com a parede.
/* Pare-o */
set_motors_speed(0, 0);
return 1;
}
/* Não conseguiu encontrar parede */
return 0;
}
// read baro and sonar altitude at 10hz
void Copter::update_altitude()
{
// read in baro altitude
read_barometer();
// read in sonar altitude
sonar_alt = read_sonar();
// write altitude info to dataflash logs
if (should_log(MASK_LOG_CTUN)) {
Log_Write_Control_Tuning();
}
}
