From e65b30830ea2c4fe08681cc800cda865482f0aa1 Mon Sep 17 00:00:00 2001 From: Peter Date: Mon, 24 Oct 2022 20:50:42 +0800 Subject: [PATCH] New tree because of the `TARGET_POSITION` macro. Original is at http://github.com/Alphurious007/DES. Copy of the `peterus` branch which has my code. Original was adapted from MoBot but I have added many changes and replaced many parts so it is almost completely different. --- .gitignore | 36 +++++ README.md | 1 + bot.h | 17 +++ des_car.ino | 355 ++++++++++++++++++++++++++++++++++++++++++++++++++ parameters.h | 61 +++++++++ steering.cpp | 64 +++++++++ steering.h | 11 ++ user_config.h | 20 +++ util.cpp | 39 ++++++ util.h | 38 ++++++ 10 files changed, 642 insertions(+) create mode 100644 .gitignore create mode 100644 README.md create mode 100644 bot.h create mode 100644 des_car.ino create mode 100644 parameters.h create mode 100644 steering.cpp create mode 100644 steering.h create mode 100644 user_config.h create mode 100644 util.cpp create mode 100644 util.h diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..6c44692 --- /dev/null +++ b/.gitignore @@ -0,0 +1,36 @@ +old/ + +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +build/ \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..75054e5 --- /dev/null +++ b/README.md @@ -0,0 +1 @@ +# DES \ No newline at end of file diff --git a/bot.h b/bot.h new file mode 100644 index 0000000..58c819c --- /dev/null +++ b/bot.h @@ -0,0 +1,17 @@ +#ifndef _BOT_H +#define _BOT_H + +#include "parameters.h" +#include "util.h" +#include "user_config.h" +#include "steering.h" + +#include +#include +#include +#include +#include +#include +#include + +#endif \ No newline at end of file diff --git a/des_car.ino b/des_car.ino new file mode 100644 index 0000000..12b4b8b --- /dev/null +++ b/des_car.ino @@ -0,0 +1,355 @@ +// ARDUINO (UNO) SETUP: +// ======================= +// Ping sensor = 5V, GRND, D11 (for both trigger & echo) +// Adafruit GPS = D7 & D8 (GPS Shield, but pins used internally) +// Adafruit Magnetometer Adafruit HMC5883L = I2C : SCL (A5) & SDA (A4) +// SD Card (D10, D11, D12, D13) +#include "bot.h" + +NeoSWSerial gps_port(GNSS_PORT); + +#ifdef COMPASS +LSM303 compass; +#endif + +/****************************** + * GPS information * + ******************************/ +static NMEAGPS gps; // this parses the GPS characters +static gps_fix fix; // this holds the latest GPS fix + +/****************************** + Waypoint information +******************************/ +#define target_waypoint (*waypoints[waypoint_number]) +#define NUM_WAYPOINTS 2 +NeoGPS::Location_t *waypoints[NUM_WAYPOINTS]; +int waypoint_number = 0; + +#ifdef ULTRASONIC_SENSOR +NewPing sonar(US_TRIGGER_PIN, US_ECHO_PIN, US_MAX_DISTANCE); // NewPing setup of pins and maximum distance. +#endif + +// NOTE: delay BLOCKS THE ISR FROM RECEIVING DATA. +static void gnss_isr(uint8_t c) +{ + gps.handle(c); +} + +void setup() +{ +#ifdef DEBUG + Serial.begin(PRINTER_BAUD_RATE); +#endif + +#ifdef COMPASS + Wire.begin(); + compass.init(); + compass.enableDefault(); + + compass.m_min = (LSM303::vector){COMPASS_CAL_MIN}; + compass.m_max = (LSM303::vector){COMPASS_CAL_MAX}; + +#endif + DEBUG_PRINTLN(F("GNSS")); + + // GPS configurations + gps_port.begin(GNSS_BAUD_RATE); + +#ifdef FAST_GNSS + gps.send_P(&gps_port, F("PUBX,41,1,3,3,38400,0")); // SET BAUD RATE TO 57600 + gps_port.flush(); + delay(100); + gps_port.end(); + gps_port.begin(38400); + send_ubx(&gps_port, UBX_UPDATE_5HZ, sizeof(UBX_UPDATE_5HZ)); +#endif + gps_port.attachInterrupt(gnss_isr); + + DEBUG_PRINTLN(F("Getting starting position")); + // FIRST GO TO THE TARGET WAYPOINT... + waypoints[0] = new NeoGPS::Location_t(TARGET_POSITION); + + while (1) + { + if (gps.available()) + { + fix = gps.read(); + DEBUG_PRINT(F("SIV ")); + DEBUG_PRINTLN(fix.satellites); + if (fix.valid.location && fix.satellites >= SATELLITES_MIN + // TODO: try and use the hdop condition for getting a good starting location + // also can get error in cm from this library - check error to end location? + // && fix.hdop < 2000 // from wiki - 1(000)-2(000) is excellent. note that the hdop value is a thousanth. + ) + { + // ...THEN GO TO THE START WAYPOINT. + waypoints[1] = new NeoGPS::Location_t(fix.location.lat(), fix.location.lon()); + break; + } + } + } + + INITIALIZE_DRIVE(); + + digitalWrite(LED_BUILTIN, HIGH); + DEBUG_PRINTLN(F("STARTED")); +} + +void loop() +{ +#ifndef COMPASS + static uint8_t err_location_counter = 0; + static uint8_t err_heading_counter = 0; +#endif + + // #define DEBUG_TIME + +#ifndef COMPASS +#define MAX_ERROR 20 +#define RETRY_HEADING_TIME 1000 +#endif + +#ifdef DEBUG_TIME + static long old_time = millis(); +#endif + + if (gps.available()) + { + fix = gps.read(); +#ifndef COMPASS + check_stop(); + if (!fix.valid.location) + { + err_location_counter++; + } + else if (!fix.valid.heading) + { + err_heading_counter++; + } + else if (fix.satellites >= SATELLITES_MIN) + adjust_course(); +#else + if (fix.valid.location && fix.satellites >= SATELLITES_MIN) + { + check_stop(); + adjust_course(); + } +#endif + +#ifdef DEBUG_TIME + DEBUG_PRINTLN(millis() - old_time); + old_time = millis(); +#endif + } + +#ifndef COMPASS + if (err_location_counter > MAX_ERROR) + { + DEBUG_PRINTLN(F("GPS location invalid!")); + err_location_counter = 0; + diff_drive(STOP, STRAIGHT); + } + // DO NOT TRY TO FIND HEADING IF LOCATION IS STILL INVALID + else if (err_heading_counter > MAX_ERROR) + { + DEBUG_PRINTLN(F("GPS heading invalid!")); + err_heading_counter = 0; + steer(STRAIGHT); + drive(FORWARD, DRIVE_SPEED); + delay(RETRY_HEADING_TIME); + drive(STOP, DRIVE_SPEED); + delay(RETRY_HEADING_TIME); + // MOVE FORWARD FOR A SECOND TO GET A CHANGE IN POSITION + // TO GET THE HEADING. + } +#endif +} + +void check_stop() +{ + float distance_k = fix.location.DistanceKm(target_waypoint); + if (distance_k * 1000 <= DISTANCE_THRESHOLD_M) + { +#ifdef ULTRASONIC_SENSOR + find_cone(); +#endif + waypoint_number++; + DEBUG_PRINT(F("REACHED WAYPOINT ")); + DEBUG_PRINTLN(waypoint_number); + for (int i = 0; i < 50; i++) + { + digitalWrite(LED_BUILTIN, HIGH); + delay(100); + digitalWrite(LED_BUILTIN, LOW); + delay(100); + } + if (waypoint_number == NUM_WAYPOINTS) + { + digitalWrite(LED_BUILTIN, LOW); + while (1) + ; + } + } + else + { + DEBUG_PRINT("SIV "); + DEBUG_PRINT(fix.satellites); + } +} + +// The function for adjusting the course of the vehicle +// Based on the current bearing and desired bearing +void adjust_course() +{ + // the distance to the current target coordinate is calculated + // calculates the desired/"optimal" bearing based on the current location and target location + float target_heading = fix.location.BearingToDegrees(target_waypoint); + float distance_k = fix.location.DistanceKm(target_waypoint); + +#ifdef COMPASS + compass.read(); + float heading = compass.heading(); +#else + float heading = fix.heading(); +#endif + + DEBUG_PRINT(F("angle/velocity: ")); + DEBUG_PRINT(fix.heading()); + DEBUG_PRINT(F("/")); + DEBUG_PRINTLN(fix.speed_kph()); + + DEBUG_PRINT(F("Current Location: ")); + DEBUG_PRINT(fix.latitudeL()); + DEBUG_PRINT(F(", ")); + DEBUG_PRINTLN(fix.longitudeL()); + + DEBUG_PRINT(F("Current heading: ")); + DEBUG_PRINTLN(String(heading, 10)); + + DEBUG_PRINT(KM_M_INT(distance_k)); + DEBUG_PRINTLN(F(" m to target")); + + DEBUG_PRINT(F("Target bearing: ")); + DEBUG_PRINTLN(target_heading); + + DEBUG_PRINT(F("NEXT WAYPOINT - ")); + DEBUG_PRINT(target_waypoint.lat()); + DEBUG_PRINT(F(",")); + DEBUG_PRINTLN(target_waypoint.lon()); + + // find the difference between the current bearing and the target bearing + int16_t error = normalize_angle(int16_t(heading - target_heading)); + + DEBUG_PRINT(F(" - Error: ")); + DEBUG_PRINT(error); + + // depending on the error, the vehicle will then adjust its course + // small delays for the left and right turns are added to ensure it doesn't oversteer. + + int drive_delay = 0; + if (abs(error) <= HEADING_THRESHOLD) + { + if (KM_M_INT(distance_k) <= DISTANCE_SLOW) + drive_delay = DRIVE_SLOW_DELAY; + else + drive_delay = DRIVE_FAST_DELAY; + DEBUG_PRINTLN(F(" On course")); + diff_drive(FORWARD, STRAIGHT); + } + else if (error > 0) + { + DEBUG_PRINTLN(F(" Adjusting towards the left")); + diff_drive(FORWARD, LEFT); + drive_delay = TURNING_DELAY(error); + } + else // if (error > 0) + { + DEBUG_PRINTLN(F(" Adjusting towards the right")); + diff_drive(FORWARD, RIGHT); + drive_delay = TURNING_DELAY(error); + } + DEBUG_PRINT(F("Drive delay ")); + DEBUG_PRINTLN(drive_delay); + DEBUG_PRINTLN(); + // DO NOT BLOCK INTERRUPTS FROM GNSS BY USING SINGLE LONG delay() + for (int i = 0; i < drive_delay; i++) + delay(10); + diff_drive(STOP, STRAIGHT); // WAIT FOR UPDATED LOCATION THEN ADJUST +} + +unsigned long window_sum(unsigned long windowing[], uint8_t sizeof_window, unsigned long next) +{ + DEBUG_PRINT(F("sum=")); + for (uint8_t i = sizeof_window - 1; i >= 1; i--) + { + windowing[i] = windowing[i - 1]; + DEBUG_PRINT(windowing[i]); + DEBUG_PRINT(","); + } + windowing[0] = next; + DEBUG_PRINTLN(windowing[0]); + unsigned long sum = 0; + for (uint8_t i = 0; i < sizeof_window; i++) + sum += windowing[i]; + return sum; +} + +#ifdef ULTRASONIC_SENSOR +void find_cone(void) +{ + DEBUG_PRINTLN(F("Finding nearest cone...")); + unsigned long distance = sonar.ping_cm(); + unsigned long prev_distance = distance; +#define WINDOW_SIZE 10 + unsigned long windowing[WINDOW_SIZE] = {0}; + memset(windowing, 0, WINDOW_SIZE * sizeof(unsigned long)); + unsigned long sum = 0; + for (int i = 0; i < US_SEARCH_TIME; i++) + { + distance = sonar.ping_cm(); + sum = window_sum(windowing, WINDOW_SIZE, distance); + float distance_k = fix.location.DistanceKm(target_waypoint); + DEBUG_PRINT(F("Distance to target ")); + DEBUG_PRINT(distance); + DEBUG_PRINT(F(", distance to gnss ")); + DEBUG_PRINT(distance_k * 1000); + DEBUG_PRINT(F(", i=")); + DEBUG_PRINTLN(i); + if (distance > 0) // && distance_k * 1000 <= DISTANCE_THRESHOLD_ERR_M) + { + // HACK TO MAKE IT WORK + memset(windowing, 10, WINDOW_SIZE * sizeof(unsigned long)); + sum = 1000; + // END HACK + for (uint8_t i = 0; i < 2; i++) + { + // TURNING A FEW MORE STEPS. + diff_drive(FORWARD, LEFT); + delay(100); + diff_drive(STOP, STRAIGHT); + delay(100); + } + + DEBUG_PRINTLN(F("TARGET ACQUIRED. DRIVING...")); + diff_drive(FORWARD, STRAIGHT); + while (sum > 10 * WINDOW_SIZE) + { + delay(100); + distance = sonar.ping_cm(); + sum = window_sum(windowing, WINDOW_SIZE, distance); + DEBUG_PRINT(F("Distance ")); + DEBUG_PRINTLN(distance); + } + DEBUG_PRINTLN(F("DONE.")); + diff_drive(STOP, STRAIGHT); // WAIT FOR UPDATED LOCATION THEN ADJUST + delay(10); + return; + } + diff_drive(FORWARD, LEFT); + delay(100); + diff_drive(STOP, STRAIGHT); + delay(100); + } +} +#endif \ No newline at end of file diff --git a/parameters.h b/parameters.h new file mode 100644 index 0000000..22ea8c6 --- /dev/null +++ b/parameters.h @@ -0,0 +1,61 @@ +#ifndef _PARAMETERS_H +#define _PARAMETERS_H + +/****************************** + * Pin definition for motors * + ******************************/ +#define WHEEL_L_F 10 // D5 +#define WHEEL_L_B 3 // D6 +#define WHEEL_R_F 6 // D9 +#define WHEEL_R_B 5 // D10 + +#define GNSS_BAUD_RATE 9600 + +/* + * OFFSET CALIBRATION + */ +#define COMPASS +/* + * CONTROLLER LOOP + */ +#define KP 1 + +// The bearing threshold is the amount of degrees the vehicle can be off its desired bearing before adjusting the course +#define HEADING_THRESHOLD 10 // [DEGREES] +// Distance threshold is the amount of meters away from target coordinate, that when exceeded it will stop. +#define DISTANCE_THRESHOLD_M 0.3 // [METERS] +#define DISTANCE_THRESHOLD_ERR_M 2 // [METERS] +#define DISTANCE_SLOW 7 // [METERS] + +#define DRIVE_SLOW_DELAY 100 // [*10 ms] +#define DRIVE_FAST_DELAY 200 // [*10 ms] + +// error between 0 and 180 +// divide by two for prop constant. +// divide 15 dry, 5 wet +#define TURNING_DELAY(error) (abs(error) / 5 + 10) + +// Derived from testing with the 50 Hz PWM configuration. +#define DRIVE_PWM_MAX_VALUE 156 +#define DRIVE_PWM_MIN_VALUE (DRIVE_PWM_MAX_VALUE / 8) +#define DRIVE_SPEED DRIVE_PWM_MAX_VALUE // (DRIVE_PWM_MAX_VALUE / 7) + +#define GNSS_PORT 8, 9 +#define SATELLITES_MIN 6 +#define NMEAGPS_INTERRUPT_PROCESSING +// #define GPS_FIX_HDOP +// #define FAST_GNSS + +#define KM_M_INT(distance_k) int(distance_k * 1000) + +#define ULTRASONIC_SENSOR + +#ifdef ULTRASONIC_SENSOR +#define US_TRIGGER_PIN 13 // Arduino pin tied to trigger pin on the ultrasonic sensor. +#define US_ECHO_PIN 11 // Arduino pin tied to echo pin on the ultrasonic sensor. +#define US_MAX_DISTANCE 500 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm. +#define US_SEARCH_TIME 300 // [*10 ms] +#define US_SEARCH_ANGLE_ITER 10 // [*10 ms] +#endif + +#endif // _PARAMETERS_H \ No newline at end of file diff --git a/steering.cpp b/steering.cpp new file mode 100644 index 0000000..aae63c2 --- /dev/null +++ b/steering.cpp @@ -0,0 +1,64 @@ + +#include +#include +#include "util.h" +#include "parameters.h" + +typedef enum driving_direction +{ + FORWARD, + STOP, +} DRIVING_DIRECTION; + +typedef enum steering_direction +{ + RIGHT, + STRAIGHT, + LEFT, +} STEERING_DIRECTION; + +// *(10 ms) +#define STEERING_DURATION 25 +#define DRIVE_DURATION 200 +#define STOP_DURATION 0 + +#define INITIALIZE_DRIVE() \ + pinMode(WHEEL_L_F, OUTPUT); \ + pinMode(WHEEL_L_B, OUTPUT); \ + pinMode(WHEEL_R_F, OUTPUT); \ + pinMode(WHEEL_R_B, OUTPUT); \ + diff_drive(STOP, STRAIGHT); + +inline void diff_drive(DRIVING_DIRECTION drive, STEERING_DIRECTION steer) +{ + if (drive == STOP) + { + digitalWrite(WHEEL_L_F, LOW); + digitalWrite(WHEEL_L_B, LOW); + digitalWrite(WHEEL_R_F, LOW); + digitalWrite(WHEEL_R_B, LOW); + return; + } + + switch (steer) + { + case LEFT: + digitalWrite(WHEEL_L_F, HIGH); + digitalWrite(WHEEL_L_B, LOW); + digitalWrite(WHEEL_R_F, LOW); + digitalWrite(WHEEL_R_B, HIGH); + return; + case RIGHT: + digitalWrite(WHEEL_L_F, LOW); + digitalWrite(WHEEL_L_B, HIGH); + digitalWrite(WHEEL_R_F, HIGH); + digitalWrite(WHEEL_R_B, LOW); + return; + case STRAIGHT: + digitalWrite(WHEEL_L_F, LOW); + digitalWrite(WHEEL_L_B, HIGH); + digitalWrite(WHEEL_R_F, LOW); + digitalWrite(WHEEL_R_B, HIGH); + return; + } +} \ No newline at end of file diff --git a/steering.h b/steering.h new file mode 100644 index 0000000..8d455a2 --- /dev/null +++ b/steering.h @@ -0,0 +1,11 @@ +#ifndef STEERING_H +#define STEERING_H + +#include +#include "parameters.h" +#include "util.h" +#include "steering.cpp" + +extern inline void diff_drive(DRIVING_DIRECTION drive, STEERING_DIRECTION steer); + +#endif \ No newline at end of file diff --git a/user_config.h b/user_config.h new file mode 100644 index 0000000..0a03b9b --- /dev/null +++ b/user_config.h @@ -0,0 +1,20 @@ +#ifndef _USER_CONFIG_H +#define _USER_CONFIG_H +/* + * ADD YOUR TARGET LATITUDE AND LONGITUDE IN THE DEFINE. + * AS A FLOATING POINT NUMBER REPRESENTING THE COORDINATE IN DEGREES + * + * ADD A NEW LINE WITH "#define DEBUG" (NO QUOTES) TO ENABLE DEBUG MESSAGES + */ + +#define TARGET_POSITION [LAT], [LON] // FORMAT [LAT], [LON] AS float OR int32_t + +// COMPASS CALIBRATION PARAMETERS +#define COMPASS_CAL_MIN -470, -1083, +837 +#define COMPASS_CAL_MAX +79, -495, +946 + +// DEBUG PRINTER +#define DEBUG +#define PRINTER_BAUD_RATE 115200 + +#endif \ No newline at end of file diff --git a/util.cpp b/util.cpp new file mode 100644 index 0000000..3b2d560 --- /dev/null +++ b/util.cpp @@ -0,0 +1,39 @@ + +#include +#include + +// https://stackoverflow.com/a/24396562 +// Very nice angle normalization +inline int16_t normalize_angle(int16_t angle) +{ + // if (angle > 180) + // angle -= 360; + // else if (angle < -180) + // angle += 360; + // return angle; + angle %= 360; + int16_t fix = angle / 180; // Integer division!! + return (fix) ? angle - (360 * (fix)) : angle; +} + +#ifdef FAST_GNSS +// https://github.com/SlashDevin/NeoGPS/blob/master/examples/ubloxRate/ubloxRate.ino +inline void send_ubx(NeoSWSerial *gps_port, const unsigned char *progmemBytes, uint16_t len) +{ + gps_port->write(0xB5); // SYNC1 + gps_port->write(0x62); // SYNC2 + + uint8_t a = 0, b = 0; + while (len-- > 0) + { + uint8_t c = pgm_read_byte(progmemBytes++); + a += c; + b += a; + gps_port->write(c); + } + + gps_port->write(a); // CHECKSUM A + gps_port->write(b); // CHECKSUM B + +} // sendUBX +#endif \ No newline at end of file diff --git a/util.h b/util.h new file mode 100644 index 0000000..c05d47a --- /dev/null +++ b/util.h @@ -0,0 +1,38 @@ + + +#ifndef _UTIL_H +#define _UTIL_H + +#include +#include +#include "user_config.h" +#include "parameters.h" +#include "util.cpp" + +#ifdef FAST_GNSS +// USE U-Center to get the UBX binary command. +const unsigned char UBX_UPDATE_5HZ[] PROGMEM = + {0x06, 0x08, 0x06, 0x00, 200, 0x00, 0x01, 0x00, 0x01, 0x00}; + +extern inline void send_ubx(NeoSWSerial *gps_port, const unsigned char *progmemBytes, uint16_t len); +#endif + +extern inline int16_t normalize_angle(int16_t angle); + +#ifdef DEBUG +#define DEBUG_PRINTLN(...) Serial.println(__VA_ARGS__) +#else +#define DEBUG_PRINTLN(...) ; +#endif + +#ifdef DEBUG +#define DEBUG_PRINT(...) Serial.print(__VA_ARGS__) +#else +#define DEBUG_PRINT(...) ; +#endif + +#define INITIALIZE_PWM_PIN(pin_name) \ + pinMode((pin_name), OUTPUT); \ + analogWrite((pin_name), 0); + +#endif \ No newline at end of file