Car controlled over Bluetooth with crash detection

1#include <Adafruit_MMC56x3.h>
2
3#include "Statemachine.hpp"
4
5#include "ApplicationConstants.hpp"
6#include "HardwareConfiguration.hpp"
7
8#include "IRSensor.hpp"
9#include "Motor.hpp"
10#include "MessageBuilder.hpp"
11#include "Control.hpp"
12#include "Compass.hpp"
13#include "UltrasoundSensor.hpp"
14#include "ArduinoApp.hpp"
15
16const unsigned long delay_ms{250};
17const unsigned long measurePeriod_ms{500};
18
19Statemachine stateMachine;
20Compass compass;
21IRSensor irsensorRight, irsensorLeft;
22
23LogarithmicModel modelMotorLeft{0.929871, 1.8471, -44.8247, -3.34696};
24LogarithmicModel modelMotorRight{0.598208, 4.90363, -224.587, -1.80617};
25
26Motor motorLeft{HardwareSetup::LeftBrakePin, HardwareSetup::LeftDirectionPin, HardwareSetup::LeftSpeedPin, modelMotorLeft, 50, 200};
27Motor motorRight{HardwareSetup::RightBrakePin, HardwareSetup::RightDirectionPin, HardwareSetup::RightSpeedPin, modelMotorRight, 70, 200};
28UltrasoundSensor ultrasoundSensor{HardwareSetup::UltrasoundTriggerPin, HardwareSetup::UltrasoundEchoPin};
29Control control{stateMachine, compass, motorRight, motorLeft, irsensorRight, irsensorLeft, ultrasoundSensor, measurePeriod_ms};
30
31ArduinoApp theApp{stateMachine, motorLeft, motorRight, control, measurePeriod_ms};
32
33/* Assign a unique ID to this sensor at the same time */
34Adafruit_MMC5603 mmc = Adafruit_MMC5603(12345);
35
36void setup() {
37 Serial.begin(9600, SERIAL_8N1);
38 compass.setUp();
39 setupIRSwitches();
40 theApp.setup();
41 Serial.println("SW ready");
42}
43
44void loop() {
45 theApp.loop();
46 delay(ApplicationConstants::loopPeriod_ms);  
47}
48
49void setupIRSwitches(){
50 pinMode(HardwareSetup::IRSwitchRight, INPUT_PULLUP);    // pin D2
51 pinMode(HardwareSetup::IRSwitchLeft, INPUT_PULLUP);     // pin D10
52
53 uint8_t interrupt0 = digitalPinToInterrupt(HardwareSetup::IRSwitchRight);
54 attachInterrupt(interrupt0, rightWheel, FALLING);
55
56  #ifndef USING_QT
57 // pin change interrupt (example for D10)
58 PCMSK0 |= bit (PCINT2); // want pin 10
59 PCIFR |= bit (PCIF0); // clear any outstanding interrupts
60 PCICR |= bit (PCIE0); // enable pin change interrupts for D8 to D13
61 #else
62  interrupt0 = digitalPinToInterrupt(HardwareSetup::IRSwitchLeft);
63  attachInterrupt(interrupt0, leftWheel, FALLING);
64 #endif
65}
66
67ISR (PCINT0_vect){
68 irsensorLeft.registerCross(); //leftCount++;
69// handle pin change interrupt for D8 to D13 here
70} // end of PCINT0_vect
71
72void rightWheel(){
73 irsensorRight.registerCross();
74}
75
76//Phone application will provide with course and speed.

1#ifndef _ARDUINOAPP_HPP_
2#define _ARDUINOAPP_HPP_
3
4#include "Compass.hpp"
5#include "Control.hpp"
6#include "MessageBuilder.hpp"
7#include "Motor.hpp"
8#include "Statemachine.hpp"
9
10class IArduinoApp {
11public:
12    virtual void setup() = 0;
13    virtual void loop() = 0;
14};
15
16class ArduinoApp : public IArduinoApp {
17  
18public:
19    ArduinoApp(IStatemachine& stateMachine, IMotor& motorRight, IMotor& motorLeft,
20               IControl& control, unsigned long period_ms);
21
22    void setup() override;
23    void loop() override;
24
25 private:
26  bool interpretFrame();
27  void setupIRSwitches();
28  void sendReply(char* reply);
29
30  IStatemachine& mStateMachine;
31  IMotor& mMotorRight;
32  IMotor& mMotorLeft;
33  MessageBuilder mMessageBuilder;
34  IControl& mControl;
35  Compass mCompass;
36  unsigned long mPrevTime;
37  SystemInputs mSystemInputs;
38  Measures mCurrentMeasures;
39  CruiseControl mBehaviour;
40};
41
42#endif

1#ifdef USING_QT
2#include <QDebug>
3#endif
4
5#include "ArduinoApp.hpp"
6#include "Arduino.h"
7
8#include "HardwareConfiguration.hpp"
9
10ArduinoApp::ArduinoApp(IStatemachine& stateMachine, IMotor& motorRight, IMotor& motorLeft, IControl& control, unsigned long period_ms)
11: mStateMachine{stateMachine}
12, mMotorRight{motorRight}
13, mMotorLeft{motorLeft}
14, mControl{control}
15, mSystemInputs{false, 0.0, 0.0}
16, mCurrentMeasures{0.0, 0.0, 0.0, 100}
17{
18}
19
20void ArduinoApp::setup() {
21 mPrevTime = millis();
22 mSystemInputs.heading_grad = mCurrentMeasures.currentHeading_grad;
23 #ifndef USING_QT
24 Serial.print("Arduino::setup. System inputs: heading= ");
25 Serial.print(mSystemInputs.heading_grad);
26 Serial.print(" linear speed=  ");
27 Serial.println(mSystemInputs.velocity_m_per_s);
28 #endif
29}
30
31void ArduinoApp::loop() {
32 if (Serial.available() > 0 ) {
33  bool validFrame{interpretFrame()};
34  if (validFrame && mStateMachine.state() == RobotState::Running){
35   //calculateOutputs(mSystemInputs);
36  }
37  mMessageBuilder.clear();
38 }
39 mControl.processState(mBehaviour, mCurrentMeasures, mSystemInputs);
40 if (mStateMachine.state() == RobotState::Running) {
41  mControl.evaluateSystemState(mCurrentMeasures, millis());
42  ControlActions controlActions{mBehaviour.calculateControlActions(mSystemInputs, mCurrentMeasures)};
43  mControl.applyControlActions(controlActions);
44  mPrevTime = millis();
45 }
46}
47
48bool ArduinoApp::interpretFrame(){
49 String frame = Serial.readString();
50 for (char car : frame){
51  bool result = mMessageBuilder.push_back(car);
52  if (result == false)
53   return false;
54  if (mMessageBuilder.isFrameComplete() && mMessageBuilder.isWellFormedFrame()) {
55   char reply[14];
56   ParsedFrame parsedFrame = mMessageBuilder.parseMessage();
57   mControl.processFrame(parsedFrame, mCurrentMeasures, mSystemInputs, reply);
58   sendReply(reply);
59   return true;
60  }
61 }
62 return false;
63}
64
65void ArduinoApp::sendReply(char* reply) {
66 if (reply[0] == 0) return;
67 int idx{0};
68 do {
69  Serial.print(reply[idx]);
70  reply[idx] = 0;
71  idx++;
72 } while (reply[idx] != '>');
73 Serial.println(">");
74 Serial.flush();
75}