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 6907f0159a
No known key found for this signature in database
GPG Key ID: 53FA13A3F7F8571B
3 changed files with 60 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
robot/include/servos.h
View File

@ -7,7 +7,7 @@
#define SERVO_L_BIT PD6 #define SERVO_L_BIT PD6
#define SERVO_R_BIT PD5 #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) // 19 and not 20 because of integer division)
#define SERVO_MID 1381UL #define SERVO_MID 1381UL
#define SERVO_MAX 1883UL #define SERVO_MAX 1883UL
@ -21,18 +21,17 @@
#define SERVO_R_FORW SERVO_MIN #define SERVO_R_FORW SERVO_MIN
#define SERVO_R_BACKW SERVO_MAX #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 PRESCALER 1024UL
#define PWM_PERIOD_MS (uint16_t)((256000UL * PRESCALER) / F_CPU) #define PWM_PERIOD_MS (uint16_t)((256000UL * PRESCALER) / F_CPU)
#define US2TIMER0(us) (uint16_t)((51U * us) / (200U * PWM_PERIOD_MS)) #define US2TIMER0(us) (uint16_t)((51U * us) / (200U * PWM_PERIOD_MS))
void init_servos(void); void init_servos(void);
void servo_speed(uint8_t direction, uint8_t load_percent_l, uint8_t load_percent_r);
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);
#endif /* SERVOS_H */ #endif /* SERVOS_H */

47
robot/src/main.c
View File

@ -14,7 +14,10 @@
#include "debug.h" #include "debug.h"
#endif /* DEBUG */ #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)]; uint8_t tx_raw[sizeof(data_packet_t)];
data_packet_t tx_data; data_packet_t tx_data;
@ -28,31 +31,30 @@ int main(void)
return R_PACKAGE_SIZE; return R_PACKAGE_SIZE;
init_leds(); init_leds();
init_servos();
init_sonar(); init_sonar();
init_servos();
nrf24_init(); 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_config(2, sizeof(data_packet_t)); // Channel #2
nrf24_tx_address(base_mac); nrf24_tx_address(base_mac);
nrf24_rx_address(node_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(); sei();
servo_full_forw();
while (1) { while (1) {
// Distance is measured early so the same value gets sent to the base // 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 // station and gets used to decide movement. Needs to be done in order
// for base station to receive meaningful distance value. // for base station to receive meaningful distance value.
distance_in_cm = read_sonar(); distance_in_cm = read_sonar();
decide_movement(&distance_in_cm);
// Update base station // Update base station
send_sensor_data(&distance_in_cm); send_sensor_data(&distance_in_cm);
decide_movement(&distance_in_cm);
} }
cli(); cli();
@ -71,9 +73,9 @@ void send_sensor_data(const int32_t *distance_in_cm)
nrf24_send(tx_raw); nrf24_send(tx_raw);
// Indicate package sending with a signal LED // Indicate package sending with a signal LED
SET(LED_PORT, LED_GREEN_BIT); SET(LED_PORT, LED_BLUE_BIT);
while (nrf24_isSending()); while (nrf24_isSending());
CLR(LED_PORT, LED_GREEN_BIT); CLR(LED_PORT, LED_BLUE_BIT);
} }
/// Control servos based on received distance data /// 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; uint8_t avoid_obstacle = 0;
int32_t distance_to_obstacle = *distance_in_cm; int32_t distance_to_obstacle = *distance_in_cm;
// No obstacle found
if (distance_to_obstacle == ECHO_TIMEOUT) return;
while (1) { while (1) {
if (distance_to_obstacle == ECHO_TIMEOUT) { if (distance_to_obstacle == ECHO_TIMEOUT) {
// No obstacle found // No obstacle found
servo_full_forw(); servo_speed(SERVO_DIR_FORW, 100, 100);
avoid_obstacle = 0; avoid_obstacle = 0;
} else if (distance_to_obstacle >= 0 } else if (distance_to_obstacle >= 0
&& distance_to_obstacle <= MIN_SONAR_RANGE) { && distance_to_obstacle <= MIN_SONAR_RANGE) {
// Obstacle is right in front, can't make a turn // 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; avoid_obstacle = 1;
_delay_ms(SONAR_DELAY);
} else if (distance_to_obstacle > MIN_SONAR_RANGE } else if (distance_to_obstacle > MIN_SONAR_RANGE
&& distance_to_obstacle <= OBSTACLE_THRESHOLD) { && distance_to_obstacle <= OBSTACLE_THRESHOLD) {
// Obstacle is close and in front, make a turn backwards // Obstacle is close and in front, make a turn left
servo_turn_backw_r(); servo_speed(SERVO_DIR_FORW, 0, 100);
_delay_ms(SERVO_90_TURN_MS);
avoid_obstacle = 1; avoid_obstacle = 1;
_delay_ms(SONAR_DELAY);
} else { } else {
// TODO: factor this in decision making about maneuvers // TODO: factor this in decision making about maneuvers
// Obstacle is in front, but a safe distance away // Obstacle is in front, but a safe distance away
servo_full_forw(); servo_speed(SERVO_DIR_FORW, 50, 50);
avoid_obstacle = 0; avoid_obstacle = 0;
} }
if (avoid_obstacle == 1) { if (avoid_obstacle == 1) {
SET(LED_PORT, LED_BLUE_BIT); SET(LED_PORT, LED_GREEN_BIT);
} else { } else {
CLR(LED_PORT, LED_BLUE_BIT); CLR(LED_PORT, LED_GREEN_BIT);
return; return;
} }
_delay_ms(500);
distance_to_obstacle = read_sonar(); distance_to_obstacle = read_sonar();
} }
} }

54
robot/src/servos.c
View File

@ -23,38 +23,34 @@ void init_servos(void)
SET(SERVO_DDR, SERVO_R_BIT); 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)
{ {
if (load_percent_r == 100 && load_percent_l == 100) {
// Registers have been reset, need to set them again
if (TCCR0A == 0x00 || TCCR0B == 0x00)
init_servos();
if (direction == SERVO_DIR_FORW) {
SERVO_L_REG = US2TIMER0(SERVO_L_FORW); SERVO_L_REG = US2TIMER0(SERVO_L_FORW);
SERVO_R_REG = US2TIMER0(SERVO_R_FORW); SERVO_R_REG = US2TIMER0(SERVO_R_FORW);
} } else if (direction == SERVO_DIR_BACKW) {
void servo_turn_forw_r(void)
{
SERVO_L_REG = US2TIMER0(SERVO_L_FORW);
SERVO_R_REG = US2TIMER0(SERVO_R_STOP);
}
void servo_turn_forw_l(void)
{
SERVO_L_REG = US2TIMER0(SERVO_L_STOP);
SERVO_R_REG = US2TIMER0(SERVO_R_FORW);
}
void servo_full_backw(void)
{
SERVO_L_REG = US2TIMER0(SERVO_L_BACKW); SERVO_L_REG = US2TIMER0(SERVO_L_BACKW);
SERVO_R_REG = US2TIMER0(SERVO_R_BACKW); SERVO_R_REG = US2TIMER0(SERVO_R_BACKW);
} }
} else if (load_percent_r == 0 && load_percent_l == 0) {
void servo_turn_backw_r(void) TCCR0A = 0x00;
{ TCCR0B = 0x00;
SERVO_L_REG = US2TIMER0(SERVO_L_STOP); } else if (direction == SERVO_DIR_FORW) {
SERVO_R_REG = US2TIMER0(SERVO_R_BACKW); // Right and left have different arithmetic because in real life the
} // once of the servos is reversed
SERVO_L_REG = US2TIMER0(SERVO_L_STOP + ((SERVO_L_FORW - SERVO_L_STOP)
void servo_turn_backw_l(void) * load_percent_l) / 100);
{ SERVO_R_REG = US2TIMER0(SERVO_R_STOP - ((SERVO_L_STOP - SERVO_R_FORW)
SERVO_L_REG = US2TIMER0(SERVO_L_BACKW); * load_percent_r) / 100);
SERVO_R_REG = US2TIMER0(SERVO_R_STOP); } else if (direction == SERVO_DIR_BACKW) {
SERVO_L_REG = US2TIMER0(SERVO_L_STOP - ((SERVO_L_STOP - SERVO_L_FORW)
* load_percent_l) / 100);
SERVO_R_REG = US2TIMER0(SERVO_R_STOP + ((SERVO_R_BACKW - SERVO_R_STOP)
* load_percent_r) / 100);
}
} }