rhssk/inspektors
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Initial logic for avoiding obstacles

Browse Source
This commit is contained in:
Rihards Skuja 2018-01-22 21:24:41 +02:00
parent 6dac67f5a9
commit e62c361970
No known key found for this signature in database
GPG Key ID: 53FA13A3F7F8571B
2 changed files with 83 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
robot/include/main.h
View File

@ -5,5 +5,7 @@
#include "common.h"
void convert_data_to_raw(data_packet_t *in, uint8_t *out);
void send_sensor_data(const int32_t *distance_in_cm);
void decide_movement(const int32_t *distance_in_cm);
#endif

110
robot/src/main.c
View File

@ -14,16 +14,18 @@
#include "debug.h"
#endif /* DEBUG */
#define OBSTACLE_THRESHOLD 10
uint8_t tx_raw[sizeof(data_packet_t)];
data_packet_t tx_data;
int main(void)
{
uint8_t *tx_raw;
data_packet_t *tx_data;
int32_t distance_in_cm;
// Data packet is too large, should split in multiple packets
if (sizeof(data_packet_t) > MAX_PAYLOAD_SIZE)
return R_PACKAGE_SIZE;
tx_data = malloc(sizeof(data_packet_t));
tx_raw = malloc(sizeof(data_packet_t));
init_leds();
init_servos();
@ -41,35 +43,85 @@ int main(void)
sei();
run_servos();
servo_full_forw();
while (1) {
uint16_t distance_int_cm = read_sonar();
double temp = ds18b20_gettemp();
// Distance is measured early so the same value gets sent to the base
// station and gets used to decide movement. Needs to be done in order
// for base station to receive meaningful distance value.
distance_in_cm = read_sonar();
decide_movement(&distance_in_cm);
if (distance_int_cm <= 10 && distance_int_cm != SONAR_ERROR) {
SET(LED_PORT, LED_BLUE_BIT);
} else {
CLR(LED_PORT, LED_BLUE_BIT);
}
if (temp >= 23) {
SET(LED_PORT, LED_GREEN_BIT);
} else {
CLR(LED_PORT, LED_GREEN_BIT);
// Update base station
send_sensor_data(&distance_in_cm);
}
memset(tx_data, 0, sizeof(data_packet_t));
tx_data->distance = distance_int_cm;
tx_data->temp = temp;
memcpy(tx_raw, tx_data, sizeof(data_packet_t));
nrf24_send(tx_raw);
// TODO: prevent transmission to block the main loop
while (nrf24_isSending()); // Wait for transmission to finish
}
free(tx_data);
free(tx_raw);
cli();
return R_OK;
}
void send_sensor_data(const int32_t *distance_in_cm)
{
double temp;
temp = ds18b20_gettemp();
/* if (temp >= 23) */
// Prepare the data to be sent
memset(&tx_data, 0, sizeof(data_packet_t));
tx_data.distance = *distance_in_cm;
tx_data.temp = temp;
memcpy(tx_raw, &tx_data, sizeof(data_packet_t));
nrf24_send(tx_raw);
// Indicate package sending with a signal LED
SET(LED_PORT, LED_GREEN_BIT);
while (nrf24_isSending());
CLR(LED_PORT, LED_GREEN_BIT);
}
/// Control servos based on received distance data
void decide_movement(const int32_t *distance_in_cm)
{
uint8_t avoid_obstacle = 0;
int32_t distance_to_obstacle = *distance_in_cm;
// No obstacle found
if (distance_to_obstacle == ECHO_TIMEOUT) return;
while (1) {
if (distance_to_obstacle == ECHO_TIMEOUT) {
// No obstacle found
servo_full_forw();
avoid_obstacle = 0;
} else if (distance_to_obstacle >= 0
&& distance_to_obstacle <= MIN_SONAR_RANGE) {
// Obstacle is right in front, can't make a turn
servo_full_backw();
avoid_obstacle = 1;
_delay_ms(SONAR_DELAY);
} else if (distance_to_obstacle > MIN_SONAR_RANGE
&& distance_to_obstacle <= OBSTACLE_THRESHOLD) {
// Obstacle is close and in front, make a turn backwards
servo_turn_backw_r();
avoid_obstacle = 1;
_delay_ms(SONAR_DELAY);
} else {
// TODO: factor this in decision making about maneuvers
// Obstacle is in front, but a safe distance away
servo_full_forw();
avoid_obstacle = 0;
}
if (avoid_obstacle == 1) {
SET(LED_PORT, LED_BLUE_BIT);
} else {
CLR(LED_PORT, LED_BLUE_BIT);
return;
}
distance_to_obstacle = read_sonar();
}
}