summaryrefslogtreecommitdiff
path: root/n/es-2006/src/servo_motor.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/es-2006/src/servo_motor.c')
-rw-r--r--n/es-2006/src/servo_motor.c59
1 files changed, 39 insertions, 20 deletions
diff --git a/n/es-2006/src/servo_motor.c b/n/es-2006/src/servo_motor.c
index d01ceac..cd919c7 100644
--- a/n/es-2006/src/servo_motor.c
+++ b/n/es-2006/src/servo_motor.c
@@ -41,17 +41,16 @@
#define SRVM_TOTEML_PIN 3 /* first totem */
#define SRVM_TOTEMR_PIN 2 /* second totem */
/** Table for the time spend by each servo in high position. */
-volatile uint8_t servo_time[3] = { SRVM_POS_IN, SRVM_POS_IN, SRVM_POS_IN };
+volatile uint8_t servo_time[3] = { SRVM_TRASH_POS_CLOSE, SRVM_POS_IN, SRVM_POS_IN };
/** Different states of this module. */
-enum ServoMotorState
-{
- sleeping = 0,
- trash,
- totem_1,
- totem_2
-};
+
+#define SRVM_STATE_SLEEP 0
+#define SRVM_STATE_TRASH 1
+#define SRVM_STATE_TOTEM1 2
+#define SRVM_STATE_TOTEM2 3
+
/** State variable. */
-volatile enum ServoMotorState servo_motor_state;
+volatile uint8_t servo_motor_state;
/** Time the TC2 have to sleep after manage all the servo motor. */
volatile uint16_t srvm_need_to_sleep;
@@ -63,7 +62,7 @@ servo_motor_init (void)
DDRB |= _BV (SRVM_TRASH_PIN) | _BV (SRVM_TOTEML_PIN) |
_BV (SRVM_TOTEMR_PIN);
/* We are sleeping */
- servo_motor_state = sleeping;
+ servo_motor_state = SRVM_STATE_SLEEP;
/* Prescaler 256 => 4.44 ms TOP */
TCCR2 = regv (FOC2, WGM20, COM21, COM20, WGM21, CS22, CS21, CS20,
0, 0, 0, 0, 0, 1, 0, 0);
@@ -84,8 +83,14 @@ servo_motor_set_pos (uint8_t servo_num, uint8_t servo_pos)
{
/* Set the value and bound it */
servo_time[num] = servo_pos;
- UTILS_BOUND (servo_time[num], SRVM_POS_IN,
- SRVM_POS_OUT);
+ /* Trash */
+ if (num == 0)
+ UTILS_BOUND (servo_time[num], SRVM_TRASH_POS_OPEN,
+ SRVM_TRASH_POS_CLOSE);
+ /* Totem* */
+ else
+ UTILS_BOUND (servo_time[num], SRVM_POS_IN,
+ SRVM_POS_OUT);
}
}
@@ -103,7 +108,7 @@ SIGNAL (SIG_OVERFLOW2)
switch (servo_motor_state)
{
- case sleeping:
+ case SRVM_STATE_SLEEP:
/* We remove from the TOP the number of TIC of this servo */
TCNT2 = TC2_TOP - servo_time[servo_motor_state];
/* We activate the pin */
@@ -111,9 +116,9 @@ SIGNAL (SIG_OVERFLOW2)
/* We remove the TIC of this servo from the main one */
srvm_need_to_sleep -= servo_time[servo_motor_state];
/* Next state */
- servo_motor_state = trash;
+ servo_motor_state = SRVM_STATE_TRASH;
break;
- case trash:
+ case SRVM_STATE_TRASH:
/* We remove from the TOP the number of TIC of this servo */
TCNT2 = TC2_TOP - servo_time[servo_motor_state];
/* Unactivate previous pin */
@@ -123,9 +128,9 @@ SIGNAL (SIG_OVERFLOW2)
/* We remove the TIC of this servo from the main one */
srvm_need_to_sleep -= servo_time[servo_motor_state];
/* Next state */
- servo_motor_state = totem_1;
+ servo_motor_state = SRVM_STATE_TOTEM1;
break;
- case totem_1:
+ case SRVM_STATE_TOTEM1:
/* We remove from the TOP the number of TIC of this servo */
TCNT2 = TC2_TOP - servo_time[servo_motor_state];
/* Unactivate previous pin */
@@ -135,9 +140,9 @@ SIGNAL (SIG_OVERFLOW2)
/* We remove the TIC of this servo from the main one */
srvm_need_to_sleep -= servo_time[servo_motor_state];
/* Next state */
- servo_motor_state = totem_2;
+ servo_motor_state = SRVM_STATE_TOTEM2;
break;
- case totem_2:
+ case SRVM_STATE_TOTEM2:
if (srvm_ov_for_sleeping == -1)
{
/* We need to wait some TIC */
@@ -152,7 +157,7 @@ SIGNAL (SIG_OVERFLOW2)
if (--srvm_ov_for_sleeping == -1)
{
/* Next state */
- servo_motor_state = sleeping;
+ servo_motor_state = SRVM_STATE_SLEEP;
/* Re init */
srvm_need_to_sleep = SRVM_CYCLE_DELAY;
}
@@ -160,3 +165,17 @@ SIGNAL (SIG_OVERFLOW2)
break;
}
}
+
+/** Open the trash. */
+void
+servo_motor_open_trash (void)
+{
+ servo_motor_set_pos (1, SRVM_TRASH_POS_OPEN);
+}
+
+/** Close the trash. */
+void
+servo_motor_close_trash (void)
+{
+ servo_motor_set_pos (1, SRVM_TRASH_POS_CLOSE);
+}