@@ -4,21 +4,9 @@ DO NOT FLASH TO TEENSY OR TO ARDUINO
4
4
5
5
*/
6
6
7
- // test MCU kill switch
8
- // test water interrupt
9
- // test delay needed for thruster initialization
10
- // test if ESCs arm after kill switches
11
- // arm all thrusters
12
-
13
7
#include < Servo.h>
14
8
15
- // boolean for interrupts
16
- volatile bool interruptFlag = false ;
17
-
18
9
// defines all MCU pins
19
- #define SYSTEM_KILLED 0
20
- #define THURSTERS_KILLED 1
21
-
22
10
#define SRG_P_PIN 2
23
11
#define SRG_S_PIN 3
24
12
#define SWY_BW_PIN 4
@@ -87,8 +75,8 @@ void initThrusters() {
87
75
88
76
updateThrusters (offCommand);
89
77
delay (7000 );
90
-
91
- interruptFlag = false ;
78
+ // reamring works when system killed automatically at 2000
79
+ // 7000 should be tested since it is more reliable based on bluerobotics
92
80
}
93
81
94
82
void killSystem () {
@@ -101,38 +89,19 @@ void powerSystem() {
101
89
delay (100 );
102
90
}
103
91
104
- void interruptRaised () {
105
- interruptFlag = true ;
106
- }
107
-
108
92
void waterInterrupt () {
109
- if (digitalRead (WATER_DETECTED) == HIGH) {
110
- killSystem ();
111
- } else {
112
- powerSystem ();
113
- }
93
+ killSystem ();
94
+ while (true ) {}
114
95
}
115
96
116
97
void setup () {
117
- pinMode (SYSTEM_KILLED, INPUT_PULLUP);
118
- pinMode (THURSTERS_KILLED, INPUT_PULLUP);
119
- pinMode (WATER_DETECTED, INPUT_PULLUP);
120
-
121
- pinMode (MCU_KS, OUTPUT);
122
-
123
- attachInterrupt (digitalPinToInterrupt (SYSTEM_KILLED), interruptRaised, RISING);
124
- attachInterrupt (digitalPinToInterrupt (THURSTERS_KILLED), interruptRaised, RISING);
125
- attachInterrupt (digitalPinToInterrupt (WATER_DETECTED), waterInterrupt, CHANGE);
98
+ // pinMode(WATER_DETECTED, INPUT_PULLUP);
99
+ // pinMode(MCU_KS, OUTPUT);
100
+ // attachInterrupt(digitalPinToInterrupt(WATER_DETECTED), waterInterrupt, RISING);
126
101
127
102
initThrusters ();
128
103
}
129
104
130
105
void loop () {
131
- if (interruptFlag) {
132
- initThrusters ();
133
- }
134
-
135
- if (digitalRead (WATER_DETECTED) == LOW) {
136
- updateThrusters (microseconds);
137
- }
106
+ updateThrusters (microseconds);
138
107
}
0 commit comments