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

@@ -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/

README.md

@@ -0,0 +1 @@
+# DES

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 <NewPing.h>
+#include <NMEAGPS.h>
+#include <NeoSWSerial.h>
+#include <Wire.h>
+#include <LSM303.h>
+#include <pins_arduino.h>
+#include <avr/interrupt.h>
+
+#endif

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<int16_t>){COMPASS_CAL_MIN};
+    compass.m_max = (LSM303::vector<int16_t>){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

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

steering.cpp

@@ -0,0 +1,64 @@
+
+#include <Arduino.h>
+#include <inttypes.h>
+#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;
+    }
+}

steering.h

@@ -0,0 +1,11 @@
+#ifndef STEERING_H
+#define STEERING_H
+
+#include <inttypes.h>
+#include "parameters.h"
+#include "util.h"
+#include "steering.cpp"
+
+extern inline void diff_drive(DRIVING_DIRECTION drive, STEERING_DIRECTION steer);
+
+#endif

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

util.cpp

@@ -0,0 +1,39 @@
+
+#include <stdint.h>
+#include <NeoSWSerial.h>
+
+// 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

util.h

@@ -0,0 +1,38 @@
+
+
+#ifndef _UTIL_H
+#define _UTIL_H
+
+#include <Arduino.h>
+#include <inttypes.h>
+#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