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

29
robot/include/servos.h
View File

@ -2,21 +2,28 @@
#define SERVOS_H
#define SERVO_DDR DDRD
#define SERVO_L_REG OCR0B
#define SERVO_R_REG OCR0A
#define SERVO_L_BIT PD5
#define SERVO_R_BIT PD6
#define SERVO_L_REG OCR0A
#define SERVO_R_REG OCR0B
#define SERVO_L_BIT PD6
#define SERVO_R_BIT PD5
#define SERVO_MIN 930 // (us). Go backwards
#define SERVO_MAX 1930 // (us). Go forwards
#define SERVO_MID (SERVO_MIN + SERVO_MAX) / 2 // (us). Stop
// Not just STOP +- 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
#define SERVO_MIN 880UL
#define PRESCALER 1024L
#define PWM_CLOCK F_CPU / (256 * PRESCALER)
#define PWM_PERIOD_US 1000000 / PWM_CLOCK
#define SERVO_L_STOP SERVO_MID
#define SERVO_L_FORW SERVO_MAX
#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 run_servos(void);

6
robot/src/main.c
View File

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

18
robot/src/servos.c
View File

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