+static unsigned short get_voltage(void) {
+ return ReadChannel(2)*1.46;
+}
+
+
+/*
+ * zero offset: 0.5V
+ * sensitivity: 133mV/A
+ */
+static unsigned short get_current(void) {
+ double volt = ReadChannel(3)*5.0171; // mV
+ return (volt-517.78)/0.12656; // mA
+}
+
+
+ISR(INT0_vect) {
+ static uint16_t t_start=0;
+ uint16_t t_now = TCNT1;
+ uint16_t t_diff;
+
+ if (bit_is_set(PIND, 2)) { // high level
+ // start timer
+ t_start = t_now;
+ } else {
+ t_diff = t_now - t_start;
+ dist_forward1 = t_diff*2.7586 + 0.5; // t [µs] / 580 = mm
+ // disable this interrupt
+ EIMSK &= ~(1 << INT0);
+ }
+}
+
+
+ISR(INT1_vect) {
+ static uint16_t t_start=0;
+ uint16_t t_now = TCNT1;
+ uint16_t t_diff;
+
+ if (bit_is_set(PIND, 3)) { // high level
+ // start timer
+ t_start = t_now;
+ } else {
+ t_diff = t_now - t_start;
+ dist_backward = t_diff*2.7586 + 0.5; // t [µs] / 580 = mm
+ // disable this interrupt
+ EIMSK &= ~(1 << INT1);
+ }
+}
+
+
+ISR(PCINT2_vect) {
+ uint8_t pind_cur = PIND;
+
+ if ((pind_cur ^ pind_pre) & (1<<4)) { // PCINT20
+ static uint16_t t_start=0;
+ uint16_t t_now = TCNT1;
+ uint16_t t_diff;
+
+ if (bit_is_set(pind_cur, 4)) { // high level
+ // start timer
+ t_start = t_now;
+ } else {
+ t_diff = t_now - t_start;
+ dist_forward2 = t_diff*2.7586 + 0.5; // t [µs] / 580 = mm
+ // disable this interrupt
+ PCMSK2 &= ~(1 << PCINT20);
+ }
+ }
+
+ pind_pre = pind_cur;
+}
+
+