-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathGreigeobar.ino
281 lines (227 loc) · 7.61 KB
/
Greigeobar.ino
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
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
//#define GREIGE
#define ZINNOBAR
#include "smart_bot.h"
#include "states.h"
#include "smart_motor.h"
//#define POT_A A14
//#define POT_B A15
// cool lights ----- //
#define BREAKLIGHTS 49
#define HEADLIGHTS 53
// state lights -------- //
#define LED_STATE_BLUE 14
#define LED_STATE_GREEN 15
#define LED_STATE_RED 16
#define LED_RED_TRACK 17
#define LED_BLUE_TRACK 19
#define LED_YELLOW_TRACK 18
// state switches ------ //
#define STATE_SWITCH_1 47
#define STATE_SWITCH_2 45
#define STATE_SWITCH_3 43
#define STATE_SWITCH_4 41
// sensor network ------- //
#define L_LIGHT_SENSOR A6 // ?
#define R_LIGHT_SENSOR A7 // ?
#define HALL 3 //
#define COLLISION 2 //
#define RED_LED 39 //
#define BLUE_LED 37 //
#define PIN1_M1 6 //
#define PIN2_M1 7 //
#define PIN1_M2 5 //
#define PIN2_M2 4 //
#define PINE_M 21 //
// ---------------------------------------------- //
#define HALL_DEBOUNCE 250 // DEBOUNCE TIME
#define COLLISION_DEBOUNCE 250 // FOR INTERRUPT SENSORS
#define STABILIZATION_TIME 20
#define STARTUP_TIME 1000
bool ROUTINE_STARTED = false;
void startup_script();
#ifdef ZINNOBAR
// communicaton ----- //
#define L_TURNSIGNAL 9
#define R_TURNSIGNAL 8
#define RECEIVING A15 //
#define SENDING 38 //
#define MOTORR_RATIO 0.9
#define MOTORL_RATIO 0.9
const float redBsl[NPARAMS+1] = {-6.068814, -18.489579, 29.352758};
const float bluBsl[NPARAMS+1] = {-12.743714, 22.864221, -3.947755};
const float redBsr[NPARAMS+1] = {-3.680531, -16.105715, 22.564435};
const float bluBsr[NPARAMS+1] = {-10.350145, 24.930389, -8.754170};
#endif
#ifdef GREIGE
// communicaton ----- //
#define L_TURNSIGNAL 9 //
#define R_TURNSIGNAL 8 //
#define RECEIVING A5 //
#define SENDING 38 //
#define MOTORR_RATIO 0.9
#define MOTORL_RATIO 0.9
const float redBsr[NPARAMS+1] = {-14.674660, 1.128543, 21.177556};
const float bluBsr[NPARAMS+1] = {-7.286187, 34.452386, -22.572911};
const float redBsl[NPARAMS+1] = {-5.795165, -13.741407, 25.120166};
const float bluBsl[NPARAMS+1] = {-4.256994, 25.752916, -18.000910};
#endif
smart_bot Bot ( LED_YELLOW_TRACK, LED_BLUE_TRACK, LED_RED_TRACK,
LED_STATE_GREEN, LED_STATE_BLUE , LED_STATE_RED,
HEADLIGHTS , BREAKLIGHTS,
L_TURNSIGNAL , R_TURNSIGNAL,
PIN1_M1, PIN2_M1, MOTORR_RATIO,
PIN1_M2, PIN2_M2, MOTORL_RATIO, PINE_M,
SENDING, RECEIVING,
L_LIGHT_SENSOR, R_LIGHT_SENSOR,
HALL, COLLISION,
RED_LED, BLUE_LED, STABILIZATION_TIME,
redBsl, bluBsl, redBsr, bluBsr);
/* DO NOT MOVE THIS INCLUDE VVVV */
#include "program.h"
/* DO NOT MOVE THIS INCLUDE ^^^^ */
void setup ()
{
/* ----------------------------------------------- *
* PINMODE SETUPS
* ----------------------------------------------- */
Serial.begin(9600);
while (!Serial) {}
Serial.print("printing ready\n");
//pinMode (POT_A, INPUT);
//pinMode (POT_B, INPUT);
// cool lights ---------
pinMode( L_TURNSIGNAL, OUTPUT);
pinMode( R_TURNSIGNAL, OUTPUT);
pinMode( BREAKLIGHTS , OUTPUT);
pinMode( HEADLIGHTS , OUTPUT);
// state lights -----------
pinMode( LED_STATE_GREEN , OUTPUT);
pinMode( LED_STATE_RED , OUTPUT);
pinMode( LED_STATE_BLUE , OUTPUT);
pinMode( LED_RED_TRACK , OUTPUT);
pinMode( LED_YELLOW_TRACK , OUTPUT);
pinMode( LED_BLUE_TRACK , OUTPUT);
// state switches ---------
pinMode( STATE_SWITCH_1 , INPUT_PULLUP);
pinMode( STATE_SWITCH_2 , INPUT_PULLUP);
pinMode( STATE_SWITCH_3 , INPUT_PULLUP);
pinMode( STATE_SWITCH_4 , INPUT_PULLUP);
// sensor network ----------- //
pinMode( L_LIGHT_SENSOR , INPUT); // ?
pinMode( R_LIGHT_SENSOR , INPUT); // ?
pinMode( HALL , INPUT); //
pinMode( COLLISION , INPUT_PULLUP); //
pinMode( RED_LED , OUTPUT); //
pinMode( BLUE_LED , OUTPUT); //
// communicaton ------------- //
pinMode( RECEIVING , INPUT ); // ?
pinMode( SENDING , OUTPUT ); // ?
// motors ------------------- //
pinMode( PIN1_M1 , OUTPUT); // ? ? ? ?
pinMode( PIN2_M1 , OUTPUT); // ? ? ?
pinMode( PIN1_M2 , OUTPUT); // ? ?
pinMode( PIN2_M2 , OUTPUT); // ?
// ENSURE MOTORS OFF
analogWrite(PIN1_M1,0);
analogWrite(PIN2_M1,0);
analogWrite(PIN1_M2,0);
analogWrite(PIN2_M2,0);
attachInterrupt(digitalPinToInterrupt(HALL), hall_detect, FALLING);
attachInterrupt(digitalPinToInterrupt(COLLISION), collision_detect_startup, FALLING);
interrupts();
}
/* --------------------------------------------------------------- */
void loop ()
{
//Serial.println(__FILE__);
//Bot.lmotor.debug();
//Bot.rmotor.debug();
digitalWrite (L_TURNSIGNAL,LOW);
digitalWrite (BREAKLIGHTS,HIGH);
// ----------------------------------
startup_script();
long long timer = millis ();
int left_headlight;
int right_headlight;
while (!ROUTINE_STARTED) {
Bot.sensors.sense();
left_headlight = analogRead(HEADLIGHTS_SENSE_L);
right_headlight = analogRead(HEADLIGHTS_SENSE_R);
VAR_(left_headlight);
Serial.print(" | ");
VAR_ln(right_headlight);
}
// attach the normal interrupt
attachInterrupt(digitalPinToInterrupt(COLLISION), collision_detect, FALLING);
int routine =
((digitalRead(STATE_SWITCH_1)^1) << 3)
+ ((digitalRead(STATE_SWITCH_2)^1) << 2)
+ ((digitalRead(STATE_SWITCH_3)^1) << 1)
+ ((digitalRead(STATE_SWITCH_4)^1) << 0);
Serial.print ("ROUTINE [");
Serial.print (routine);
Serial.println("]");
//----------------------------------
/*
Serial.print ("Lmotor: ");
Serial.println(analogRead(POT_A)>>2);
c1b1_starting_run2.Lmotor_speed = analogRead(POT_A)>>2;
Serial.print ("Rmotor: ");
Serial.println(analogRead(POT_B)>>2);
c1b1_starting_run2.Rmotor_speed = analogRead(POT_B)>>2;
*/
Bot.run_(STATE_PROGRAM,routine);
}
/* --------------------------------------------------------------- */
void hall_detect ()
{
static unsigned long LastPress = 0;
static const unsigned long debounce_delay = HALL_DEBOUNCE;
attachInterrupt(digitalPinToInterrupt(HALL), nothing, FALLING);
interrupts();
if (millis() - LastPress > debounce_delay) {
LastPress = millis();
Bot.hall_interrupt = true;
}
}
void collision_detect ()
{
static unsigned long LastPress = 0;
static const unsigned long debounce_delay = COLLISION_DEBOUNCE;
attachInterrupt(digitalPinToInterrupt(COLLISION), nothing, FALLING);
interrupts();
if (millis() - LastPress > debounce_delay) {
LastPress = millis();
Bot.collision_interrupt = true;
}
}
void collision_detect_startup ()
{
static unsigned long LastPress = 0;
static const unsigned long debounce_delay = COLLISION_DEBOUNCE;
attachInterrupt(digitalPinToInterrupt(COLLISION), nothing, FALLING);
interrupts();
if (millis() - LastPress > debounce_delay) {
LastPress = millis();
ROUTINE_STARTED = true;
}
}
void startup_script()
{
Serial.println("running...");
long long timer = millis();
Bot.leds.red_state.on_flash();
Bot.leds.green_state.on_flash();
Bot.leds.blue_state.on_flash();
Bot.leds.red_path.on_flash();
Bot.leds.blue_path.on_flash();
Bot.leds.yellow_path.on_flash();
Bot.leds.headlights.on_flash();
Bot.leds.breaklights.on_flash();
Bot.leds.lturn.on_flash();
Bot.leds.rturn.on_flash();
while (STARTUP_TIME > millis() - timer) Bot.leds.maintain();
Bot.leds.reset();
Bot.leds.maintain();
}
void nothing () {}