peter/ENSC3020-ELEC3020-des-car-2022
1
0
Fork 0
mirror of https://github.com/peter-tanner/des-car-2022.git synced 2024-11-30 12:00:16 +08:00
Code Issues Packages Projects Releases Wiki Activity

New tree because of the TARGET_POSITION macro. Original is at

Browse Source 
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.
This commit is contained in:
Peter 2022-10-24 20:50:42 +08:00
commit e65b30830e
10 changed files with 642 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
.gitignore vendored Normal file
View File

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

1
README.md Normal file
View File

@ -0,0 +1 @@
# DES

17
bot.h Normal file
View File

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

355
des_car.ino Normal file
View File

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

61
parameters.h Normal file
View File

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

64
steering.cpp Normal file
View File

@ -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;
}
}

11
steering.h Normal file
View File

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

20
user_config.h Normal file
View File

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

39
util.cpp Normal file
View File

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

38
util.h Normal file
View File

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