kleiax/Bachelorarbeit-Rover
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 1 Wiki Activity

remove most of the defines

Browse Source
This commit is contained in:
kleiax
2023-08-18 15:34:56 +02:00
parent 4cd2b9368d
commit a82b32087c
21 changed files with 155 additions and 285 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/TODO allgemein.txt
+16 -4
View File
@@ -1,12 +1,24 @@
Für irgendwann: Maybe:
-> Time Measurement in component interface
-> Time warnings
Do later:
-> Program underfloorLighting -> Program underfloorLighting
-> Add an beeper -> Add an beeper
-> Program the beeper -> Program the beeper
-> Update GNSS Lib to v3 -> Update GNSS Lib to v3
-> Engine slow down without curve
-> SiteMenus action yes and no for increment and decrement on one site
-> Network clean up (Mqtt remove?)
-> Extra class for maneuver, autopilot should inherit from int16_t
-> Better remote Control with Leds for gnss rtk etc
-> Test Menu for big curve driving
-> Api with Names and show on Maps in Browser
-> Menu structure mit add functions for each menu mit pointer return to config (additional not replace)
-> Menu Display from parent as run() to make Menu as Component
-> Racing Mode
Besser zügig: Do now:
Doxygen Kommentare aktualisieren Doxygen Kommentare aktualisieren
Motortreiber vielleicht bei stoppen ohne Kurve
Autopilot Funktionen zum einstellen der Geschwindigkeiten
include/config.h
+44 -24
View File
@@ -9,30 +9,50 @@
* *
*/ */
/** #pragma once
* @brief The MAC-Address saved in PS3-Controller
*
* The PS3-Controller only connects to the address which is saved in it.
*/
#define CONTROLLER_MAC "24:62:AB:F2:4B:3A"
/** namespace PinNumbers {
* @brief Prevents creation of the global object constexpr uint8_t spiCopi = 16;
* constexpr uint8_t spiCipo = 4;
* The PS3-Controller class creates a global object to use. constexpr uint8_t spiSck = 5;
*/ constexpr uint8_t gnssSpiCs = 17;
#define NO_GLOBAL_INSTANCES
/** namespace LeftMotor {
* @name SPI-Pins constexpr uint8_t dir1 = 27;
* @brief Pins for SPI communication with the gnss module constexpr uint8_t dir2 = 12;
*/ constexpr uint8_t pwm = 13;
///@{ constexpr uint8_t encoder = 26;
#define UBLOX_GNSS_SPI_CS 17 constexpr uint8_t pmwChannel = 0;
#define UBLOX_GNSS_SPI_COPI 16 }
#define UBLOX_GNSS_SPI_CIPO 4
#define UBLOX_GNSS_SPI_SCK 5
///@}
#define RF24_CSN_PIN 18 namespace RightMotor {
#define RF24_CE_PIN 2 constexpr uint8_t dir1 = 14;
constexpr uint8_t dir2 = 23;
constexpr uint8_t pwm = 22;
constexpr uint8_t encoder = 33;
constexpr uint8_t pmwChannel = 0;
}
}
namespace Settings {
constexpr float wheelDiameter = 0.1263;
constexpr uint16_t encoderSteps = 360;
namespace Pid {
namespace Left {
constexpr uint8_t P = 75;
constexpr uint8_t I = 0;
constexpr uint8_t D = 0;
}
namespace Right {
constexpr uint8_t P = 75;
constexpr uint8_t I = 0;
constexpr uint8_t D = 0;
}
constexpr int8_t outMin = -100;
constexpr int8_t outMax = 100;
constexpr uint8_t sampleTime = 30;
}
}
include/moveControl.h
+1 -1
View File
@@ -18,7 +18,7 @@
#include "motorControl.h" #include "motorControl.h"
#include "speedometer.h" #include "speedometer.h"
#include "moveControlConfig.h" #include "config.h"
#include "debugTimes.h" #include "debugTimes.h"
#include "component.h" #include "component.h"
include/moveControlConfig.h
-42
View File
@@ -1,42 +0,0 @@
/**
* @file moveControlConfig.h
* @author Alexander Klein (alex@kleiax.de)
* @brief This file inherits some configurations for moveControl.h
* @version 0.1
* @date 2021-12-14
*
* @copyright Copyright (c) 2021
*
*/
//Pin config
//Left Motor
#define M_DIR_11 27
#define M_DIR_12 12
#define M_PWM_1 13
#define M_ENCODE_LEFT 26
//Right Motor
#define M_DIR_21 14
#define M_DIR_22 23
#define M_PWM_2 22
#define M_ENCODE_RIGHT 33
// PID config
#define PID_OUT_MIN -100
#define PID_OUT_MAX 100
#define PID_SAMPLETIME 30
// Left PID
#define PID_LEFT_P 75
#define PID_LEFT_I 0
#define PID_LEFT_D 0
// Right PID
#define PID_RIGHT_P 75
#define PID_RIGHT_I 0
#define PID_RIGHT_D 0
// PWM
#define PWM_CHANNEL_M1 0
#define PWM_CHANNEL_M2 1
// SPEEDOMETER
#define WHEEL_DIAMETER 0.1263
#define ENC_STEPS 360
include/networkConfig.h
+2 -48
View File
@@ -56,24 +56,13 @@
#ifdef NTRIP_RTK2GO #ifdef NTRIP_RTK2GO
#define NTRIP_HOST "rtk2go.com" #define NTRIP_HOST "rtk2go.com"
#define NTRIP_PORT 2101 #define NTRIP_PORT 2101
#define NTRIP_MOUNT_POINT "GER-Papenburg" #define NTRIP_MOUNT_POINT "GER-Dortmund"
#define NTRIP_USER "alklein1@gmx.de" #define NTRIP_USER "alklein1@gmx.de"
#define NTRIP_PASSWORD "none" #define NTRIP_PASSWORD "none"
#endif // NTRIP_RTK2GO #endif // NTRIP_RTK2GO
#define RHEDE #define HOTSPOT
//Network config ESP32
#ifdef ESP32BROKER
#define WLAN_SSID "ESP32-Broker"
#define WLAN_PASSWORD "doit2022"
#define WLAN_IP "192.168.4.77"
#define WLAN_SUBNETMASK "255.255.255.0"
#define WLAN_GATEWAY "192.168.4.1"
#define MQTT_SERVER "192.168.4.1"
#define MQTT_PORT 1883
#endif //ESP32BROKER
//Network config Hotspot //Network config Hotspot
#ifdef HOTSPOT #ifdef HOTSPOT
@@ -114,38 +103,3 @@
#define MQTT_PORT 1883 #define MQTT_PORT 1883
#endif //DORO #endif //DORO
//Network config HW1
#ifdef HW1
#define WLAN_SSID "hw1_gast"
#define WLAN_PASSWORD "KeineAhnung"
#define WLAN_IP "192.168.0.67"
#define WLAN_SUBNETMASK "255.255.255.0"
#define WLAN_GATEWAY "192.168.0.1"
#define MQTT
#define MQTT_SERVER "172.22.64.216"
#define MQTT_PORT 1883
#endif //HW1
//Network config FRENZY
#ifdef FRENZY
#define WLAN_SSID "GNX7EDD84-2"
#define WLAN_PASSWORD "2W2ZLPJ9NVQ3"
#define WLAN_IP 0x0F01a8c0 //192.168.1.15
#define WLAN_SUBNETMASK 0x00FFFFFF //255.255.255.0
#define MQTT
#define WLAN_GATEWAY 0xfe01a8c0 //192.168.1.254
#define MQTT_SERVER 0x0201a8c0 //192.168.1.2
#define MQTT_PORT 1883
#endif //FRENZY
//Network config TPMOBIL
#ifdef TPMOBIL
#define WLAN_SSID "TP-Mobil"
#define WLAN_PASSWORD "Punica-699"
#define WLAN_IP 0x0F01a8c0 //192.168.1.15
#define WLAN_SUBNETMASK 0x00FFFFFF //255.255.255.0
#define WLAN_GATEWAY 0x0101a8c0 //192.168.1.1
#define MQTT
#define MQTT_SERVER 0x0201a8c0 //192.168.1.2
#define MQTT_PORT 1883
#endif //TPMOBIL
lib/Counter/counter.cpp
+2 -2
View File
@@ -13,8 +13,8 @@ Counter::Counter(uint8_t pin) {
pcnt_config_t config; pcnt_config_t config;
config.unit = this->unit; config.unit = this->unit;
config.channel = PCNT_CHANNEL_0; config.channel = PCNT_CHANNEL_0;
config.counter_h_lim = COUNTER_HIGH_LIMIT; config.counter_h_lim = Counter::highLimit;
config.counter_l_lim = COUNTER_LOW_LIMIT; config.counter_l_lim = Counter::lowLimit;
config.ctrl_gpio_num = PCNT_PIN_NOT_USED; config.ctrl_gpio_num = PCNT_PIN_NOT_USED;
config.hctrl_mode = PCNT_CHANNEL_LEVEL_ACTION_KEEP; config.hctrl_mode = PCNT_CHANNEL_LEVEL_ACTION_KEEP;
config.lctrl_mode = PCNT_CHANNEL_LEVEL_ACTION_KEEP; config.lctrl_mode = PCNT_CHANNEL_LEVEL_ACTION_KEEP;
lib/Counter/counter.h
+3 -3
View File
@@ -3,9 +3,6 @@
#include <Arduino.h> #include <Arduino.h>
#include <driver/pcnt.h> #include <driver/pcnt.h>
#define COUNTER_HIGH_LIMIT INT16_MAX
#define COUNTER_LOW_LIMIT 0
class Counter { class Counter {
public: public:
Counter(uint8_t pin); Counter(uint8_t pin);
@@ -21,6 +18,9 @@ class Counter {
void filterDisable(); void filterDisable();
private: private:
static constexpr int16_t highLimit = INT16_MAX;
static constexpr uint8_t lowLimit = 0;
static uint8_t amountOfCounter; static uint8_t amountOfCounter;
bool initalised = false; bool initalised = false;
lib/LcdWrapper/LcdWrapper.cpp
+9 -9
View File
@@ -22,20 +22,20 @@ void LcdWrapper::run() {
return; return;
this->lcd->clear(); this->lcd->clear();
for (uint8_t i = 0; i < DISPLAY_WRAPPER_LINES; i++) { for (uint8_t i = 0; i < LcdWrapper::totalLines; i++) {
this->lcd->setCursor(0, i); this->lcd->setCursor(0, i);
this->lcd->print(this->data[i]); this->lcd->print(this->data[i]);
} }
if (this->callback) if (this->callback)
this->callback(this->data, DISPLAY_WRAPPER_LINES, DISPLAY_WRAPPER_ROWS); this->callback(this->data, LcdWrapper::totalLines, LcdWrapper::totalRows);
this->changed = false; this->changed = false;
} }
void LcdWrapper::clear() { void LcdWrapper::clear() {
for (uint8_t i = 0; i < DISPLAY_WRAPPER_LINES; i++) { for (uint8_t i = 0; i < LcdWrapper::totalLines; i++) {
for (uint8_t j = 0; j < DISPLAY_WRAPPER_ROWS; j++) { for (uint8_t j = 0; j < LcdWrapper::totalRows; j++) {
data[i][j] = ' '; data[i][j] = ' ';
} }
} }
@@ -43,11 +43,11 @@ void LcdWrapper::clear() {
} }
void LcdWrapper::setCursor(uint8_t row, uint8_t line) { void LcdWrapper::setCursor(uint8_t row, uint8_t line) {
if (row > DISPLAY_WRAPPER_ROWS - 1) if (row > LcdWrapper::totalRows - 1)
row = DISPLAY_WRAPPER_ROWS - 1; row = LcdWrapper::totalRows - 1;
if (line > DISPLAY_WRAPPER_LINES - 1) if (line > LcdWrapper::totalLines - 1)
line = DISPLAY_WRAPPER_LINES - 1; line = LcdWrapper::totalLines - 1;
this->cursorRow = row; this->cursorRow = row;
this->cursorLine = line; this->cursorLine = line;
@@ -55,7 +55,7 @@ void LcdWrapper::setCursor(uint8_t row, uint8_t line) {
void LcdWrapper::print(const char *str) { void LcdWrapper::print(const char *str) {
uint8_t inputStringPosition = 0; uint8_t inputStringPosition = 0;
for (uint8_t i = this->cursorRow; i < DISPLAY_WRAPPER_ROWS; i++) { for (uint8_t i = this->cursorRow; i < LcdWrapper::totalRows; i++) {
if (str[inputStringPosition] == '\0') if (str[inputStringPosition] == '\0')
break; break;
else else
lib/LcdWrapper/LcdWrapper.h
+5 -7
View File
@@ -17,11 +17,7 @@
#include <displayWrapper.h> #include <displayWrapper.h>
#include <component.h> #include <component.h>
#define DISPLAY_WRAPPER_ROWS 16 typedef void (*LcdWrapperCallback) (const char data[][16], uint8_t lines, uint8_t rows);
#define DISPLAY_WRAPPER_LINES 2
typedef void (*LcdWrapperCallback) (const char data[][DISPLAY_WRAPPER_ROWS], uint8_t lines, uint8_t rows);
/** /**
* @brief A class for the Menu class to print information * @brief A class for the Menu class to print information
* *
@@ -60,13 +56,15 @@ class LcdWrapper : public DisplayWrapper, public Component {
*/ */
void print(const char *str) override; void print(const char *str) override;
static constexpr uint8_t totalRows = 16;
static constexpr uint8_t totalLines = 2;
private: private:
void run() override; void run() override;
LiquidCrystal_I2C* lcd; LiquidCrystal_I2C* lcd;
LcdWrapperCallback callback = nullptr; LcdWrapperCallback callback = nullptr;
char data[DISPLAY_WRAPPER_LINES][DISPLAY_WRAPPER_ROWS]; char data[LcdWrapper::totalLines][LcdWrapper::totalRows];
uint8_t cursorRow = 0; uint8_t cursorRow = 0;
uint8_t cursorLine = 0; uint8_t cursorLine = 0;
lib/MotorControl/motorControl.cpp
+14 -19
View File
@@ -13,9 +13,9 @@
#include "motorControl.h" #include "motorControl.h"
MotorControl::MotorControl() { MotorControl::MotorControl() {
this->setMinPwm(PWMMIN); this->setMinPwm(MotorControl::pwmMin);
this->setMaxPwm(PWMMAX); this->setMaxPwm(MotorControl::pwmMax);
this->loopDelay = DELAY; Component::loopDelay = MotorControl::loopDelay;
} }
void MotorControl::init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8_t dir_2) { void MotorControl::init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8_t dir_2) {
@@ -30,7 +30,7 @@ void MotorControl::init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8
digitalWrite(this->dir_1, LOW); digitalWrite(this->dir_1, LOW);
digitalWrite(this->dir_2, LOW); digitalWrite(this->dir_2, LOW);
ledcSetup(this->pwmChannel, PWMFREQ, this->pwmRes); ledcSetup(this->pwmChannel, MotorControl::pwmFreq, MotorControl::pwmRes);
ledcAttachPin(this->pwmPin, this->pwmChannel); ledcAttachPin(this->pwmPin, this->pwmChannel);
ledcWrite(this->pwmChannel, 0); ledcWrite(this->pwmChannel, 0);
} }
@@ -42,14 +42,14 @@ void MotorControl::run() {
// Difference between targetPower and power // Difference between targetPower and power
int16_t difference = this->targetPower - this->power; int16_t difference = this->targetPower - this->power;
// Check that the target speed is close to 0 and that the abs_difference is lower than powersteps // Check that the target speed is close to 0 and that the abs_difference is lower than MotorControl::powerSteps
if (abs(this->targetPower) < powersteps && abs_difference < powersteps) { if (abs(this->targetPower) < MotorControl::powerSteps && abs_difference < MotorControl::powerSteps) {
this->setRealPower(0); this->setRealPower(0);
return; return;
} }
// Correct speed // Correct speed
if (abs_difference < powersteps) { if (abs_difference < MotorControl::powerSteps) {
return; return;
} }
@@ -58,23 +58,23 @@ void MotorControl::run() {
// Positive or negative speed // Positive or negative speed
if (this->power >= 0) { if (this->power >= 0) {
if (difference > 0) { if (difference > 0) {
this->increasePower(powersteps); this->increasePower(MotorControl::powerSteps);
} else { } else {
this->increasePower(-powersteps); this->increasePower(-MotorControl::powerSteps);
} }
} else { } else {
this->increasePower(powersteps); this->increasePower(MotorControl::powerSteps);
} }
} else { } else {
// Positive or negative speed // Positive or negative speed
if (this->power >= 0) { if (this->power >= 0) {
this->increasePower(-powersteps); this->increasePower(-MotorControl::powerSteps);
} else { } else {
if (difference > 0) { if (difference > 0) {
this->increasePower(powersteps); this->increasePower(MotorControl::powerSteps);
} else { } else {
this->increasePower(-powersteps); this->increasePower(-MotorControl::powerSteps);
} }
} }
} }
@@ -94,11 +94,6 @@ void MotorControl::setMaxPwm(uint8_t max) {
this->dutycycleMax = max; this->dutycycleMax = max;
} }
uint16_t MotorControl::setPowerSteps(uint8_t increment) {
this->powersteps = increment;
return (uint16_t) (this->loopDelay * ( 100 / powersteps ));
}
void MotorControl::setTargetPower(int8_t power) { void MotorControl::setTargetPower(int8_t power) {
if (power <= 100 && power >= -100) if (power <= 100 && power >= -100)
this->targetPower = power; this->targetPower = power;
@@ -168,7 +163,7 @@ void MotorControl::setRealPower(int8_t power) {
void MotorControl::increasePower(int8_t power) { void MotorControl::increasePower(int8_t power) {
//TODO: Exceptionhandling //TODO: Exceptionhandling
//TODO: make a stop befor a direction change //TODO: make a stop befor a direction change
if (abs(power) > 2 * powersteps) { if (abs(power) > 2 * MotorControl::powerSteps) {
Serial.println("Invalid Argument in MotorControl::increasePower"); Serial.println("Invalid Argument in MotorControl::increasePower");
return; return;
} }
lib/MotorControl/motorControl.h
+8 -29
View File
@@ -16,14 +16,7 @@
#include <iostream> #include <iostream>
#include <Arduino.h> #include <Arduino.h>
#include <component.h> #include "component.h"
#define DELAY 10
#define PWMFREQ 16000
#define PWMRES 8
#define POWERSTEPS 2 // A total of 20 levels ( 100 / SPEED_STEPS ) * RUN_MOTOR_CONTROL_DELAY = 500ms
#define PWMMIN 55
#define PWMMAX 94 // Max 98% of 2^PWM_RES
/** /**
* @brief A class which use PWM to control the power of DC Motor * @brief A class which use PWM to control the power of DC Motor
@@ -58,24 +51,6 @@ class MotorControl : public Component {
*/ */
void setMaxPwm(uint8_t max); void setMaxPwm(uint8_t max);
/**
* @brief Set the Power Steps
*
* Set the increment of the steps with which the dutycycle is
* increased or decreased. Note the dependency between the increment
* and delayLoop().
*
* The formula for the time between 0% and 100% power is:
* time[ms] = delayLoop * ( 100 / increment )
* 500 ms are recommended
*
* @see setDelay()
*
* @param increment
* @return time from 0% power to 100% power in Milliseconds
*/
uint16_t setPowerSteps(uint8_t increment);
/** /**
* @brief Set the Target Power * @brief Set the Target Power
* *
@@ -122,20 +97,24 @@ class MotorControl : public Component {
void setRealPower(int8_t power); void setRealPower(int8_t power);
void increasePower(int8_t power); void increasePower(int8_t power);
static constexpr uint8_t loopDelay = 10;
static constexpr uint16_t pwmFreq = 16000;
static constexpr uint8_t pwmRes = 8;
static constexpr uint8_t powerSteps = 2; // A total of 20 levels ( 100 / SPEED_STEPS ) * RUN_MOTOR_CONTROL_DELAY = 500ms
static constexpr uint8_t pwmMin = 55;
static constexpr uint8_t pwmMax = 98; // Max 98% of 2^PWM_RES
int8_t targetPower = 0; int8_t targetPower = 0;
int8_t power = 0; int8_t power = 0;
uint8_t direction = 0; // 0 = stop, 1 = forward, 2 = backward uint8_t direction = 0; // 0 = stop, 1 = forward, 2 = backward
uint8_t pwmPin; uint8_t pwmPin;
uint8_t pwmChannel; uint8_t pwmChannel;
uint8_t pwmRes = PWMRES;
uint16_t dutycycle = 0; uint16_t dutycycle = 0;
uint8_t dutycycleMin; uint8_t dutycycleMin;
uint8_t dutycycleMax; uint8_t dutycycleMax;
uint8_t dir_1; uint8_t dir_1;
uint8_t dir_2; uint8_t dir_2;
uint8_t powersteps = POWERSTEPS;
}; };
#endif // MOTOR_CONTROL_H #endif // MOTOR_CONTROL_H
lib/Navigation/navigation.cpp
+3 -3
View File
@@ -63,7 +63,7 @@ void Navigation::init(Route* route) {
caliCompass.loadData(); caliCompass.loadData();
caliCompass.useData(); caliCompass.useData();
this->loopDelay =AZIMUTH_UPDATE_DELAY; Component::loopDelay = Navigation::loopDelay;
} }
Navigation::~Navigation() { Navigation::~Navigation() {
@@ -168,8 +168,8 @@ Navigation::Status Navigation::addCurrentPosToRoute() {
// Every Point after the first // Every Point after the first
double distance = this->currentPosition.distanceTo(this->lastPointRouteInsert); double distance = this->currentPosition.distanceTo(this->lastPointRouteInsert);
if (MIN_DISTANCE_BETWEEN_POINTS <= distance if (Navigation::minDisBetweenPoints <= distance
&& MAX_DISTANCE_BETWEEN_POINTS >= distance){ && Navigation::maxDisBetweenPoints >= distance){
this->route->addPointToRoute(this->currentPosition); this->route->addPointToRoute(this->currentPosition);
this->lastPointRouteInsert = this->currentPosition; this->lastPointRouteInsert = this->currentPosition;
return Status::Updated; return Status::Updated;
lib/Navigation/navigation.h
+4 -14
View File
@@ -23,18 +23,6 @@
#include "calibrateCompass.h" #include "calibrateCompass.h"
#include "component.h" #include "component.h"
/**
* @brief The minimal distance between Points
*
* This is a very critical option. It have to be
* around the half accuracy of the positioning system
*
*/
#define MIN_DISTANCE_BETWEEN_POINTS 0.3
#define MAX_DISTANCE_BETWEEN_POINTS 10
#define MIN_DISTANCE_TO_REACH_POINT 0.5
#define AZIMUTH_UPDATE_DELAY 20
/** /**
* @brief This struct inherits the result of the navigation * @brief This struct inherits the result of the navigation
* *
@@ -221,6 +209,10 @@ class Navigation : public Component {
bool setTargetPoint(Point target); bool setTargetPoint(Point target);
int16_t calculateCourseCorrection(Point& point); int16_t calculateCourseCorrection(Point& point);
static constexpr uint8_t loopDelay = 20;
static constexpr uint8_t maxDisBetweenPoints = 10;
static constexpr float minDisBetweenPoints = 0.3;
SFE_UBLOX_GNSS* gps; SFE_UBLOX_GNSS* gps;
UBX_NAV_PVT_data_t* ubxData = nullptr; UBX_NAV_PVT_data_t* ubxData = nullptr;
Route* route = nullptr; Route* route = nullptr;
@@ -235,8 +227,6 @@ class Navigation : public Component {
Point::Accuracy minAccuracy = Point::Accuracy::twoDigOfCM; Point::Accuracy minAccuracy = Point::Accuracy::twoDigOfCM;
CalcAzimuthState calcAzimuthState = CalcAzimuthState::Invalid; CalcAzimuthState calcAzimuthState = CalcAzimuthState::Invalid;
bool navigationStarted = false; bool navigationStarted = false;
bool navigationFinished = false; bool navigationFinished = false;
bool isNtripInit = false; bool isNtripInit = false;
lib/Route/route.cpp
+1 -1
View File
@@ -71,7 +71,7 @@ int16_t Point::courseTo(const Coordinates& point) const {
Coordinates begin = this->coordinates; Coordinates begin = this->coordinates;
Coordinates end = point; Coordinates end = point;
double phi = log( tan(end.lat * ROUTE_DEGREE_TO_RADIANT / 2 + ROUTE_PI / 4) / tan(begin.lat * ROUTE_DEGREE_TO_RADIANT / 2 + ROUTE_PI / 4) ); double phi = log( tan(end.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4) / tan(begin.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4) );
double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT); double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT);
int16_t res = static_cast<int16_t>(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1; int16_t res = static_cast<int16_t>(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1;
lib/Route/route.h
-1
View File
@@ -18,7 +18,6 @@
#define ROUTE_DEGREE_TO_RADIANT 0.01745 #define ROUTE_DEGREE_TO_RADIANT 0.01745
#define ROUTE_DISTANCE_BETWEEN_LATITUDE 111300 #define ROUTE_DISTANCE_BETWEEN_LATITUDE 111300
#define ROUTE_PI 3.14159265358979323846
/** /**
* @brief A to handle points on the earth * @brief A to handle points on the earth
lib/Speedometer/speedometer.cpp
+14 -39
View File
@@ -11,18 +11,22 @@
*/ */
#include "speedometer.h" #include "speedometer.h"
Speedometer::Speedometer(uint8_t pin, double diameter, uint16_t steps, uint8_t numOfValForAvg) {
this->init(pin, diameter, steps);
this->bufSize = numOfValForAvg;
this->loopDelay = DELAY_SPEEDOMETER;
}
Speedometer::Speedometer(uint8_t pin, double diameter, uint16_t steps) { Speedometer::Speedometer(uint8_t pin, double diameter, uint16_t steps) {
this->init(pin, diameter, steps); this->diameter = diameter;
this->steps = steps;
this->pulseCounter = new Counter(pin);
this->pulseCounter->setFilterValue(1000); // ignore pulses less than 1000 x 2.5ns
this->pulseCounter->clear();
this->pulseCounter->resume();
Component::loopDelay = Speedometer::loopDelay;
clearAvgBuf();
} }
Speedometer::~Speedometer() { Speedometer::~Speedometer() {
delete[] this->buf;
delete this->pulseCounter; delete this->pulseCounter;
} }
@@ -58,7 +62,7 @@ void Speedometer::run() {
break; break;
} }
this->addValToBuf(static_cast<int16_t>(this->speed * CONVERSION_FACTOR)); this->addValToBuf(static_cast<int16_t>(this->speed * Speedometer::conversionFactor));
} }
void Speedometer::setDirection(Direction dir) { void Speedometer::setDirection(Direction dir) {
@@ -68,11 +72,6 @@ void Speedometer::setDirection(Direction dir) {
this->clearAvgBuf(); this->clearAvgBuf();
} }
void Speedometer::setNumOfValForAvg(uint8_t val) {
this->bufSize = val;
updateAvgBufSize();
}
void Speedometer::setEncFilter(uint16_t val) { void Speedometer::setEncFilter(uint16_t val) {
if (val > 1023) if (val > 1023)
val = 1023; val = 1023;
@@ -81,7 +80,7 @@ void Speedometer::setEncFilter(uint16_t val) {
double Speedometer::getAvgSpeed() const { double Speedometer::getAvgSpeed() const {
int16_t avg = this->calcAverage(); int16_t avg = this->calcAverage();
return (float)avg / CONVERSION_FACTOR; return (float)avg / Speedometer::conversionFactor;
} }
void Speedometer::calibrationMeasurementStart() { void Speedometer::calibrationMeasurementStart() {
@@ -101,25 +100,6 @@ uint16_t Speedometer::calibrationMeasurementStop() {
return res; return res;
} }
void Speedometer::init(uint8_t pin, double diameter, uint16_t steps) {
this->diameter = diameter;
this->steps = steps;
this->pulseCounter = new Counter(pin);
this->pulseCounter->setFilterValue(1000); // ignore pulses less than 1000 x 2.5ns
this->pulseCounter->clear();
this->pulseCounter->resume();
initAvgBuf();
}
void Speedometer::initAvgBuf() {
this->buf = new int16_t[bufSize];
for (uint8_t i = 0; i < bufSize; i++)
this->buf[i] = 0;
}
void Speedometer::clearAvgBuf() { void Speedometer::clearAvgBuf() {
for (uint8_t i = 0; i < bufSize; i++) for (uint8_t i = 0; i < bufSize; i++)
this->buf[i] = 0; this->buf[i] = 0;
@@ -133,11 +113,6 @@ void Speedometer::addValToBuf(int16_t val) {
bufPos = 0; bufPos = 0;
} }
void Speedometer::updateAvgBufSize() {
delete[] this->buf;
initAvgBuf();
}
int16_t Speedometer::calcAverage() const { int16_t Speedometer::calcAverage() const {
int16_t sum = 0; int16_t sum = 0;
for (int i = 0; i < this->bufSize; i++) for (int i = 0; i < this->bufSize; i++)
lib/Speedometer/speedometer.h
+5 -19
View File
@@ -19,19 +19,6 @@
#include "counter.h" #include "counter.h"
#include "component.h" #include "component.h"
/**
* @brief The default size of numbers to be taken in account for the average.
*
*/
#define BUFSIZE 5
#define CONVERSION_FACTOR 100
/**
* @brief Default value for min Millisseconds between each loop
* @see setDelay(uint8_t val)
*/
#define DELAY_SPEEDOMETER 30
/** /**
* @brief A class which use a encoder to calc the speed * @brief A class which use a encoder to calc the speed
* *
@@ -60,9 +47,7 @@ class Speedometer : public Component {
* @param pin Pin on the Esp from the encoder. * @param pin Pin on the Esp from the encoder.
* @param diameter Diameter of the wheel in meters. * @param diameter Diameter of the wheel in meters.
* @param steps Encodersteps for a complete wheel rotation. * @param steps Encodersteps for a complete wheel rotation.
* @param numOfValForAvg Number of last values ​​to be taken into account for the average.
*/ */
Speedometer(uint8_t pin, double diameter, uint16_t steps, uint8_t numOfValForAvg);
Speedometer(uint8_t pin, double diameter, uint16_t steps); Speedometer(uint8_t pin, double diameter, uint16_t steps);
~Speedometer(); ~Speedometer();
@@ -127,12 +112,14 @@ class Speedometer : public Component {
private: private:
void run() override; void run() override;
void init(uint8_t pin, double diameter, uint16_t steps); void init(uint8_t pin, double diameter, uint16_t steps);
void initAvgBuf();
void clearAvgBuf(); void clearAvgBuf();
void addValToBuf(int16_t val); void addValToBuf(int16_t val);
void updateAvgBufSize();
int16_t calcAverage() const; int16_t calcAverage() const;
static constexpr uint8_t loopDelay = 30;
static constexpr uint8_t bufSize = 5;
static constexpr uint8_t conversionFactor = 100;
Counter* pulseCounter; Counter* pulseCounter;
Direction currentDirection = Direction::None; Direction currentDirection = Direction::None;
@@ -142,11 +129,10 @@ class Speedometer : public Component {
double diameter; double diameter;
uint8_t printCounter = 0; uint8_t printCounter = 0;
uint8_t bufSize = BUFSIZE;
uint8_t bufPos = 0; uint8_t bufPos = 0;
uint16_t steps; uint16_t steps;
int16_t *buf = nullptr; int16_t buf[Speedometer::bufSize];
uint32_t lastMillisCalc = 0; uint32_t lastMillisCalc = 0;
}; };
src/SpecialMenus/driveModi/TestMode/menuTestMode.h
+3 -3
View File
@@ -8,8 +8,8 @@
* @copyright Copyright (c) 2022 * @copyright Copyright (c) 2022
* *
*/ */
#ifndef MENU_TEST_MODE #ifndef MENU_TEST_MODE_H
#define MENU_TEST_MODE #define MENU_TEST_MODE_H
#include "SpecialMenus/driveModi/menuDriveMode.h" #include "SpecialMenus/driveModi/menuDriveMode.h"
#include "driveModi/Modi/TestMode/testMode.h" #include "driveModi/Modi/TestMode/testMode.h"
@@ -96,4 +96,4 @@ class MenuTestModeWrapper : public MenuIntInputWrapper {
TestModeFunctionDouble testModeFunctionDouble = nullptr; TestModeFunctionDouble testModeFunctionDouble = nullptr;
}; };
#endif // MENU_TEST_MODE #endif // MENU_TEST_MODE_H
src/driveModi/driveManager.cpp
+1 -1
View File
@@ -20,7 +20,7 @@ DriveManager::DriveManager(MoveControl *moveControl, SPIClass *spiPort, const Co
this->input = input; this->input = input;
this->spiPort = spiPort; this->spiPort = spiPort;
this->navigation = new Navigation(spiPort, UBLOX_GNSS_SPI_CS); this->navigation = new Navigation(spiPort, PinNumbers::gnssSpiCs);
if (wifi) if (wifi)
this->navigation->initNtrip(NTRIP_HOST, NTRIP_PORT, NTRIP_MOUNT_POINT, NTRIP_USER, NTRIP_PASSWORD); this->navigation->initNtrip(NTRIP_HOST, NTRIP_PORT, NTRIP_MOUNT_POINT, NTRIP_USER, NTRIP_PASSWORD);
src/main.cpp
+3 -3
View File
@@ -64,7 +64,7 @@ void makeMenu(void);
void restart(void); void restart(void);
void receiveCallback (const uint8_t * mac, const uint8_t *incomingData, int len); void receiveCallback (const uint8_t * mac, const uint8_t *incomingData, int len);
void sendCallback (const uint8_t *mac_addr, esp_now_send_status_t status); void sendCallback (const uint8_t *mac_addr, esp_now_send_status_t status);
void lcdWrapperCallback (const char data[][DISPLAY_WRAPPER_ROWS], uint8_t lines, uint8_t rows); void lcdWrapperCallback (const char data[][LcdWrapper::totalRows], uint8_t lines, uint8_t rows);
void setup() { void setup() {
@@ -78,7 +78,7 @@ void setup() {
Serial.println(WiFi.macAddress()); Serial.println(WiFi.macAddress());
spiPort = new SPIClass(HSPI); spiPort = new SPIClass(HSPI);
spiPort->begin(UBLOX_GNSS_SPI_SCK, UBLOX_GNSS_SPI_CIPO, UBLOX_GNSS_SPI_COPI, UBLOX_GNSS_SPI_CS); spiPort->begin(PinNumbers::spiSck, PinNumbers::spiCipo, PinNumbers::spiCopi, PinNumbers::gnssSpiCs);
mainBattery = new Battery(35); mainBattery = new Battery(35);
controlPad = new ControlPad(); controlPad = new ControlPad();
@@ -289,7 +289,7 @@ void sendCallback (const uint8_t *mac_addr, esp_now_send_status_t status) {
std::cout << "sendCallback - Delivery Fail" << std::endl; std::cout << "sendCallback - Delivery Fail" << std::endl;
} }
void lcdWrapperCallback (const char data[][DISPLAY_WRAPPER_ROWS], uint8_t lines, uint8_t rows) { void lcdWrapperCallback (const char data[][LcdWrapper::totalRows], uint8_t lines, uint8_t rows) {
if (!controlPad->isControlPadConnected()) if (!controlPad->isControlPadConnected())
return; return;
src/moveControl.cpp
+16 -12
View File
@@ -14,30 +14,34 @@ MoveControl::MoveControl() {
this->loopDelay = 20; this->loopDelay = 20;
this->left_motor = new MotorControl(); this->left_motor = new MotorControl();
this->right_motor = new MotorControl(); this->right_motor = new MotorControl();
this->left_speedometer = new Speedometer(M_ENCODE_LEFT, WHEEL_DIAMETER, ENC_STEPS); this->left_speedometer = new Speedometer(PinNumbers::LeftMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps);
this->right_speedometer = new Speedometer(M_ENCODE_RIGHT, WHEEL_DIAMETER, ENC_STEPS); this->right_speedometer = new Speedometer(PinNumbers::RightMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps);
this->left_pid = new PID( &this->wheelspeed_left, this->left_pid = new PID( &this->wheelspeed_left,
&this->left_pid_out, &this->left_pid_out,
&this->wheelspeed_left_target, &this->wheelspeed_left_target,
PID_LEFT_P, PID_LEFT_I, PID_LEFT_D, Settings::Pid::Left::P,
Settings::Pid::Left::I,
Settings::Pid::Left::D,
DIRECT); DIRECT);
this->right_pid = new PID( &this->wheelspeed_right, this->right_pid = new PID( &this->wheelspeed_right,
&this->right_pid_out, &this->right_pid_out,
&this->wheelspeed_right_target, &this->wheelspeed_right_target,
PID_RIGHT_P, PID_RIGHT_I, PID_RIGHT_D, Settings::Pid::Right::P,
Settings::Pid::Right::I,
Settings::Pid::Right::D,
DIRECT); DIRECT);
this->left_pid->SetOutputLimits(PID_OUT_MIN, PID_OUT_MAX); this->left_pid->SetOutputLimits(Settings::Pid::outMin, Settings::Pid::outMax);
this->left_pid->SetSampleTime(PID_SAMPLETIME); this->left_pid->SetSampleTime(Settings::Pid::sampleTime);
this->left_pid->SetMode(AUTOMATIC); this->left_pid->SetMode(AUTOMATIC);
this->right_pid->SetOutputLimits(PID_OUT_MIN, PID_OUT_MAX); this->right_pid->SetOutputLimits(Settings::Pid::outMin, Settings::Pid::outMax);
this->right_pid->SetSampleTime(PID_SAMPLETIME); this->right_pid->SetSampleTime(Settings::Pid::sampleTime);
this->right_pid->SetMode(AUTOMATIC); this->right_pid->SetMode(AUTOMATIC);
this->left_motor->init(M_PWM_1, PWM_CHANNEL_M1, M_DIR_11, M_DIR_12); this->left_motor->init(PinNumbers::LeftMotor::pwm, PinNumbers::LeftMotor::pmwChannel, PinNumbers::LeftMotor::dir1, PinNumbers::LeftMotor::dir2);
this->right_motor->init(M_PWM_2, PWM_CHANNEL_M2, M_DIR_21, M_DIR_22); this->right_motor->init(PinNumbers::RightMotor::pwm, PinNumbers::RightMotor::pmwChannel, PinNumbers::RightMotor::dir1, PinNumbers::RightMotor::dir2);
this->addChildComponent(this->left_motor); this->addChildComponent(this->left_motor);
this->addChildComponent(this->right_motor); this->addChildComponent(this->right_motor);
@@ -165,8 +169,8 @@ void MoveControl::calcTargetWheelSpeed() {
// (1 / r) * b // (1 / r) * b
constexpr double B = 2.096518987; constexpr double B = 2.096518987;
this->wheelspeed_right_target = (A * this->x_speed + B * this->rotation_speed) * (WHEEL_DIAMETER / 2); this->wheelspeed_right_target = (A * this->x_speed + B * this->rotation_speed) * (Settings::wheelDiameter / 2);
this->wheelspeed_left_target = (A * this->x_speed + (-B) * this->rotation_speed) * (WHEEL_DIAMETER / 2); this->wheelspeed_left_target = (A * this->x_speed + (-B) * this->rotation_speed) * (Settings::wheelDiameter / 2);
} }
void MoveControl::regulateMotors() { void MoveControl::regulateMotors() {