diff --git a/.gitignore b/.gitignore index 2e0589b..4c2cd1d 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,11 @@ +# KDE .directory +# Personal files TODO.txt +# VSCode configuration files +.vscode/ + +# Compiled files build/ \ No newline at end of file diff --git a/README.md b/README.md index 3bbeb00..1bed57e 100644 --- a/README.md +++ b/README.md @@ -1,14 +1,21 @@ # arduino-robot-car -Repository for a 4-wheel robot car based on Arduino for the "Elegoo Smart Robot Car Kit V3.0 Plus" and similar ones. +Source code to control a 4-wheel robot car based on Arduino Uno. The code has been tested for the "Elegoo Smart Robot Car Kit V3.0 Plus", but can be easily adatped for other similar ones. -## Requirements +The robot consists of a 4 DC motors driven by a H-Bridge with dual output, connecting the two left wheels and the two right ones to its outputs. The car is remotely controlled either by Bluetooth, through the Elegoo Tool app, or by infrared. An ultrasonic sensor attached to a servo motor measures the front distance to objects. A line tracking sensor on the base, with 3 pairs of LED + photoresistors, allows to follow a line drawn on the floor. +Some functionalities have been added in the software to the official Elegoo release. +## Requirements +Apart from the standard Arduino libraries, some other ones must be installed: +- ArduinoJson: https://arduinojson.org +- IRRemote: https://github.com/z3t0/Arduino-IRremote +- Servo: https://www.arduino.cc/reference/en/libraries/servo/ ## Installation - +1. Install the required libraries. +2. Download the repository and flash the Arduino. For VSCode, modify the includePath to use the downloaded libraries; if using the Arduino IDE, change the name of the src folder to something appropriate. ## Usage - +The oficial Elegoo Tool application for Android must be downloaded to interact with the robot. Nevertheless, changing the initial robot mode will allow you to use it without the app. ## Contributing Pull requests are welcome. For major changes, please open an issue first to discuss what you would like to change. diff --git a/src/IRreceiver.cpp b/src/IRreceiver.cpp new file mode 100644 index 0000000..af55374 --- /dev/null +++ b/src/IRreceiver.cpp @@ -0,0 +1,61 @@ +/** + * @file IRreceiver.cpp + * @author José Ángel Sánchez (https://github.com/gelanchez) + * @brief Library for receiving data from the IR sensor. + * @version 1.0.0 + * @date 2020-08-22 + * @copyright GPL-3.0 + */ + +#include "constants.h" +#include "IRreceiver.h" + +IRreceiver::IRreceiver(): m_irrecv{ Constants::IRPin }, m_isEnabled{ false }, +m_previousKey{ 0xFFFFFFFF } // Member initializer lists; it can't go inside the constructor +{ +} + +IRreceiver::~IRreceiver() +{ +} + +void IRreceiver::enable() +{ + if (!m_isEnabled) + { + m_irrecv.enableIRIn(); + m_irrecv.blink13(false); // Disable blinking the LED 13 during reception + m_isEnabled = true; + } +} + +RemoteOrder IRreceiver::decodeIR() +{ + if (m_irrecv.decode(&m_results)) + { + unsigned long pressedKey = m_results.value; + m_irrecv.resume(); + + if (pressedKey == 0xFFFFFFFF) // Repeat previous key + pressedKey = m_previousKey; + else + m_previousKey = pressedKey; // Update previous key to current key + + switch (pressedKey) + { + case Constants::okKey: + return RemoteOrder::STOP; + case Constants::upKey: + return RemoteOrder::FORWARD; + case Constants::downKey: + return RemoteOrder::BACKWARD; + case Constants::leftKey: + return RemoteOrder::ROTATELEFT; + case Constants::rightKey: + return RemoteOrder::ROTATERIGHT; + default: + return RemoteOrder::UNKNOWN; + } + } + return RemoteOrder::UNKNOWN; +} \ No newline at end of file diff --git a/src/IRreceiver.h b/src/IRreceiver.h new file mode 100644 index 0000000..4756dff --- /dev/null +++ b/src/IRreceiver.h @@ -0,0 +1,37 @@ +/** + * @file IRreceiver.h + * @author José Ángel Sánchez (https://github.com/gelanchez) + * @brief Library for receiving data from the IR sensor. + * @version 1.0.0 + * @date 2020-08-22 + * @copyright GPL-3.0 + */ + +#ifndef IRRECEIVER_H +#define IRRECEIVER_H + +// To avoid using old versions of Arduino IDE, which used WPProgram.h instead or Arduino.h +// Also possible #define IRPRONTO +#ifndef ARDUINO + #define ARDUINO 108012 // Arduino IDE version used when writing the program +#endif + +#include "constants.h" +#include + +class IRreceiver +{ + private: + IRrecv m_irrecv; // Receiver object + bool m_isEnabled; + decode_results m_results; // Struct to store the received information + unsigned long m_previousKey; // Store previous order for repeating key + + public: + IRreceiver(); + ~IRreceiver(); + void enable(); + RemoteOrder decodeIR(); +}; + +#endif \ No newline at end of file diff --git a/src/constants.cpp b/src/constants.cpp new file mode 100644 index 0000000..cb938e7 --- /dev/null +++ b/src/constants.cpp @@ -0,0 +1,11 @@ +/** + * @file constants.cpp + * @author José Ángel Sánchez (https://github.com/gelanchez) + * @brief Constants used along the program. + * This file is only used because if not, program doesn't compile (it can't find constants.h) + * @version 1.0.0 + * @date 2020-08-22 + * @copyright GPL-3.0 + */ + +#include "constants.h" // A code file should #include its paired header file (if it exists) \ No newline at end of file diff --git a/src/constants.h b/src/constants.h new file mode 100644 index 0000000..48481c6 --- /dev/null +++ b/src/constants.h @@ -0,0 +1,119 @@ +/** + * @file constants.h + * @author José Ángel Sánchez (https://github.com/gelanchez) + * @brief Constants used along the program + * @version 1.0.0 + * @date 2020-08-22 + * @copyright GPL-3.0 + */ + +#ifndef CONSTANTS_H +#define CONSTANTS_H + +#include + +namespace Constants +{ + // MOTORS + // Right motors pins + const uint8_t motorsEnA(6); + const uint8_t motorsIn1(11); + const uint8_t motorsIn2(9); + // Left motors pins + const uint8_t motorsEnB(5); + const uint8_t motorsIn3(8); + const uint8_t motorsIn4(7); + // Motors min speed (measured) + const uint8_t crankSpeed(140); // Around 120 @ full battery + const uint8_t idleSpeed(90); + + // DEFAULT speeds + const uint8_t moveSpeed(170); + const uint8_t rotateSpeed(150); + const uint16_t rotate90Time(650); // Time to rotate 90deg @ moveSpeed @ full battery + const uint16_t rotate180Time(1200); // Time to rotate 180deg @ rotateSpeed @ full battery + + // OBSTACLEAVOIDANCE constants + const uint16_t updateInterval(250); // Default update time for states + const uint16_t minDistance(30); + + // LINETRACKING constants + const uint8_t ltLeftPin(2); + const uint8_t ltMidPin(4); + const uint8_t ltRightPin(10); + const uint16_t minDetourDistance(10); // Distance to keep with the obstacle + const uint16_t updateUltrasonicInterval(20); + const uint16_t extraTime(100); // Extra time to keep moving the robot to pass the object + const uint16_t extraTimeLine(50); // Extra time to pass the line + const uint16_t timeUntilLost(1000); // Wait time until LOSTLINE and turn back + const uint16_t timeLost(5000); // Time lost to find a new line + const uint8_t marginObject(1); // Margin +- distance to the object + + // PARK mode constants + const uint16_t timeMoving(500); + const uint16_t timeMoveAway(150); + + // SERVO + // Servo pin + const uint8_t servoPin(3); + // 0deg and 180deg PWM positions + const uint16_t servo0(500); // Calibration 450, default 544 + const uint16_t servo180(2400); // Calibration 2430, default 2400 + + // ULTRASONIC SENSOR + const uint8_t trigPin(A5); // Pin 19 + const uint8_t echoPin(A4); // Pin 18 + const float soundSpeed(0.0343); // cm/s + const uint16_t maxDistance(250); // Maximun distance to meassure in cm + const uint16_t maxDistanceLineTracking(100); // Maximun distance to meassure in cm to be used in the linetraking mode + + // IRCONTROL constants + const uint8_t IRPin(12); + const unsigned long okKey(0xFF02FD); + const unsigned long upKey(0xFF629D); + const unsigned long downKey(0xFFA857); + const unsigned long leftKey(0xFF22DD); + const unsigned long rightKey(0xFFC23D); + const uint16_t IRMovingInterval(500); // Default time for moving in IR +} + +/** + * @brief Different modes for the robot + */ +enum class RobotMode +{ + REMOTECONTROL, + IRCONTROL, + OBSTACLEAVOIDANCE, + LINETRACKING, + PARK, + CUSTOM, +}; + +/** + * @brief Orders for the RemoteControl and IRControl modes + */ +enum class RemoteOrder +{ + STOP, + FORWARD, + BACKWARD, + ROTATELEFT, + ROTATERIGHT, + UNKNOWN, +}; + +/** + * @brief States for the different Robotmode + */ +enum class RobotModeState +{ + START, + FORWARD, + OBSTACLE, + ROTATE, + BLOCKED, + LINELOST, // Only used in linetracking +}; + +#endif \ No newline at end of file diff --git a/src/linetracking.cpp b/src/linetracking.cpp new file mode 100644 index 0000000..f328148 --- /dev/null +++ b/src/linetracking.cpp @@ -0,0 +1,57 @@ +/** + * @file linetracking.cpp + * @author José Ángel Sánchez (https://github.com/gelanchez) + * @brief Library to handle the linketracking sensors. + * @version 1.0.0 + * @date 2020-08-22 + * @copyright GPL-3.0 + */ + +#include "constants.h" +#include "linetracking.h" + +LineTracking::LineTracking() +{ + pinMode(Constants::ltLeftPin, INPUT); + pinMode(Constants::ltMidPin, INPUT); + pinMode(Constants::ltRightPin, INPUT); +} + +LineTracking::~LineTracking() +{ +} + +bool LineTracking::leftLine() +{ + return !digitalRead(Constants::ltLeftPin); +} + +bool LineTracking::midLine() +{ + return !digitalRead(Constants::ltMidPin); +} + +bool LineTracking::rightLine() +{ + return !digitalRead(Constants::ltRightPin); +} + +bool LineTracking::anyLine() +{ + return (leftLine() || midLine() || rightLine()); +} + +bool LineTracking::allLines() +{ + return (leftLine() && midLine() && rightLine()); +} + +void LineTracking::printLines() +{ + Serial.print(leftLine()); + Serial.print(" "); + Serial.print(midLine()); + Serial.print(" "); + Serial.print(rightLine()); + Serial.println(); +} \ No newline at end of file diff --git a/src/linetracking.h b/src/linetracking.h new file mode 100644 index 0000000..bf126a9 --- /dev/null +++ b/src/linetracking.h @@ -0,0 +1,29 @@ +/** + * @file linetracking.h + * @author José Ángel Sánchez (https://github.com/gelanchez) + * @brief Library to handle the linketracking sensors. + * @version 1.0.0 + * @date 2020-08-22 + * @copyright GPL-3.0 + */ + +#ifndef LINETRACKING_H +#define LINETRACKING_H + +class LineTracking +{ + private: + + public: + LineTracking(); + ~LineTracking(); + bool leftLine(); + bool midLine(); + bool rightLine(); + bool anyLine(); + bool allLines(); + void printLines(); +}; + + +#endif \ No newline at end of file diff --git a/src/main.ino b/src/main.ino new file mode 100644 index 0000000..c6897c0 --- /dev/null +++ b/src/main.ino @@ -0,0 +1,161 @@ +/** + * @file main.ino + * @author José Ángel Sánchez (https://github.com/gelanchez) + * @brief Main program. + * @version 1.0.0 + * @date 2020-08-22 + * @copyright GPL-3.0 + */ + +#include "constants.h" +#include "robot.h" +#include + +RobotControl Robot = RobotControl(); // Initialization of the RobotControl object +String dataBT = ""; + +/** + * @brief Structure storing the JSON data received, which keeps the mode of the robot. + */ +struct Data +{ + RobotMode mode = RobotMode::REMOTECONTROL; // Default mode + //RobotMode mode = RobotMode::CUSTOM; // Default mode + uint8_t parameter1 = 0; + uint8_t paremter2 = 0; + uint8_t speed = 0; + uint8_t time = 0; +}; + +Data data; // Creation of the struct data +StaticJsonDocument<150> elegooDoc; + +/** + * @brief Decode Elegoo JSON object, setting the struct data. + * Modes in bluetooth (Elegoo specifications): + * {"N":5} square down bluetoothFollowing + * {"N":3,"D1":1} line tracking + * {"N":3,"D1":2} obstacle avoidance + * {"N":2,"D1":1...5} joystick + */ +void decodeElegooJSON() +{ + DeserializationError error = deserializeJson(elegooDoc, dataBT); + if (!error) + { + uint8_t N = elegooDoc["N"]; + uint8_t D1; + switch (N) // See Elegoo specifications + { + case 2: // remoteControlMode + D1 = elegooDoc["D1"]; + switch (D1) + { + case 1: + data.parameter1 = static_cast(RemoteOrder::ROTATELEFT); + return; + case 2: + data.parameter1 = static_cast(RemoteOrder::ROTATERIGHT); + return; + case 3: + data.parameter1 = static_cast(RemoteOrder::FORWARD); + return; + case 4: + data.parameter1 = static_cast(RemoteOrder::BACKWARD); + return; + case 5: + data.parameter1 = static_cast(RemoteOrder::STOP); + return; + default: + data.parameter1 = static_cast(RemoteOrder::UNKNOWN); + return; + } + case 3: // lineTrackingMode or obstacleAvoidanceMode + D1 = elegooDoc["D1"]; + switch (D1) + { + case 1: + if (data.mode != RobotMode::LINETRACKING) + { + Robot.restartState(); + data.mode = RobotMode::LINETRACKING; + } + return; + case 2: + if (data.mode != RobotMode::OBSTACLEAVOIDANCE) + { + Robot.restartState(); + data.mode = RobotMode::OBSTACLEAVOIDANCE; + } + return; + default: + return; + } + case 5: // IRControlMode mode activation or deactivation (deactivate other modes) + if (data.mode == RobotMode::REMOTECONTROL) + { + Robot.restartState(); + data.mode = RobotMode::IRCONTROL; + Robot.m_IRreceiver.enable(); + return; + } + else + { + Robot.restartState(); + data.mode = RobotMode::REMOTECONTROL; + return; + } + case 100: // parkMode activation + data.mode = RobotMode::PARK; + return; + default: + data.mode = RobotMode::REMOTECONTROL; + return; + } + } +} + +void setup() +{ + Robot.begin(); + delay(500); // To make Serial work +} + +void loop() +{ + dataBT = Robot.getBTData(); + if (dataBT != "") + decodeElegooJSON(); + + switch (data.mode) + { + case RobotMode::REMOTECONTROL: + Robot.remoteControlMode(static_cast(data.parameter1)); + break; + + case RobotMode::IRCONTROL: + Robot.IRControlMode(); + break; + + case RobotMode::OBSTACLEAVOIDANCE: + Robot.obstacleAvoidanceMode(); + break; + + case RobotMode::LINETRACKING: + Robot.lineTrackingMode(); + break; + + case RobotMode::PARK: + Robot.parkMode(); + data.mode = RobotMode::REMOTECONTROL; + Robot.restartState(); + break; + + case RobotMode::CUSTOM: + Robot.customMode(); + break; + + default: + break; + } +} diff --git a/src/motors.cpp b/src/motors.cpp new file mode 100644 index 0000000..f24e241 --- /dev/null +++ b/src/motors.cpp @@ -0,0 +1,146 @@ +/** + * @file motors.cpp + * @author José Ángel Sánchez (https://github.com/gelanchez) + * @brief Library for dealing with motors. + * @version 1.0.0 + * @date 2020-08-22 + * @copyright GPL-3.0 + */ + +#include "constants.h" +#include "motors.h" + +Motors::Motors() : m_leftSpeed(0), m_rightSpeed(0) // Member initializer lists +{ + // Pins used for motors + pinMode(Constants::motorsEnA, OUTPUT); + pinMode(Constants::motorsIn1, OUTPUT); + pinMode(Constants::motorsIn2, OUTPUT); + pinMode(Constants::motorsEnB, OUTPUT); + pinMode(Constants::motorsIn3, OUTPUT); + pinMode(Constants::motorsIn4, OUTPUT); + + off(); +} + +Motors::~Motors() +{ + off(); +} + +int16_t Motors::getLeftSpeed() +{ + return m_leftSpeed; +} + +int16_t Motors::getRightSpeed() +{ + return m_rightSpeed; +} + +void Motors::off() +{ + stop(); + digitalWrite(Constants::motorsIn1, LOW); + digitalWrite(Constants::motorsIn2, LOW); + digitalWrite(Constants::motorsIn3, LOW); + digitalWrite(Constants::motorsIn4, LOW); +} + +void Motors::stop() +{ + digitalWrite(Constants::motorsEnA, LOW); + digitalWrite(Constants::motorsEnB, LOW); + m_leftSpeed = 0; + m_rightSpeed = 0; +} + +void Motors::move(int16_t leftSpeed, int16_t rightSpeed) +{ + // Return if no speed change + if ((m_leftSpeed == leftSpeed) && (m_rightSpeed == rightSpeed)) + return; + + // Limit values + leftSpeed = constrain(leftSpeed, -255, 255); + rightSpeed = constrain(rightSpeed, -255, 255); + + // Prevent buzzing at low speeds + uint8_t minLeftSpeed = (m_leftSpeed == 0) ? Constants::crankSpeed : Constants::idleSpeed; + uint8_t minRightSpeed = (m_rightSpeed == 0) ? Constants::crankSpeed : Constants::idleSpeed; + + // Member variables update + m_leftSpeed = (abs(leftSpeed) < minLeftSpeed) ? 0 : leftSpeed; + m_rightSpeed = (abs(rightSpeed) < minRightSpeed) ? 0 : rightSpeed; + + if (m_leftSpeed<0) + { + digitalWrite(Constants::motorsIn3, HIGH); + digitalWrite(Constants::motorsIn4, LOW); + analogWrite(Constants::motorsEnB, abs(m_leftSpeed)); + } + else if (m_leftSpeed==0) + analogWrite(Constants::motorsEnB, 0); + else + { + digitalWrite(Constants::motorsIn3, LOW); + digitalWrite(Constants::motorsIn4, HIGH); + analogWrite(Constants::motorsEnB, m_leftSpeed); + } + + if (m_rightSpeed<0) + { + digitalWrite(Constants::motorsIn1, LOW); + digitalWrite(Constants::motorsIn2, HIGH); + analogWrite(Constants::motorsEnA, abs(m_rightSpeed)); + } + else if (m_rightSpeed==0) + analogWrite(Constants::motorsEnA, 0); + else + { + digitalWrite(Constants::motorsIn1, HIGH); + digitalWrite(Constants::motorsIn2, LOW); + analogWrite(Constants::motorsEnA, m_rightSpeed); + } +} + +void Motors::forward(uint8_t speed) +{ + move(speed, speed); +} + +void Motors::backward(uint8_t speed) +{ + move(-speed, -speed); +} + +void Motors::rotateLeft(uint8_t speed) +{ + move(-speed, speed); +} + +void Motors::rotateRight(uint8_t speed) +{ + move(speed, -speed); +} + +bool Motors::isStopped() +{ + if ((m_leftSpeed == 0) && (m_rightSpeed == 0)) + return true; + return false; +} + +bool Motors::isRotatingLeft() +{ + if ((m_leftSpeed < 0) && (m_rightSpeed > 0)) + return true; + return false; +} + +bool Motors::isRotatingRight() +{ + if ((m_leftSpeed > 0) && (m_rightSpeed < 0)) + return true; + return false; +} \ No newline at end of file diff --git a/src/motors.h b/src/motors.h new file mode 100644 index 0000000..752b656 --- /dev/null +++ b/src/motors.h @@ -0,0 +1,38 @@ +/** + * @file motors.h + * @author José Ángel Sánchez (https://github.com/gelanchez) + * @brief Library for dealing with motors. + * @version 1.0.0 + * @date 2020-08-22 + * @copyright GPL-3.0 + */ + +#ifndef MOTORS_H +#define MOTORS_H + +#include "constants.h" +#include + +class Motors +{ + private: + int16_t m_leftSpeed, m_rightSpeed; + + public: + Motors(); + ~Motors(); + int16_t getLeftSpeed(); + int16_t getRightSpeed(); + void off(); + void stop(); + void move(int16_t leftSpeed, int16_t rightSpeed); + void forward(uint8_t speed = Constants::moveSpeed); + void backward(uint8_t speed = Constants::moveSpeed); + void rotateLeft(uint8_t speed = Constants::rotateSpeed); + void rotateRight(uint8_t speed = Constants::rotateSpeed); + bool isStopped(); + bool isRotatingLeft(); + bool isRotatingRight(); +}; + +#endif \ No newline at end of file diff --git a/src/myservo.cpp b/src/myservo.cpp new file mode 100644 index 0000000..2c1dbb0 --- /dev/null +++ b/src/myservo.cpp @@ -0,0 +1,29 @@ +/** + * @file myservo.cpp + * @author José Ángel Sánchez (https://github.com/gelanchez) + * @brief Library that inherits from Servo.h. + * @version 1.0.0 + * @date 2020-08-22 + * @copyright GPL-3.0 + */ + +#include "constants.h" +#include "myservo.h" +#include + +MyServo::MyServo(): Servo() +{ +} + +MyServo::~MyServo() +{ +} + +/** + * @brief Servo initialization can not be done inside Robot constructor. + */ +void MyServo::begin() +{ + Servo::attach(Constants::servoPin, Constants::servo0, Constants::servo180); + Servo::write(90); +} \ No newline at end of file diff --git a/src/myservo.h b/src/myservo.h new file mode 100644 index 0000000..f44f6cf --- /dev/null +++ b/src/myservo.h @@ -0,0 +1,25 @@ +/** + * @file myservo.h + * @author José Ángel Sánchez (https://github.com/gelanchez) + * @brief Library that inherits from Servo.h. + * @version 1.0.0 + * @date 2020-08-22 + * @copyright GPL-3.0 + */ + +#ifndef MYSERVO_H +#define MYSERVO_H + +#include + +class MyServo: public Servo +{ + private: + + public: + MyServo(); + ~MyServo(); + void begin(); +}; + +#endif \ No newline at end of file diff --git a/src/robot.cpp b/src/robot.cpp new file mode 100644 index 0000000..0b7f45c --- /dev/null +++ b/src/robot.cpp @@ -0,0 +1,432 @@ +/** + * @file robot.cpp + * @author José Ángel Sánchez (https://github.com/gelanchez) + * @brief Library for controling the robot. + * @version 1.0.0 + * @date 2020-08-22 + * @copyright GPL-3.0 + */ + +#include "constants.h" +#include "IRreceiver.h" +#include "linetracking.h" +#include "motors.h" +#include "myservo.h" +#include "robot.h" +#include "ultrasonic.h" +#include + +RobotControl::RobotControl() // Member initializer lists +: m_motors{}, m_servo{}, m_ultrasonic{}, m_IRreceiver{}, m_lineTracking{}, m_state{ RobotModeState::START }, +m_previousAngle{ 90 }, m_interval{ Constants::updateInterval }, +m_sonarMap{ Constants::maxDistance, Constants::maxDistance, Constants::maxDistance, Constants::maxDistance, Constants::maxDistance } +{ + m_lastUpdate = millis(); + // m_motors = Motors(); + // m_servo = MyServo(); // do NOT attach the pin here but in begin + // m_ultrasonic = Ultrasonic(); + // m_lineTracking = LineTraking(); + // m_sonarMap{Constants::maxDistance, Constants::maxDistance, Constants::maxDistance, Constants::maxDistance, Constants::maxDistance}; + // m_state = RobotModeState::START; // Initial state of the RobotMode + // m_previousAngle = 90; // Previous angle of the servo, initial value + // m_interval = Constants::defaultInterval; +} + +RobotControl::~RobotControl() +{ + m_servo.detach(); +} + +void RobotControl::restartState() +{ + if (m_servo.read() != 90) + m_servo.write(90); + m_lastUpdate = millis(); + if (!m_motors.isStopped()) + m_motors.stop(); + m_state = RobotModeState::START; + m_previousAngle = 90; + m_interval = Constants::updateInterval; + for (int i = 0; i < 5; i++) + { + m_sonarMap[i] = Constants::maxDistance; // Default values + } +} + +void RobotControl::begin() +{ + Serial.begin(9600); // Can not be inside a constructor + m_servo.begin(); // Servo initialization can not be done inside Robot constructor +} + +String RobotControl::getBTData() +{ + String dataBT = ""; + while ((Serial.available() > 0) && (dataBT.endsWith("}") == false)) + { + + dataBT += static_cast(Serial.read()); + delay(3); + } + if ((dataBT.length() > 0) && dataBT.endsWith("}")) + return dataBT; + else + return ""; +} + +void RobotControl::remoteControlMode(RemoteOrder order) +{ + switch (order) + { + case RemoteOrder::STOP: + m_motors.stop(); + return; + case RemoteOrder::FORWARD: + m_motors.forward(); + return; + case RemoteOrder::BACKWARD: + m_motors.backward(); + return; + case RemoteOrder::ROTATELEFT: + m_motors.rotateLeft(); + return; + case RemoteOrder::ROTATERIGHT: + m_motors.rotateRight(); + return; + default: + return; + } +} + +void RobotControl::IRControlMode() +{ + RemoteOrder order = m_IRreceiver.decodeIR(); + switch (order) // Equal to remoteControlMode but updating the timer + { + case RemoteOrder::STOP: + m_motors.stop(); + m_lastUpdate = millis(); + break; + case RemoteOrder::FORWARD: + m_motors.forward(); + m_lastUpdate = millis(); + break; + case RemoteOrder::BACKWARD: + m_motors.backward(); + m_lastUpdate = millis(); + break; + case RemoteOrder::ROTATELEFT: + m_motors.rotateLeft(); + m_lastUpdate = millis(); + break; + case RemoteOrder::ROTATERIGHT: + m_motors.rotateRight(); + m_lastUpdate = millis(); + break; + default: + break; + } + + if ((millis() - m_lastUpdate) >= Constants::IRMovingInterval) // Stop after IRMovingInterval + { + m_lastUpdate = millis(); + m_motors.stop(); + } +} + +void RobotControl::obstacleAvoidanceMode() +{ + if ((millis() - m_lastUpdate) >= m_interval) + { + m_lastUpdate = millis(); + m_sonarMap[mapAngle(m_servo.read())] = m_ultrasonic.getDistance(); + switch (m_state) + { + case RobotModeState::START: + if (m_sonarMap[2] >= Constants::minDistance) + { + m_previousAngle = 30; + moveServoSequence(); + m_motors.forward(calculateSpeed(m_sonarMap[2])); + m_state = RobotModeState::FORWARD; + m_interval = Constants::updateInterval; // Reset scan interval + } + else + { + m_previousAngle = 0; + moveServoSequence(); + m_motors.stop(); + m_state = RobotModeState::OBSTACLE; + m_interval = Constants::updateInterval; // Reset scan interval + } + break; + case RobotModeState::FORWARD: + if (m_sonarMap[2] < Constants::minDistance) + { + m_previousAngle = 0; + moveServoSequence(); // Go to 180 + m_motors.stop(); + m_state = RobotModeState::OBSTACLE; + m_interval = Constants::updateInterval; // Reset scan interval + } + else if (m_sonarMap[1] < Constants::minDistance) + { + if (!m_motors.isRotatingLeft()) + m_motors.rotateLeft(); + m_interval = 0; // Scan faster and don't turn the servo + } + else if (m_sonarMap[3] < Constants::minDistance) + { + if (!m_motors.isRotatingRight()) + m_motors.rotateRight(); + m_interval = 0; // Scan faster and don't turn the servo + } + else + { + moveServoSequence(); + m_motors.forward(calculateSpeed(m_sonarMap[2])); + m_interval = Constants::updateInterval; // Reset scan interval + } + break; + case RobotModeState::OBSTACLE: + if (m_servo.read() != 0) + moveServoSequence(); + else + { + moveServoSequence(); // Go back to 90 + if ((m_sonarMap[0] < Constants::minDistance) && (m_sonarMap[2] < Constants::minDistance) && (m_sonarMap[4] < Constants::minDistance)) + m_state = RobotModeState::BLOCKED; + else + m_state = RobotModeState::ROTATE; + } + break; + case RobotModeState::ROTATE: + (m_sonarMap[0] < m_sonarMap[4]) ? m_motors.rotateLeft() : m_motors.rotateRight(); // Condicional operator + m_interval = Constants::rotate90Time; // Rotate 90 + m_state = RobotModeState::START; + m_sonarMap[1] = Constants::maxDistance; // Reset values + m_sonarMap[3] = Constants::maxDistance; // Reset values + break; + case RobotModeState::BLOCKED: + (m_sonarMap[0] < m_sonarMap[4]) ? m_motors.rotateLeft() : m_motors.rotateRight(); // Condicional operator + m_interval = Constants::rotate180Time; // Rotate 180 + m_state = RobotModeState::START; + m_sonarMap[1] = Constants::maxDistance; // Reset values + m_sonarMap[3] = Constants::maxDistance; // Reset values + break; + default: + break; + } + } +} + +void RobotControl::lineTrackingMode() +{ + switch (m_state) + { + case RobotModeState::START: + if (m_lineTracking.allLines()) // Car not on the floor + break; + if ((millis() - m_lastUpdate) >= Constants::updateUltrasonicInterval) + { + m_sonarMap[mapAngle(90)] = m_ultrasonic.getDistance(Constants::maxDistanceLineTracking); + m_lastUpdate = millis(); + if ((m_sonarMap[mapAngle(90)] >= Constants::minDetourDistance) && m_lineTracking.anyLine()) + m_state = RobotModeState::FORWARD; // Move only if no obstacle and any line detected + } + break; + case RobotModeState::FORWARD: + // Update ultrasonic map + if ((millis() - m_lastUpdate) >= Constants::updateUltrasonicInterval) + { + m_lastUpdate = millis(); + m_sonarMap[2] = m_ultrasonic.getDistance(Constants::maxDistanceLineTracking); + if (m_sonarMap[2] < Constants::minDetourDistance) // Obstacle found + { + m_motors.stop(); + m_servo.write(0); // Look right + m_state = RobotModeState::ROTATE; + m_motors.rotateLeft(); + m_lastUpdate = millis(); + delay(Constants::rotate90Time/2); // To avoid the line detection in ROTATE + return; + } + } + + if (m_lineTracking.leftLine()) + m_motors.rotateLeft(); + else if (m_lineTracking.rightLine()) + m_motors.rotateRight(); + else if (m_lineTracking.midLine()) + m_motors.forward(calculateSpeed(m_sonarMap[2], Constants::minDetourDistance, Constants::maxDistanceLineTracking, Constants::moveSpeed)); + else // No line detected + { + // Wait to avoid line missing in between sensors + unsigned long timeNoLine = millis(); + while ((millis() - timeNoLine) < Constants::timeUntilLost) + { + if (m_lineTracking.anyLine()) + return; + } + m_motors.stop(); + m_state = RobotModeState::LINELOST; + } + break; + case RobotModeState::OBSTACLE: + if (!m_lineTracking.midLine()) + { + if ((millis() - m_lastUpdate) >= Constants::updateUltrasonicInterval) + { + m_sonarMap[0] = m_ultrasonic.getDistance(Constants::maxDistanceLineTracking); + if (m_sonarMap[0] > (Constants::minDetourDistance - Constants::marginObject) && m_sonarMap[0] < (Constants::minDetourDistance + Constants::marginObject)) + m_motors.forward(Constants::moveSpeed); + else if (m_sonarMap[0] > Constants::minDetourDistance ) // Go closer + m_motors.move(calculateSpeed(m_sonarMap[0], 0, 100, Constants::moveSpeed), 0); + else // Go further + m_motors.move(0, calculateSpeed(m_sonarMap[0], 0, 100, Constants::moveSpeed)); + } + } + else // Going around finished, line detected, last rotation + { + m_motors.forward(); + delay(Constants::extraTimeLine); // Extra time to over pass the line + m_motors.stop(); + m_servo.write(90); // Look front + m_motors.rotateLeft(); + while (!m_lineTracking.midLine()); // Keep rotating until you find the line + m_state = RobotModeState::START; + m_motors.stop(); + } + break; + case RobotModeState::ROTATE: // Totate 90 deg + if ((millis() - m_lastUpdate) >= Constants::rotate90Time) + { + m_motors.stop(); + m_lastUpdate = millis(); + m_state = RobotModeState::OBSTACLE; + } + break; + + case RobotModeState::LINELOST: + m_motors.rotateRight(); // Rotate 180 and find the line + delay(Constants::rotate180Time); + m_motors.forward(); // Try and find the line backwards + + // Wait and see if backwards you can find the line + unsigned long timeNoLine = millis(); + while ((millis() - timeNoLine) < Constants::timeLost) + { + if (m_lineTracking.anyLine()) + break; + } + m_motors.stop(); + m_state = RobotModeState::START; + break; + case RobotModeState::BLOCKED: + break; + default: + break; + } +} + +void RobotControl::parkMode() +{ + for (int i = 0; i <= 1; i++) + { + m_servo.write(i * 180); + delay(2 * Constants::updateInterval); + m_sonarMap[mapAngle(m_servo.read())] = m_ultrasonic.getDistance(); + } + if (m_sonarMap[mapAngle(0)] < m_sonarMap[mapAngle(180)]) // Park on the right + m_servo.write(0); + else + m_servo.write(180); // Park on the left + + delay(2 * Constants::updateInterval); // Enough time to move the servo + + // Pass the 1st object + while (m_ultrasonic.getDistance() < Constants::minDistance) + { + delay(20); + m_motors.forward(Constants::crankSpeed); + } + + // Arrive to the second object + while (m_ultrasonic.getDistance() > Constants::minDistance) + { + delay(50); + m_motors.forward(Constants::crankSpeed); + } + + m_motors.backward(Constants::crankSpeed); + delay(Constants::timeMoveAway); // Time to move away + + if (m_sonarMap[mapAngle(0)] < m_sonarMap[mapAngle(180)]) // Park on the right + m_motors.rotateRight(); + else // Park on the left + m_motors.rotateLeft(); + delay(Constants::rotate90Time); + m_motors.stop(); + m_motors.forward(Constants::crankSpeed); + delay(Constants::timeMoving); + + if (m_sonarMap[mapAngle(0)] < m_sonarMap[mapAngle(180)]) // Park on the right + m_motors.rotateLeft(); + else // Park on the left + m_motors.rotateRight(); + delay(Constants::rotate90Time); + m_motors.stop(); +} + +void RobotControl::customMode() +{ + if (m_servo.read() != 0) + { + m_servo.write(0); + delay(300); + return; + } + m_sonarMap[0] = m_ultrasonic.getDistance(Constants::maxDistanceLineTracking); + if (m_sonarMap[0] > (Constants::minDetourDistance - Constants::marginObject) && m_sonarMap[0] < (Constants::minDetourDistance + Constants::marginObject)) + m_motors.forward(Constants::moveSpeed); + else if (m_sonarMap[0] > Constants::minDetourDistance ) // Go closer + m_motors.move(calculateSpeed(m_sonarMap[0], 0, 100, Constants::moveSpeed), 0); + else // Go further + m_motors.move(0, Constants::moveSpeed); +} + +uint8_t RobotControl::mapAngle(uint8_t angle) +{ + switch (angle) + { + case 0: return 0; + case 30: return 1; + case 90: return 2; + case 150: return 3; + case 180: return 4; + } +} + +void RobotControl::moveServoSequence() // Sequences: {90, 150, 90, 30} {90, 180, 90, 0} +{ + uint8_t currentAngle = m_servo.read(); + if (currentAngle != 90) + m_servo.write(90); + else + { + if (m_previousAngle == 150) + m_servo.write(30); + else if (m_previousAngle == 30) + m_servo.write(150); + else if (m_previousAngle == 180) + m_servo.write(0); + else if (m_previousAngle == 0) + m_servo.write(180); + } + m_previousAngle = currentAngle; +} + +uint8_t RobotControl::calculateSpeed(uint16_t distance, uint16_t minDistance, uint16_t maxDistance, uint8_t minSpeed) +{ + return minSpeed + static_cast((distance - minDistance) * (255 - minSpeed) / (maxDistance - minDistance)); +} diff --git a/src/robot.h b/src/robot.h new file mode 100644 index 0000000..3baad96 --- /dev/null +++ b/src/robot.h @@ -0,0 +1,54 @@ +/** + * @file robot.h + * @author José Ángel Sánchez (https://github.com/gelanchez) + * @brief Library for controling the robot. + * @version 1.0.0 + * @date 2020-08-22 + * @copyright GPL-3.0 + */ +#ifndef ROBOT_H +#define ROBOT_H + +#include "motors.h" +#include "myservo.h" +#include "IRreceiver.h" +#include "linetracking.h" +#include "ultrasonic.h" +#include + +class RobotControl +{ + private: + Motors m_motors; + MyServo m_servo; + Ultrasonic m_ultrasonic; + LineTracking m_lineTracking; + uint16_t m_sonarMap[5]; + RobotModeState m_state; // State of the RobotMode + uint8_t m_previousAngle; // Previous angle of the servo + unsigned long m_lastUpdate; + uint16_t m_interval; + + public: + IRreceiver m_IRreceiver; // Member variable as public to enable from main + RobotControl(); + ~RobotControl(); + void restartState(); + void begin(); + String getBTData(); + + void remoteControlMode(RemoteOrder order); + void IRControlMode(); + void obstacleAvoidanceMode(); + void lineTrackingMode(); + void parkMode(); + void customMode(); + + protected: + void speedControl(); + uint8_t mapAngle(uint8_t angle); + void moveServoSequence(); + uint8_t calculateSpeed(uint16_t distance, uint16_t minDistance = Constants::minDistance, uint16_t maxDistance = Constants::maxDistance, uint8_t minSpeed = Constants::crankSpeed); +}; + +#endif \ No newline at end of file diff --git a/src/ultrasonic.cpp b/src/ultrasonic.cpp new file mode 100644 index 0000000..bdb160b --- /dev/null +++ b/src/ultrasonic.cpp @@ -0,0 +1,47 @@ +/** + * @file ultrasonic.cpp + * @author José Ángel Sánchez (https://github.com/gelanchez) + * @brief Library for using the ultrasonic sensor HC-SR04. + * @version 1.0.0 + * @date 2020-08-22 + * @copyright GPL-3.0 + */ + +#include "constants.h" +#include "ultrasonic.h" +#include + +Ultrasonic::Ultrasonic() +{ + pinMode(Constants::trigPin, OUTPUT); + digitalWrite(Constants::trigPin, LOW); + pinMode(Constants::echoPin, INPUT); +} + +Ultrasonic::~Ultrasonic() +{ +} + +/** + * @brief Mesaures the distance using the ultrasonic sensor. + * @param maxDistance Max distance to be considered. + * @return uint16_t Distance measured in cm. + */ +uint16_t Ultrasonic::getDistance(uint16_t maxDistance) +{ + digitalWrite(Constants::trigPin, LOW); + delayMicroseconds(3); + digitalWrite(Constants::trigPin, HIGH); + delayMicroseconds(10); + digitalWrite(Constants::trigPin, LOW); + // noInterrupts(); + unsigned long duration = pulseIn(Constants::echoPin, HIGH, maxDistance * 2 / Constants::soundSpeed); + // interrupts(); + + if (duration == 0) + return maxDistance; + + // Calculate distance (cm) + uint16_t distance = static_cast((duration/2)*Constants::soundSpeed); + return distance; +} \ No newline at end of file diff --git a/src/ultrasonic.h b/src/ultrasonic.h new file mode 100644 index 0000000..cc461f2 --- /dev/null +++ b/src/ultrasonic.h @@ -0,0 +1,24 @@ +/** + * @file ultrasonic.h + * @author José Ángel Sánchez (https://github.com/gelanchez) + * @brief Library for using the ultrasonic sensor HC-SR04. + * @version 1.0.0 + * @date 2020-08-22 + * @copyright GPL-3.0 + */ +#ifndef ULTRASONIC_H +#define ULTRASONIC_H + +#include + +class Ultrasonic +{ + private: + + public: + Ultrasonic(); + ~Ultrasonic(); + uint16_t getDistance(uint16_t maxDistance = Constants::maxDistance); +}; + +#endif \ No newline at end of file