1 files changed, 39 insertions, 10 deletions

diff --git a/fw/main.c b/fw/main.c
index 0d08576..7db22ce 100644
--- a/fw/main.c
+++ b/fw/main.c
@@ -40,6 +40,8 @@ void run_menu(void);
 void update_display(void);
 void calc_next_alarm(void);
 void update_servo(void);
+void servo_activate(void);
+void servo_deactivate(void);
 
 uint8_t random() {
     static uint8_t x, a, b, c;
@@ -202,7 +204,7 @@ enum {
 
 int servo_lvl_set[NUM_SERVO_LEVELS] = {
     [LVL_SERVO_ON] = 1050,
-    [LVL_SERVO_IDLE] = 1220,
+    [LVL_SERVO_IDLE] = 1250,
     [LVL_SERVO_OFF] = 1390,
 };
 
@@ -212,8 +214,10 @@ const char *servo_lvl_string[NUM_SERVO_LEVELS] = {
     [LVL_SERVO_ON] = "ON",
 };
 
-const int servo_lvl_set_ms = 1000;
+const int servo_reps_set = 3;
+const int servo_lvl_set_ms[] = { 1000, 1000, 1500, 1500, 1000, 1000 };
 int servo_lvl_ms = 0;
+int servo_reps = 0;
 
 enum {
     MENU_DEFAULT,
@@ -243,7 +247,7 @@ enum {
 
 int alarms[NUM_ALARMS] = {
     9<<16 | 0<<8 | 0,
-    21<<16 | 0<<8 | 0,
+    20<<16 | 0<<8 | 0,
 };
 const char *alarm_names[NUM_ALARMS] = {" ON", "OFF"};
 int next_alarm = -1;
@@ -255,13 +259,21 @@ void RTC_IRQHandler() {
     /* TODO FIXME handle alarm */
 }
 
-void update_servo() {
-    if (next_alarm == ALR_TON) {
+void servo_activate() {
+    if (next_alarm == ALR_TON)
         TIM3->CCR1 = servo_lvl_set[LVL_SERVO_ON];
-    } else {
+    else
         TIM3->CCR1 = servo_lvl_set[LVL_SERVO_OFF];
-    }
-    servo_lvl_ms = servo_lvl_set_ms;
+}
+
+void servo_deactivate() {
+    TIM3->CCR1 = servo_lvl_set[LVL_SERVO_IDLE];
+}
+
+void update_servo() {
+    servo_activate();
+    servo_reps = servo_reps_set * 2 - 1;
+    servo_lvl_ms = servo_lvl_set_ms[servo_reps_set];
 }
 
 void adj_alarm(int alarm, int sel, int adj) {
@@ -319,10 +331,17 @@ void calc_next_alarm() {
 }
 
 void update_display() {
+    static int updates_since_init = 0;
     char buf[17];
     bool blink_flag = sys_time_ms < 300;
     int tr = RTC->TR;
 
+    updates_since_init++;
+    if (updates_since_init > 20000) {
+        lcd1602_init();
+        updates_since_init = 0;
+    }
+
     switch (menu_state) {
         case MENU_DEFAULT:
             mini_snprintf(buf, sizeof(buf), "%d%d:%d%d:%d%d" LCD_FILL,
@@ -581,8 +600,18 @@ void SysTick_Handler(void) {
     if (servo_lvl_ms > 0) {
         servo_lvl_ms -= TICK_MS;
         if (servo_lvl_ms <= 0) {
-            servo_lvl_ms = 0;
-            TIM3->CCR1 = servo_lvl_set[LVL_SERVO_IDLE];
+            
+            if (servo_reps & 1)
+                servo_activate();
+            else
+                servo_deactivate();
+
+            if (servo_reps > 0) {
+                servo_reps --;
+                servo_lvl_ms = servo_lvl_set_ms[servo_reps_set];
+            } else {
+                servo_lvl_ms = 0;
+            }
         }
     }
 }