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

Fix Fast PWM output for servos

Browse Source
This commit is contained in:
Rihards Skuja 2018-01-22 13:01:37 +02:00
parent 363e2bfade
commit a843375c7e
No known key found for this signature in database
GPG Key ID: 53FA13A3F7F8571B
3 changed files with 32 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
robot/include/servos.h
View File

@ -1,22 +1,29 @@
#ifndef SERVOS_H #ifndef SERVOS_H
#define SERVOS_H #define SERVOS_H
#define SERVO_DDR DDRD #define SERVO_DDR DDRD
#define SERVO_L_REG OCR0B #define SERVO_L_REG OCR0A
#define SERVO_R_REG OCR0A #define SERVO_R_REG OCR0B
#define SERVO_L_BIT PD5 #define SERVO_L_BIT PD6
#define SERVO_R_BIT PD6 #define SERVO_R_BIT PD5
#define SERVO_MIN 930 // (us). Go backwards // Not just STOP +- 500us because of rounding errors (e.g. 19.9 will become
#define SERVO_MAX 1930 // (us). Go forwards // 19 and not 20 because of integer division)
#define SERVO_MID (SERVO_MIN + SERVO_MAX) / 2 // (us). Stop #define SERVO_MID 1381UL
#define SERVO_MAX 1883UL
#define SERVO_MIN 880UL
#define PRESCALER 1024L #define SERVO_L_STOP SERVO_MID
#define PWM_CLOCK F_CPU / (256 * PRESCALER) #define SERVO_L_FORW SERVO_MAX
#define PWM_PERIOD_US 1000000 / PWM_CLOCK #define SERVO_L_BACKW SERVO_MIN
#define US2TIMER0(us) (255 * (uint32_t)us) / PWM_PERIOD_US #define SERVO_R_STOP SERVO_MID
#define SERVO_R_FORW SERVO_MIN
#define SERVO_R_BACKW SERVO_MAX
#define PRESCALER 1024UL
#define PWM_PERIOD_MS (uint16_t)((256000UL * PRESCALER) / F_CPU)
#define US2TIMER0(us) (uint16_t)((51U * us) / (200U * PWM_PERIOD_MS))
void init_servos(void); void init_servos(void);
void run_servos(void); void run_servos(void);

6
robot/src/main.c
View File

@ -26,7 +26,7 @@ int main(void)
tx_raw = malloc(sizeof(data_packet_t)); tx_raw = malloc(sizeof(data_packet_t));
init_leds(); init_leds();
/* init_servos(); */ init_servos();
init_sonar(); init_sonar();
nrf24_init(); nrf24_init();
@ -40,8 +40,8 @@ int main(void)
/* SET(LED_PORT, LED_GREEN_BIT); */ /* SET(LED_PORT, LED_GREEN_BIT); */
sei(); sei();
/* run_servos(); */
/* read_temp(); */ run_servos();
while (1) { while (1) {
uint16_t distance_int_cm = read_sonar(); uint16_t distance_int_cm = read_sonar();

18
robot/src/servos.c
View File

@ -5,11 +5,10 @@
void init_servos(void) void init_servos(void)
{ {
SET(SERVO_DDR, SERVO_L_BIT); // Clear OC0A on Compare Match, set OC0A at BOTTOM (non-inverting mode)
SET(SERVO_DDR, SERVO_R_BIT); SET(TCCR0A, COM0A1);
// Clear OC0B on Compare Match, set OC0B at BOTTOM, (non-inverting mode)
SET(TCCR0A, COM0A1); // Clear OC0A on Compare Match, set OC0A at BOTTOM SET(TCCR0A, COM0B1);
SET(TCCR0A, COM0B1); // Clear OC0B on Compare Match, set OC0B at BOTTOM
// Fast PWM, 0xFF TOP // Fast PWM, 0xFF TOP
SET(TCCR0A, WGM00); SET(TCCR0A, WGM00);
@ -18,11 +17,14 @@ void init_servos(void)
// Prescaler = 1024 // Prescaler = 1024
SET(TCCR0B, CS00); SET(TCCR0B, CS00);
SET(TCCR0B, CS02); SET(TCCR0B, CS02);
// Set sevo pins as outputs
SET(SERVO_DDR, SERVO_L_BIT);
SET(SERVO_DDR, SERVO_R_BIT);
} }
void run_servos(void) void run_servos(void)
{ {
SERVO_L_REG = US2TIMER0(SERVO_MAX); SERVO_L_REG = US2TIMER0(SERVO_L_FORW);
SERVO_R_REG = US2TIMER0(SERVO_MID); SERVO_R_REG = US2TIMER0(SERVO_R_FORW);
_delay_ms(10000); // 10s
} }