SmartVehicle-Sortic
SmartFactory
main.cpp
Go to the documentation of this file.
1 
16 #include "Arduino.h"
17 #include "LogConfiguration.h"
18 #include "VehicleCtrl.h"
19 
20 // #include "Hoist.h"
21 
22 // #include "DriveCtrl.h"
23 // #include "EnvironmentDetection.h"
24 // #include "HoistCtrl.h"
25 // #include "MQTTCommunication.h"
26 // #include "NavigationCtrl.h"
27 
28 // enum class TestCase {
29 // RUN,
30 // HOIST,
31 // HOISTCTRL,
32 // DRIVE,
33 // DRIVECTRL,
34 // ENVDETEC,
35 // NAVIGATIONCTRL,
36 // VISION,
37 // NETWORK,
38 // SONAR,
39 // MOTIVATION = 99
40 // } Test = TestCase::RUN;
41 
42 // Sonar *vehicleSonar;
43 // Vision *vehicleVision;
44 // Hoist *vehicleHoist;
45 
46 // Communication *comm;
47 // myJSONStr tmp_mess;
48 // HoistCtrl *hoistctrl;
49 // DriveCtrl *drivectrl;
50 // NavigationCtrl *navctrl;
51 // Drive *drive;
52 
53 // EnvironmentDetection *envdetect;
54 
55 VehicleCtrl *vehictrl;
56 
57 unsigned long currentMillis = 0;
58 unsigned long previousMillis = 0;
59 
60 void (*FuncFPtr)(void) = &loop;
61 void run();
62 void test_communication();
63 void test_hardware();
64 void test_ctrl();
65 
73 void setup() {
74 // Initialize serial and wait for port to open:
75 #ifdef DEBUGGER
76  Serial.begin(115200);
77  while (!Serial) {
78  ; // wait for serial port to connect. Needed for native USB port only
79  }
80 #endif
81 
82  DBFUNCCALLln("==setup()==");
83  DBSTATUSln("Vehicle: booting...");
84  // switch (Test) {
85  // case TestCase::HOIST:
86  // vehicleHoist = new Hoist(HOIST_SERVO_PIN, HOIST_SERVO_DELAY, HOIST_POISITION_MAX, HOIST_POSITION_MIN);
87  // break;
88  // case TestCase::HOISTCTRL:
89  // hoistctrl = new HoistCtrl();
90  // break;
91  // case TestCase::DRIVE:
92  // drive = new Drive(RIGHT_MOTOR, LEFT_MOTOR);
93  // // vehicleChassis = new Chassis(SPEED, K_P, K_I, K_D, RIGHT_MOTOR, LEFT_MOTOR, PIN_SENSOR_0, PIN_SENSOR_1, PIN_SENSOR_2, PIN_SENSOR_3, PIN_SENSOR_4);
94  // // state.chassis.speed = SPEED;
95  // break;
96  // case TestCase::DRIVECTRL:
97  // drivectrl = new DriveCtrl();
98  // break;
99  // case TestCase::NAVIGATIONCTRL:
100  // navctrl = new NavigationCtrl();
101  // break;
102  // case TestCase::ENVDETEC:
103  // envdetect = new EnvironmentDetection();
104  // break;
105  // case TestCase::VISION:
106  // // vehicleVision = new Vision(VISION_START_ANGLE, VISION_SERVO_PIN, VISION_DELAY_FACTOR, VISION_TOLERANCE_LEFT, VISION_TOLERANCE_RIGHT);
107  // break;
108  // case TestCase::NETWORK:
109  // // vehicleAPI = new VehicleWebAPI(&state.api);
110 
111  // comm = new Communication(DEFAULT_HOSTNAME);
112  // // comm->init();
113  // comm->printNetworkInfo();
114  // comm->subscribe("Sortic/Gateway");
115  // break;
116  // case TestCase::SONAR:
117  // // vehicleSonar = new Sonar(SONAR_SERVO_PIN, SONAR_TRIGGER_PIN, SONAR_ECHO_PIN, SONAR_MAX_DISTANCE, MIN_ERROR, MAX_ERROR, MIN_TURN_ANGLE, MAX_TURN_ANGLE);
118  // break;
119  // case TestCase::RUN:
120  // vehictrl = new VehicleCtrl();
121  // break;
122  // default:
123 
124  // break;
125  // }
126  vehictrl = new VehicleCtrl();
127  DBSTATUSln("Booting complete!");
128  // // you're connected now, so print out the data:
129  // Serial.print("You're connected to the network");
130 }
131 
141 void loop() {
142  vehictrl->loop();
143  // DBINFO1ln("==loop()==");
144  // currentMillis = millis();
145  // Serial.print("LoopTime [ms]: ");
146  // Serial.println(currentMillis - previousMillis);
147  // previousMillis = currentMillis;
148  // // Serial.print("RandNumber: ");
149  // // unsigned int randDelay = random(30) / 10.0 * 1000;
150  // // Serial.println(randDelay);
151  // switch (Test) {
152  // case TestCase::RUN:
153  // FuncFPtr = &run;
154  // break;
155  // case TestCase::HOIST:
156  // case TestCase::DRIVE:
157  // case TestCase::ENVDETEC:
158  // case TestCase::VISION:
159  // case TestCase::SONAR:
160  // FuncFPtr = &test_hardware;
161  // break;
162  // case TestCase::NAVIGATIONCTRL:
163  // case TestCase::HOISTCTRL:
164  // case TestCase::DRIVECTRL:
165  // FuncFPtr = &test_ctrl;
166  // break;
167  // case TestCase::NETWORK:
168  // FuncFPtr = &test_communication;
169  // break;
170  // default:
171  // break;
172  // }
173  // FuncFPtr();
174 }
175 
176 // void run() {
177 // vehictrl->loop();
178 // }
179 
180 // void test_communication() {
181 // previousMillis = millis();
182 // currentMillis = millis();
183 // while (((currentMillis - previousMillis) / 1000 < 10)) {
184 // currentMillis = millis();
185 // comm->loop();
186 // }
187 
188 // DBINFO1ln("=====PRINT LAST MESSAGE=====");
189 // DBINFO1ln(comm->size());
190 // if (comm->isEmpty()) {
191 // DBINFO1ln("Buff empty ");
192 // } else {
193 // DBINFO1ln("Buff NOT empty ");
194 // myJSONStr newMessage1;
195 // while (!comm->isEmpty()) {
196 // DBINFO1ln(comm->size());
197 // tmp_mess = comm->pop();
198 // DBINFO1("Topic: ");
199 // DBINFO1ln(tmp_mess.topic);
200 // DBINFO1("token: ");
201 // DBINFO1ln(tmp_mess.token);
202 // }
203 // }
204 
205 // delay(1000);
206 // }
207 
208 // void test_hardware() {
209 // switch (Test) {
210 // case TestCase::HOIST:
211 // DBSTATUSln("==Test HOIST==");
212 // // vehicleHoist->Hoist::Test(0);
213 // while (!vehicleHoist->raise()) {
214 // };
215 // delay(1000);
216 // while (!vehicleHoist->lower()) {
217 // };
218 // delay(1000);
219 // break;
220 // case TestCase::DRIVE:
221 // DBSTATUSln("==Test DRIVE==");
222 // drive->drive(Drive::Direction::Forward, 250);
223 // delay(1000);
224 // drive->stop();
225 // delay(1000);
226 // drive->drive(Drive::Direction::Backward, 250);
227 // delay(1000);
228 // drive->stop();
229 // delay(5000);
230 // // vehicleChassis->Chassis::Test();
231 // break;
232 // case TestCase::VISION:
233 // DBSTATUSln("==Test Vision==");
234 // // vehicleVision->Vision::Test(1);
235 // break;
236 // case TestCase::ENVDETEC:
237 // envdetect->Linedeviation();
238 // break;
239 // case TestCase::SONAR:
240 // DBSTATUSln("==Test Sonar==");
241 // /*
242 // * 0 - run all tests
243 // * 1 - run test for servo
244 // * 2 - run test for obstacle detection
245 // * */
246 // // vehicleSonar->Sonar::Test(1);
247 // break;
248 
249 // default:
250 // break;
251 // }
252 // }
253 
254 // void test_ctrl() {
255 // //https://www.arduino.cc/en/Tutorial/SwitchCase2
256 // int inByte;
257 // bool firstcall = true;
258 // switch (Test) {
259 // case TestCase::HOISTCTRL:
260 // DBSTATUSln("==Test Hoist CTRL==");
261 // //read Serial in and generate events
262 // Serial.println("Possible Events are:");
263 // Serial.println("R - Raise");
264 // Serial.println("L - Lower");
265 // Serial.println("E - Error");
266 // Serial.println("r - Resume");
267 // Serial.println("N - No Event");
268 // Serial.println("p - Position Reached");
269 // Serial.print("Choose Event: ");
270 // while (Serial.available() <= 0) {
271 // }
272 // inByte = Serial.read();
273 // Serial.print((char)inByte);
274 // Serial.println();
275 // switch (inByte) {
276 // case 'R':
277 // DBINFO1ln("Event: Raise");
278 // // while (hoistctrl->getcurrentState() != HoistCtrl::State::high)
279 // hoistctrl->loop(HoistCtrl::Event::Raise);
280 // break;
281 // case 'L':
282 // DBINFO1ln("Event: Lower");
283 // // while (hoistctrl->getcurrentState() != HoistCtrl::State::low)
284 // hoistctrl->loop(HoistCtrl::Event::Lower);
285 // break;
286 // case 'E':
287 // DBINFO1ln("Event: Error");
288 // hoistctrl->loop(HoistCtrl::Event::Error);
289 // break;
290 // case 'r':
291 // DBINFO1ln("Event: Resume");
292 // hoistctrl->loop(HoistCtrl::Event::Resume);
293 // break;
294 // case 'N':
295 // DBINFO1ln("Event: No Event");
296 // hoistctrl->loop(HoistCtrl::Event::NoEvent);
297 // break;
298 // case 'p':
299 // DBINFO1ln("Event: posPeached");
300 // hoistctrl->loop(HoistCtrl::Event::PosReached);
301 // break;
302 // default:
303 // DBINFO1ln("Error: Unknown value entered");
304 // break;
305 // }
306 // break;
307 // case TestCase::DRIVECTRL:
308 // DBSTATUSln("==Test Drive CTRL==");
309 // //read Serial in and generate events
310 // Serial.println("Possible Events are:");
311 // Serial.println("L - TurnLeft");
312 // Serial.println("R - TurnRight");
313 // Serial.println("A - TurnAround");
314 // Serial.println("F - FollowLineForward");
315 // Serial.println("B - FollowLineBackward");
316 // Serial.println("E - Error");
317 // Serial.println("r - Resume");
318 // Serial.println("N - No Event");
319 // Serial.println("l - LineAligned");
320 // Serial.println("f - FullLineDetected");
321 // Serial.print("Choose Event: ");
322 // while (Serial.available() <= 0) {
323 // }
324 // inByte = Serial.read();
325 // Serial.print((char)inByte);
326 // Serial.println();
327 // switch (inByte) {
328 // case 'L':
329 // DBINFO1ln("Event: TurnLeft");
330 // drivectrl->loop(DriveCtrl::Event::TurnLeft);
331 // while ((drivectrl->getcurrentState() == DriveCtrl::State::turningLeft) && (Serial.read() != 'E')) {
332 // drivectrl->loop();
333 // }
334 // break;
335 // case 'R':
336 // DBINFO1ln("Event: TurnRight");
337 // drivectrl->loop(DriveCtrl::Event::TurnRight);
338 // while ((drivectrl->getcurrentState() == DriveCtrl::State::turningRight) && (Serial.read() != 'E')) {
339 // drivectrl->loop();
340 // }
341 // break;
342 // case 'A':
343 // DBINFO1ln("Event: TurnAround");
344 // drivectrl->loop(DriveCtrl::Event::TurnAround);
345 // while ((drivectrl->getcurrentState() == DriveCtrl::State::turningAround) && (Serial.read() != 'E')) {
346 // drivectrl->loop();
347 // }
348 // break;
349 // case 'F':
350 // DBINFO1ln("Event: FollowLineForward");
351 // drivectrl->loop(DriveCtrl::Event::FollowLineForward);
352 // while ((drivectrl->getcurrentState() == DriveCtrl::State::followingLineForward) && (Serial.read() != 'E')) {
353 // drivectrl->loop();
354 // }
355 // break;
356 // case 'B':
357 // DBINFO1ln("Event: FollowLineBackward");
358 // drivectrl->loop(DriveCtrl::Event::FollowLineBackward);
359 // while ((drivectrl->getcurrentState() == DriveCtrl::State::followingLineBackward) && (Serial.read() != 'E')) {
360 // drivectrl->loop();
361 // }
362 // break;
363 // case 'E':
364 // DBINFO1ln("Event: Error");
365 // drivectrl->loop(DriveCtrl::Event::Error);
366 // break;
367 // case 'r':
368 // DBINFO1ln("Event: Resume");
369 // drivectrl->loop(DriveCtrl::Event::Resume);
370 // break;
371 // case 'N':
372 // DBINFO1ln("Event: No Event");
373 // drivectrl->loop(DriveCtrl::Event::NoEvent);
374 // break;
375 // case 'l':
376 // DBINFO1ln("Event: LineAligned");
377 // drivectrl->loop(DriveCtrl::Event::LineAligned);
378 // break;
379 // case 'f':
380 // DBINFO1ln("Event: FullLineDetected");
381 // drivectrl->loop(DriveCtrl::Event::FullLineDetected);
382 // break;
383 // case 'x':
384 // DBINFO1ln("AUTOSWITCH LANE");
385 // DBINFO1ln("Event: FollowLineBackward");
386 // drivectrl->loop(DriveCtrl::Event::FollowLineBackward);
387 // while ((drivectrl->getcurrentState() == DriveCtrl::State::followingLineBackward) && (Serial.read() != 'E')) {
388 // drivectrl->loop();
389 // }
390 // DBINFO1ln("Event: TurnAround");
391 // drivectrl->loop(DriveCtrl::Event::TurnAround);
392 // while ((drivectrl->getcurrentState() == DriveCtrl::State::turningAround) && (Serial.read() != 'E')) {
393 // drivectrl->loop();
394 // }
395 // DBINFO1ln("Event: FollowLineForward");
396 // drivectrl->loop(DriveCtrl::Event::FollowLineForward);
397 // while ((drivectrl->getcurrentState() == DriveCtrl::State::followingLineForward) && (Serial.read() != 'E')) {
398 // drivectrl->loop();
399 // }
400 // DBINFO1ln("Event: TurnRight");
401 // drivectrl->loop(DriveCtrl::Event::TurnRight);
402 // while ((drivectrl->getcurrentState() == DriveCtrl::State::turningRight) && (Serial.read() != 'E')) {
403 // drivectrl->loop();
404 // }
405 // DBINFO1ln("Event: FollowLineForward");
406 // drivectrl->loop(DriveCtrl::Event::FollowLineForward);
407 // while ((drivectrl->getcurrentState() == DriveCtrl::State::followingLineForward) && (Serial.read() != 'E')) {
408 // drivectrl->loop();
409 // }
410 // DBINFO1ln("Event: TurnRight");
411 // drivectrl->loop(DriveCtrl::Event::TurnRight);
412 // while ((drivectrl->getcurrentState() == DriveCtrl::State::turningRight) && (Serial.read() != 'E')) {
413 // drivectrl->loop();
414 // }
415 // DBINFO1ln("Event: FollowLineForward");
416 // drivectrl->loop(DriveCtrl::Event::FollowLineForward);
417 // while ((drivectrl->getcurrentState() == DriveCtrl::State::followingLineForward) && (Serial.read() != 'E')) {
418 // drivectrl->loop();
419 // }
420 // DBINFO1ln("Event: FollowLineForward");
421 // drivectrl->loop(DriveCtrl::Event::FollowLineForward);
422 // while ((drivectrl->getcurrentState() == DriveCtrl::State::followingLineForward) && (Serial.read() != 'E')) {
423 // drivectrl->loop();
424 // }
425 // break;
426 // default:
427 // DBINFO1ln("Error: Unknown value entered");
428 // break;
429 // }
430 // break;
431 // case TestCase::NAVIGATIONCTRL:
432 // DBSTATUSln("==Test Navigation CTRL==");
433 // //read Serial in and generate events
434 // Serial.println("Possible Events are:");
435 // Serial.println("S - MoveToTargetPosition Sortic 1");
436 // Serial.println("s - MoveToTargetPosition Sortic 3");
437 // Serial.println("T - MoveToTargetPosition Transfer 1");
438 // Serial.println("t - MoveToTargetPosition Transfer 3");
439 // Serial.println("E - Error");
440 // Serial.println("r - Resume");
441 // Serial.println("N - No Event");
442 // Serial.println("K - PosEndPointReached");
443 // Serial.println("k - PosTransitReached");
444 // Serial.println("p - Position Reached");
445 // Serial.print("Choose Event: ");
446 // while (Serial.available() <= 0) {
447 // }
448 // inByte = Serial.read();
449 // Serial.print((char)inByte);
450 // Serial.println();
451 // firstcall = true;
452 // switch (inByte) {
453 // case 'S':
454 // DBINFO1ln("Event: MoveToTargetPosition Sortic 1");
455 // navctrl->setTargetPosition(NavigationCtrl::Sector::SorticHandover, 1);
456 // navctrl->loop(NavigationCtrl::Event::MoveToTargetPosition);
457 // while ((navctrl->getcurrentState() != NavigationCtrl::State::endPoint) && (Serial.read() != 'E')) {
458 // navctrl->loop();
459 // // if (navctrl->getcurrentState() == NavigationCtrl::State::gateway && firstcall) {
460 // // firstcall = false;
461 // // delay(3000);
462 // // navctrl->giveToken();
463 // // }
464 // }
465 // break;
466 // case 's':
467 // DBINFO1ln("Event: MoveToTargetPosition Sortic 3");
468 // navctrl->setTargetPosition(NavigationCtrl::Sector::SorticHandover, 3);
469 // navctrl->loop(NavigationCtrl::Event::MoveToTargetPosition);
470 // while ((navctrl->getcurrentState() != NavigationCtrl::State::endPoint) && (Serial.read() != 'E')) {
471 // navctrl->loop();
472 // // if (navctrl->getcurrentState() == NavigationCtrl::State::gateway && firstcall) {
473 // // firstcall = false;
474 // // delay(3000);
475 // // navctrl->giveToken();
476 // // }
477 // }
478 // break;
479 // case 'T':
480 // DBINFO1ln("Event: MoveToTargetPosition Transfer 1");
481 // navctrl->setTargetPosition(NavigationCtrl::Sector::TransferHandover, 1);
482 // navctrl->loop(NavigationCtrl::Event::MoveToTargetPosition);
483 // while ((navctrl->getcurrentState() != NavigationCtrl::State::endPoint) && (Serial.read() != 'E')) {
484 // navctrl->loop();
485 // // if (navctrl->getcurrentState() == NavigationCtrl::State::gateway && firstcall) {
486 // // firstcall = false;
487 // // delay(3000);
488 // // navctrl->giveToken();
489 // // }
490 // }
491 // break;
492 // case 't':
493 // DBINFO1ln("Event: MoveToTargetPosition Transfer 3");
494 // navctrl->setTargetPosition(NavigationCtrl::Sector::TransferHandover, 3);
495 // navctrl->loop(NavigationCtrl::Event::MoveToTargetPosition);
496 // while ((navctrl->getcurrentState() != NavigationCtrl::State::endPoint) && (Serial.read() != 'E')) {
497 // navctrl->loop();
498 // // if (navctrl->getcurrentState() == NavigationCtrl::State::gateway && firstcall) {
499 // // firstcall = false;
500 // // delay(3000);
501 // // navctrl->giveToken();
502 // // }
503 // }
504 // break;
505 // case 'E':
506 // DBINFO1ln("Event: Error");
507 // navctrl->loop(NavigationCtrl::Event::Error);
508 // break;
509 // case 'r':
510 // DBINFO1ln("Event: Resume");
511 // navctrl->loop(NavigationCtrl::Event::Resume);
512 // break;
513 // case 'N':
514 // DBINFO1ln("Event: No Event");
515 // navctrl->loop(NavigationCtrl::Event::NoEvent);
516 // break;
517 // case 'K':
518 // DBINFO1ln("Event: PosEndPointReached");
519 // navctrl->loop(NavigationCtrl::Event::PosEndPointReached);
520 // break;
521 // case 'k':
522 // DBINFO1ln("Event: PosTransitReached");
523 // navctrl->loop(NavigationCtrl::Event::PosTransitReached);
524 // break;
525 // case 'p':
526 // DBINFO1ln("Event: posPeached");
527 // navctrl->loop(NavigationCtrl::Event::PosReached);
528 // break;
529 // default:
530 // DBINFO1ln("Error: Unknown value entered");
531 // break;
532 // }
533 // break;
534 // default:
535 // break;
536 // }
537 // }
vehictrl
VehicleCtrl * vehictrl
Definition: main.cpp:55
currentMillis
unsigned long currentMillis
will store current time
Definition: main.cpp:57
loop
void loop()
After creating a setup() function, which initializes and sets the initial values, the loop() function...
Definition: main.cpp:141
setup
void setup()
For initialisation of the Board.
Definition: main.cpp:73
test_hardware
void test_hardware()
VehicleCtrl
The Vehicle Controll class contains the FSM for the complete Vehicle.
Definition: VehicleCtrl.h:34
test_ctrl
void test_ctrl()
VehicleCtrl.h
The Vehicle Controll class contains the FSM for the Vehicle.
run
void run()
test_communication
void test_communication()
FuncFPtr
void(* FuncFPtr)(void)
Definition: main.cpp:60
LogConfiguration.h
Contains Pre-Compiler directives for diffent Serialprints for Datalogin.
DBFUNCCALLln
#define DBFUNCCALLln(x)
Definition: LogConfiguration.h:125
VehicleCtrl::loop
void loop()
Calls the do-function of the active state and hence generates Events.
Definition: VehicleCtrl.cpp:31
DBSTATUSln
#define DBSTATUSln(x)
Definition: LogConfiguration.h:53
previousMillis
unsigned long previousMillis
will store last time
Definition: main.cpp:58