summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorFlorent Duchon2012-04-03 23:59:17 +0200
committerFlorent Duchon2012-04-03 23:59:17 +0200
commit7f9d0acca718ad5e1fc276d5c59c43525f61ab40 (patch)
treeb0eeb49c6fc74f8ca8b4a832510f1380e9406933
parent265bff4e052ced11bce4ea51cc1c798c21018c2e (diff)
digital/beacon: servomotor calibration
-rw-r--r--digital/beacon/src/servo.c202
-rw-r--r--digital/beacon/src/servo.h26
2 files changed, 37 insertions, 191 deletions
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 <types.h>
#include <irq.h>
#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<<PB5) | (1<<PB6);
-// /* All pins are at low state by default */
-//
-// /* Set-up the timer/counter 1:
-// - prescaler 1 => 4.44 ms TOP */
-// TCCR1B |= (1<<CS11)|(1<<CS10);
-// TCCR1A |= (1<<WGM10);
-//
-//
-// /* The state machine start with the first servo */
-// servo_updating_id_ = 0;
-//
-// /* Enable overflow interrupt */
-// TIMSK1 |= (1<<TOIE1);
-//
-// /* By default, servo init disable all servo. */
-// uint8_t i;
-// for (i = 0; i < SERVO_NUMBER; i++)
-// servo_set_high_time (i, 0x25);
-// }
-//
-// void servo_set_high_time (uint8_t servo, uint8_t high_time)
-// {
-// // DDRD=0xE0;
-// uint8_t filtered = high_time;
-// if (filtered != 0)
-// UTILS_BOUND (filtered, SERVO_HIGH_TIME_MIN, SERVO_HIGH_TIME_MAX);
-// /* Sanity check */
-// if (servo < SERVO_NUMBER)
-// /* Set new desired position (high value time) */
-// servo_high_time_[servo] = filtered;
-// }
-//
-//
-// uint8_t servo_get_high_time (uint8_t servo)
-// {
-// /* Sanity check */
-// if (servo < SERVO_NUMBER)
-// return servo_high_time_[servo];
-// return 0;
-// }
-//
-// SIGNAL (SIG_OVERFLOW1)
-// {
-// DDRD=0xE0;
-//
-// /* Overflow count (used when we wait in the lower state).
-// -1 is used for the first count where we wait less than a complete
-// overflow */
-// static int8_t servo_overflow_count = -1;
-// /* Time spent by each servo motor at high state during a whole cycle */
-// static uint16_t servo_high_time_cycle = servo_tic_cycle_;
-//
-// /* State machine actions */
-// if (servo_updating_id_ >= 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