MiniRaceLogic/Optic.cpp at main · pink0D/MiniRaceLogic · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
#include "Settings.h"
#include "Optic.h"
#include "PowerControl.h"

static Optic s_Optic;

void int_optic() {
  s_Optic.handle_interrupt();
  s_PowerControl.timer_reset();
}

void Optic::handle_interrupt() {

  if (test_mode)
    return;

  int new_signal = digitalRead(OPTIC_SIGNAL_PIN);
  unsigned long old_update_time = update_time;

  if (signal != new_signal){
      signal = new_signal;
      update_time = millis();

      if (signal == 1) {
        velocity = (CAR_VELOCITY_SCALE * CAR_LENGTH_MM / (double) (update_time - old_update_time)); // meters per second

        if ( (velocity < 5) || (velocity > 500) )
          velocity = 0;
      }

      optic_updated = true;
  }
}

void Optic::begin() {
  pinMode(OPTIC_SENSOR_POWER_PIN, OUTPUT);
  digitalWrite(OPTIC_SENSOR_POWER_PIN, HIGH);

  pinMode(OPTIC_LED_POWER_PIN, OUTPUT);
  digitalWrite(OPTIC_LED_POWER_PIN, HIGH);

  pinMode(OPTIC_SIGNAL_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(OPTIC_SIGNAL_PIN), int_optic, CHANGE);
}

bool Optic::get_update_flag() {
  return optic_updated;
}

unsigned long Optic::get_update_time() {
  return update_time;
}

int Optic::get_signal() {
  return signal;
}

double Optic::get_velocity() { // meters per seccond
  return velocity;
}

void Optic::reset_update_flag() {
  optic_updated = false;
}

void Optic::reset_velocity() {
  velocity = 0;
}

void Optic::set_test_mode(bool mode) {
  noInterrupts();
  test_mode = mode;
  interrupts();
}

bool Optic::test() {

  bool test_result = true;
  set_test_mode(true);

  digitalWrite(OPTIC_LED_POWER_PIN, LOW);
  delay(50);
  if ( digitalRead(OPTIC_SIGNAL_PIN) != 0)
    test_result = false;

  digitalWrite(OPTIC_LED_POWER_PIN, HIGH);
  delay(50);
  if ( digitalRead(OPTIC_SIGNAL_PIN) != 1)
    test_result = false;

  set_test_mode(false);
  return test_result;
}