From 7f9d0acca718ad5e1fc276d5c59c43525f61ab40 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Tue, 3 Apr 2012 23:59:17 +0200 Subject: digital/beacon: servomotor calibration --- digital/beacon/src/servo.c | 202 +++++---------------------------------------- digital/beacon/src/servo.h | 26 ++++-- 2 files changed, 37 insertions(+), 191 deletions(-) (limited to 'digital') diff --git a/digital/beacon/src/servo.c b/digital/beacon/src/servo.c index 7d7e5dc4..96a5b0d4 100644 --- a/digital/beacon/src/servo.c +++ b/digital/beacon/src/servo.c @@ -26,15 +26,15 @@ #include #include #include "servo.h" - +#include "debug.h" /* This function initializes the timer used for servomotor signal generation */ -void SERVO_timer1_init(void) +void servo_timer1_init(void) { //Fpwm = f_IO / (prescaler * (1 + TOP)) = 7200 Hz. */ - OCR1B = 210; - OCR1A = 279; + OCR1B = SERVO_1_ANGLE_INIT; + OCR1A = SERVO_2_ANGLE_INIT; /* Fast PWM 10bits with TOP=0x03FF */ @@ -62,220 +62,58 @@ void SERVO_timer1_init(void) } -/* This function increase by one unit the angle of the defined servo */ -void SERVO_angle_increase(int servo_id) +/* This function increase by one unit the angle of the defined servo and returns "angle" value */ +int servo_angle_increase(TServo_ID servo_id) { switch(servo_id) { - case 1: + case SERVO_1: if(OCR1A < SERVO_1_ANGLE_MAX) { OCR1A++; + return OCR1A; } break; - case 2: + case SERVO_2: if(OCR1B < SERVO_2_ANGLE_MAX) { OCR1B++; + return OCR1B; } break; default: - return; + break; } + return 0; } -/* This function decrease by one unit the angle of the defined servo */ -void SERVO_angle_decrease(int servo_id) +/* This function decrease by one unit the angle of the defined servo and returns "angle" value*/ +int servo_angle_decrease(TServo_ID servo_id) { switch(servo_id) { - case 1: + case SERVO_1: if(OCR1A > SERVO_1_ANGLE_MIN) { OCR1A--; + return OCR1A; } break; - case 2: + case SERVO_2: if(OCR1B > SERVO_2_ANGLE_MIN) { OCR1B--; + return OCR1B; } break; default: - return; + break; } + return 0; } SIGNAL (SIG_OVERFLOW1) { } - -// #define SERVO_NUMBER 8 -// #define SERVO_HIGH_TIME_MIN 0x24 -// #define SERVO_HIGH_TIME_MAX 0x88 -// #define SERVO_TCNT_TOP 0x00FF -// #define set_bit(port, bit) (port |= _BV(bit)) -// -// #define UTILS_BOUND(v, min, max) \ -// do { \ -// if ((v) < (min)) \ -// (v) = (min); \ -// else if ((v) > (max)) \ -// (v) = (max); \ -// } while (0) -// -// -// -// volatile int8_t servo_updating_id_; -// volatile uint8_t servo_high_time_[SERVO_NUMBER]; -// static const uint16_t servo_tic_cycle_ = 8000000 / 64 * 20 / 1000; -// -// union _utils_byte_access -// { -// uint8_t v8[4]; -// uint16_t v16[2]; -// uint32_t v32; -// }; -// -// /** Byte packing macro, pack 4 bytes into a double word. */ -// extern inline uint32_t -// v8_to_v32 (uint8_t b3, uint8_t b2, uint8_t b1, uint8_t b0) -// { -// union _utils_byte_access ba; -// ba.v8[0] = b0; -// ba.v8[1] = b1; -// ba.v8[2] = b2; -// ba.v8[3] = b3; -// return ba.v32; -// } -// -// extern inline uint8_t -// v16_to_v8 (uint16_t w, int pos) -// { -// union _utils_byte_access ba; -// ba.v16[0] = w; -// return ba.v8[pos]; -// } -// -// -// -// SIGNAL (SIG_OVERFLOW2); -// -// -// void servo_apbteam_init (void) -// { -// /* Set-up all the pins of the servo to out direction */ -// DDRB |= (1< 4.44 ms TOP */ -// TCCR1B |= (1<= 0) -// { -// /* Servos motor high state mode */ -// -// /* Set to low state the previous servo motor pin if needed (not for -// * the first one) */ -// if (servo_updating_id_ != 0) -// PORTB &= ~_BV (servo_updating_id_ - 1); -// /* Set to high state the current servo motor pin, unless is zero */ -// if (servo_high_time_[servo_updating_id_]) -// set_bit (PORTB, servo_updating_id_); -// /* Plan next timer overflow to the TOP minus the current configuration -// * of the servo motor */ -// TCNT1 = SERVO_TCNT_TOP - servo_high_time_[servo_updating_id_]; -// /* Update the time spent at high state by all servo motors for this -// * cycle */ -// servo_high_time_cycle += servo_high_time_[servo_updating_id_]; -// /* Update the identifier of the current servo motor (and manage when -// * we are at the last one) */ -// if (++servo_updating_id_ == SERVO_NUMBER) -// servo_updating_id_ = -1; -// } -// else -// { -// -// /* Sleeping time mode */ -// -// /* Is it the first we are in this mode? */ -// if (servo_overflow_count == -1) -// { -// /* Set to low state the previous servo motor pin */ -// PORTB &= ~_BV (SERVO_NUMBER - 1); -// /* Number of full overflow (from 0 to SERVO_TCNT_TOP) we need to -// * wait (division by SERVO_TCNT_TOP or >> 8) */ -// servo_overflow_count = servo_high_time_cycle >> 8; -// /* Restart the counter from remaining TIC that are left and can -// * not be used to make a full overflow */ -// TCNT1 = SERVO_TCNT_TOP - v16_to_v8 (servo_high_time_cycle, 0); -// } -// else -// { -// /* We just have an overflow, are we at the last one needed? The -1 -// * is normal: we do not count the first overflow of the sleeping -// * mode because it is not a full one */ -// if (--servo_overflow_count == -1) -// { -// /* Restart with first servo motor */ -// servo_updating_id_ = 0; -// /* Re-initialize the counter of time spent by each servo motor -// * at high state */ -// servo_high_time_cycle = servo_tic_cycle_; -// } -// } -// } -// } - - - diff --git a/digital/beacon/src/servo.h b/digital/beacon/src/servo.h index 52580575..eb3ab0fd 100644 --- a/digital/beacon/src/servo.h +++ b/digital/beacon/src/servo.h @@ -26,19 +26,27 @@ #ifndef _SERVO_H #define _SERVO_H -#define SERVO_1_ANGLE_MIN 3000 -#define SERVO_1_ANGLE_MAX 3000 -#define SERVO_2_ANGLE_MIN 3000 -#define SERVO_2_ANGLE_MAX 3000 +#define SERVO_1_ANGLE_INIT 150 +#define SERVO_1_ANGLE_MIN 69 +#define SERVO_1_ANGLE_MAX 300 +#define SERVO_2_ANGLE_INIT 150 +#define SERVO_2_ANGLE_MIN 69 +#define SERVO_2_ANGLE_MAX 300 +/* SERVO ID */ +typedef enum +{ + SERVO_1=1, + SERVO_2 +} TServo_ID; /* This function initializes the timer used for servomotor signal generation */ -void SERVO_timer1_init(void); +void servo_timer1_init(void); -/* This function increase by one unit the angle of the defined servo */ -void SERVO_angle_increase(int servo_id); +/* This function increase by one unit the angle of the defined servo and returns "angle" value */ +int servo_angle_increase(TServo_ID servo_id); -/* This function decrease by one unit the angle of the defined servo */ -void SERVO_angle_decrease(int servo_id); +/* This function decrease by one unit the angle of the defined servo and returns "angle" value*/ +int servo_angle_decrease(TServo_ID servo_id); #endif \ No newline at end of file -- cgit v1.2.3