make ergodone's led work well

2 files changed, 98 insertions, 12 deletions

diff --git a/keyboards/ergodone/ergodone.c b/keyboards/ergodone/ergodone.c
index 6b8d8a063..dd05a8def 100644
--- a/keyboards/ergodone/ergodone.c
+++ b/keyboards/ergodone/ergodone.c
@@ -1,5 +1,52 @@
 #include "ergodone.h"
 
+extern inline void ergodox_board_led_on(void);
+extern inline void ergodox_right_led_1_on(void);
+extern inline void ergodox_right_led_2_on(void);
+extern inline void ergodox_right_led_3_on(void);
+extern inline void ergodox_right_led_on(uint8_t led);
+
+extern inline void ergodox_board_led_off(void);
+extern inline void ergodox_right_led_1_off(void);
+extern inline void ergodox_right_led_2_off(void);
+extern inline void ergodox_right_led_3_off(void);
+extern inline void ergodox_right_led_off(uint8_t led);
+
+void ergodox_led_init(void);
+void ergodox_blink_all_leds(void);
+
 void matrix_init_kb(void) {
+  ergodox_led_init();
+  ergodox_blink_all_leds();
   matrix_init_user();
 }
+
+void ergodox_led_init(void)
+ {
+     DDRB  |=  (1<<PB5 | 1<<PB6 | 1<<PB3);
+     PORTB &= ~(1<<PB5 | 1<<PB6 | 1<<PB3);
+     DDRB  |=  (1<<PB0);
+     PORTB |=  (1<<PB0);
+     DDRD  |=  (1<<PB5);
+     PORTD |=  (1<<PB5);
+}
+
+void ergodox_blink_all_leds(void)
+{
+    ergodox_led_all_off();
+    ergodox_led_all_set(LED_BRIGHTNESS_HI);
+    ergodox_right_led_1_on();
+    _delay_ms(50);
+    ergodox_right_led_2_on();
+    _delay_ms(50);
+    ergodox_right_led_3_on();
+    _delay_ms(50);
+    ergodox_right_led_1_off();
+    _delay_ms(50);
+    ergodox_right_led_2_off();
+    _delay_ms(50);
+    ergodox_right_led_3_off();
+    //ergodox_led_all_on();
+    //_delay_ms(333);
+    ergodox_led_all_off();
+}
diff --git a/keyboards/ergodone/ergodone.h b/keyboards/ergodone/ergodone.h
index 9f6c8f841..0826e9521 100644
--- a/keyboards/ergodone/ergodone.h
+++ b/keyboards/ergodone/ergodone.h
@@ -10,18 +10,57 @@
 
 void init_ergodox(void);
 
-inline void ergodox_right_led_1_off(void)               {}
-inline void ergodox_right_led_1_on(void)                {}
-inline void ergodox_right_led_2_off(void)               {}
-inline void ergodox_right_led_2_on(void)                {}
-inline void ergodox_right_led_3_off(void)               {}
-inline void ergodox_right_led_3_on(void)                {}
-inline void ergodox_right_led_on(uint8_t l)             {}
-inline void ergodox_right_led_off(uint8_t l)            {}
-inline void ergodox_board_led_off(void)                 {}
-inline void ergodox_board_led_on(void)                  {}
-inline void ergodox_led_all_on(void)                    {}
-inline void ergodox_led_all_off(void)                   {}
+inline void ergodox_right_led_1_off(void) { DDRB &= ~(1<<PB5); PORTB &= ~(1<<PB5); }
+inline void ergodox_right_led_1_on(void) { DDRB |= (1<<PB5); PORTB |= (1<<PB5); }
+inline void ergodox_right_led_2_off(void) { DDRB &= ~(1<<PB6); PORTB &= ~(1<<PB6); }
+inline void ergodox_right_led_2_on(void) { DDRB |= (1<<PB6); PORTB |= (1<<PB6); }
+inline void ergodox_right_led_3_off(void) { DDRB &= ~(1<<PB3); PORTB &= ~(1<<PB3); }
+inline void ergodox_right_led_3_on(void) { DDRB |= (1<<PB3); PORTB |= (1<<PB3); }
+inline void ergodox_right_led_on(uint8_t l) {
+    switch (l) {
+         case 1:
+             ergodox_right_led_1_on();
+             break;
+         case 2:
+             ergodox_right_led_2_on();
+             break;
+         case 3:
+             ergodox_right_led_3_on();
+             break;
+         default:
+             break;
+        }
+}
+
+inline void ergodox_right_led_off(uint8_t l) {
+    switch (l) {
+         case 1:
+             ergodox_right_led_1_off();
+             break;
+         case 2:
+             ergodox_right_led_2_off();
+             break;
+         case 3:
+             ergodox_right_led_3_off();
+             break;
+         default:
+             break;
+        }
+}
+inline void ergodox_board_led_off(void) { DDRB &= ~(1<<PB0); PORTB |= (1<<PB0); }
+inline void ergodox_board_led_on(void) { DDRB |= (1<<PB0); PORTB &= ~(1<<PB0); }
+inline void ergodox_led_all_on(void) {
+    ergodox_right_led_1_on();
+    ergodox_right_led_2_on();
+    ergodox_right_led_3_on();
+    ergodox_board_led_on();
+}
+inline void ergodox_led_all_off(void) {
+    ergodox_right_led_1_off();
+    ergodox_right_led_2_off();
+    ergodox_right_led_3_off();
+    ergodox_board_led_off();
+}
 inline void ergodox_right_led_1_set(uint8_t n)          {}
 inline void ergodox_right_led_2_set(uint8_t n)          {}
 inline void ergodox_right_led_3_set(uint8_t n)          {}