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

Allow precise servo speed adjustments based on desired load

Browse Source
This commit is contained in:
Rihards Skuja 2018-01-23 23:24:20 +02:00
parent 9f6e90f27f
commit 35b8bd0ab2
No known key found for this signature in database
GPG Key ID: 53FA13A3F7F8571B
3 changed files with 89 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
robot/include/servos.h
View File

@ -7,7 +7,7 @@
#define SERVO_L_BIT PD6
#define SERVO_R_BIT PD5
// Not just STOP +- 500us because of rounding errors (e.g. 19.9 will become
// Not just MID +- 500us because of rounding errors (e.g. 19.9 will become
// 19 and not 20 because of integer division)
#define SERVO_MID 1381UL
#define SERVO_MAX 1883UL
@ -21,18 +21,17 @@
#define SERVO_R_FORW SERVO_MIN
#define SERVO_R_BACKW SERVO_MAX
#define SERVO_DIR_BACKW 0
#define SERVO_DIR_FORW 1
#define SERVO_90_TURN_MS 1000
#define SERVO_CM_PER_S 15 // Distance in cm that servos travel in 1s
#define PRESCALER 1024UL
#define PWM_PERIOD_MS (uint16_t)((256000UL * PRESCALER) / F_CPU)
#define US2TIMER0(us) (uint16_t)((51U * us) / (200U * PWM_PERIOD_MS))
#define US2TIMER0(us) (uint8_t)((51U * us) / (200U * PWM_PERIOD_MS))
void init_servos(void);
void servo_full_forw(void);
void servo_turn_forw_r(void);
void servo_turn_forw_l(void);
void servo_full_backw(void);
void servo_turn_backw_r(void);
void servo_turn_backw_l(void);
void servo_speed(uint8_t direction, uint8_t load_percent_l, uint8_t load_percent_r);
#endif /* SERVOS_H */

47
robot/src/main.c
View File

@ -14,7 +14,10 @@
#include "debug.h"
#endif /* DEBUG */
#define OBSTACLE_THRESHOLD 10
// Distance in cm after which node should start evading the obstacle
#define OBSTACLE_THRESHOLD 20
// Wait 2s before starting to drive
#define START_DELAY_MS 2000
uint8_t tx_raw[sizeof(data_packet_t)];
data_packet_t tx_data;
@ -28,31 +31,30 @@ int main(void)
return R_PACKAGE_SIZE;
init_leds();
init_servos();
init_sonar();
init_servos();
nrf24_init();
SET(LED_PORT, LED_BLUE_BIT);
_delay_ms(START_DELAY_MS);
CLR(LED_PORT, LED_BLUE_BIT);
nrf24_config(2, sizeof(data_packet_t)); // Channel #2
nrf24_tx_address(base_mac);
nrf24_rx_address(node_mac);
// Use green LED as power LED
// TODO: need all LEDs for debugging, will use as power when UART debugger
// will arrive
/* SET(LED_PORT, LED_GREEN_BIT); */
sei();
servo_full_forw();
while (1) {
// 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);
// Update base station
send_sensor_data(&distance_in_cm);
decide_movement(&distance_in_cm);
}
cli();
@ -71,9 +73,9 @@ void send_sensor_data(const int32_t *distance_in_cm)
nrf24_send(tx_raw);
// Indicate package sending with a signal LED
SET(LED_PORT, LED_GREEN_BIT);
SET(LED_PORT, LED_BLUE_BIT);
while (nrf24_isSending());
CLR(LED_PORT, LED_GREEN_BIT);
CLR(LED_PORT, LED_BLUE_BIT);
}
/// Control servos based on received distance data
@ -82,40 +84,39 @@ 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();
servo_speed(SERVO_DIR_FORW, 100, 100);
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();
servo_speed(SERVO_DIR_BACKW, 100, 100);
_delay_ms(SERVO_90_TURN_MS);
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();
// Obstacle is close and in front, make a turn left
servo_speed(SERVO_DIR_FORW, 0, 100);
_delay_ms(SERVO_90_TURN_MS);
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();
servo_speed(SERVO_DIR_FORW, 50, 50);
avoid_obstacle = 0;
}
if (avoid_obstacle == 1) {
SET(LED_PORT, LED_BLUE_BIT);
SET(LED_PORT, LED_GREEN_BIT);
} else {
CLR(LED_PORT, LED_BLUE_BIT);
CLR(LED_PORT, LED_GREEN_BIT);
return;
}
_delay_ms(500);
distance_to_obstacle = read_sonar();
}
}

85
robot/src/servos.c
View File

@ -23,38 +23,65 @@ void init_servos(void)
SET(SERVO_DDR, SERVO_R_BIT);
}
void servo_full_forw(void)
void servo_speed(uint8_t direction, uint8_t load_percent_l, uint8_t load_percent_r)
{
SERVO_L_REG = US2TIMER0(SERVO_L_FORW);
SERVO_R_REG = US2TIMER0(SERVO_R_FORW);
}
// Check max and min values (0 and 100) first to avoid division since that
// will lose some precision
void servo_turn_forw_r(void)
{
SERVO_L_REG = US2TIMER0(SERVO_L_FORW);
SERVO_R_REG = US2TIMER0(SERVO_R_STOP);
}
// Special case for full stop since it requires register clearing
if (load_percent_l == 0 && load_percent_r == 0) {
TCCR0A = 0x00;
TCCR0B = 0x00;
return;
}
void servo_turn_forw_l(void)
{
SERVO_L_REG = US2TIMER0(SERVO_L_STOP);
SERVO_R_REG = US2TIMER0(SERVO_R_FORW);
}
// Registers have been cleared, need to set them again
if (TCCR0A == 0x00 || TCCR0B == 0x00)
init_servos();
void servo_full_backw(void)
{
SERVO_L_REG = US2TIMER0(SERVO_L_BACKW);
SERVO_R_REG = US2TIMER0(SERVO_R_BACKW);
}
// Left motor
if (load_percent_l == 0) {
SERVO_L_REG = US2TIMER0(SERVO_L_STOP);
} else {
// Forwards
if (direction == SERVO_DIR_FORW) {
if (load_percent_l == 100) {
SERVO_L_REG = US2TIMER0(SERVO_L_FORW);
} else {
SERVO_L_REG = US2TIMER0(SERVO_L_STOP + ((SERVO_L_FORW - SERVO_L_STOP)
* load_percent_l) / 100);
}
} else {
// Backwards
if (load_percent_l == 100) {
SERVO_L_REG = US2TIMER0(SERVO_L_BACKW);
} else {
SERVO_L_REG = US2TIMER0(SERVO_L_STOP - ((SERVO_L_STOP - SERVO_L_BACKW)
* load_percent_l) / 100);
}
}
}
void servo_turn_backw_r(void)
{
SERVO_L_REG = US2TIMER0(SERVO_L_STOP);
SERVO_R_REG = US2TIMER0(SERVO_R_BACKW);
}
void servo_turn_backw_l(void)
{
SERVO_L_REG = US2TIMER0(SERVO_L_BACKW);
SERVO_R_REG = US2TIMER0(SERVO_R_STOP);
// Right motor
if (load_percent_r == 0) {
SERVO_R_REG = US2TIMER0(SERVO_R_STOP);
} else {
// Forwards
if (direction == SERVO_DIR_FORW) {
if (load_percent_r == 100) {
SERVO_R_REG = US2TIMER0(SERVO_R_FORW);
} else {
SERVO_R_REG = US2TIMER0(SERVO_R_STOP - ((SERVO_R_STOP - SERVO_R_FORW)
* load_percent_r) / 100);
}
} else {
// Backwards
if (load_percent_r == 100) {
SERVO_R_REG = US2TIMER0(SERVO_R_BACKW);
} else {
SERVO_R_REG = US2TIMER0(SERVO_R_STOP + ((SERVO_R_BACKW - SERVO_R_STOP)
* load_percent_r) / 100);
}
}
}
}