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

add menuTest

Browse Source
This commit is contained in:
kleiax
2023-01-31 08:21:04 +01:00
parent c63428412f
commit d96f382702
7 changed files with 177 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/Navigation/navigation.cpp
+3 -5
View File
@@ -66,15 +66,13 @@ void Navigation::init(Route* route) {
Wire.write(0x01);
Wire.endTransmission();
this->compass->setMode(0x01,0x0C,0x10,0X00);
this->compass->setCalibration(-1903, 367, -893, 1468, -1110, 310);
}
Navigation::~Navigation() {
delete this->gps;
if (this->isNtripInit)
delete this->compass;
delete this->ntripClient;
if (this->route)
delete this->route;
}
@@ -130,7 +128,7 @@ CourseCorrection Navigation::getCourseCorrection() {
return courseCorrection;
// Check if I need a new Point
if (distance < MIN_DISTANCE_BETWEEN_POINTS) {
if (distance < MIN_DISTANCE_TO_REACH_POINT) {
bool goOn = this->nextPoint();
if (!goOn) {
this->navigationFinished = true;
lib/Navigation/navigation.h
+1
View File
@@ -29,6 +29,7 @@
*
*/
#define MIN_DISTANCE_BETWEEN_POINTS 0.1
#define MIN_DISTANCE_TO_REACH_POINT 3 // TODO: Only for testing!
#define AZIMUTH_UPDATE_DELAY 20
/**
lib/Speedometer/speedometer.cpp
+34 -15
View File
@@ -1,7 +1,7 @@
/**
* @file speedometer.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief Implemention of the class speedometer.h
* @brief Implementation of the class speedometer.h
* @see speedometer.h
* @version 0.1
* @date 2021-12-13
@@ -12,6 +12,9 @@
#include "speedometer.h"
Speedometer::Speedometer() {}
Speedometer::~Speedometer() {
delete[] this->buf;
}
void Speedometer::init(uint8_t pinA, uint8_t pinB, double diameter, uint16_t steps, uint8_t numOfValForAvg) {
this->init(pinA, pinB, diameter, steps);
@@ -30,13 +33,16 @@ void Speedometer::init(uint8_t pinA, uint8_t pinB, double diameter, uint16_t ste
}
uint16_t Speedometer::loop() {
if (this->calibrationRunning)
return -1;
uint32_t time = millis();
uint16_t elapsed_time = time - this->last_millis_loop;
//Cancel if delay is not reached
if (elapsed_time < delay) {
//Cancel if delayLoop is not reached
if (elapsed_time < this->delayLoop)
return elapsed_time;
}
runSpeedometer();
this->last_millis_loop = time;
return elapsed_time;
@@ -50,7 +56,7 @@ void Speedometer::runSpeedometer() {
int16_t count = encoder.getCount();
this->addValToBuf(count);
encoder.clearCount();
this->encoder.clearCount();
uint16_t count_abs = abs(this->calcAverage()); // Absolute time in milliseconds
double n = (double)count_abs / steps; // Wheel revolutions in absolute time
@@ -64,6 +70,8 @@ void Speedometer::runSpeedometer() {
} else {
this->speed = 0;
}
std::cout << "Speedometer::runSpeedometer speed: " << (int) this->speed << std::endl;
}
void Speedometer::setNumOfValForAvg(uint8_t val) {
@@ -73,18 +81,31 @@ void Speedometer::setNumOfValForAvg(uint8_t val) {
}
void Speedometer::setEncFilter(uint16_t val) {
if (val > 1023) val = 1023;
if (val > 1023)
val = 1023;
this->encoder.setFilter(val);
}
void Speedometer::setDelay(uint8_t delay) {
this->delay = delay;
void Speedometer::setDelay(uint8_t delayLoop) {
this->delayLoop = delayLoop;
}
double Speedometer::getSpeed() {
return this->speed;
}
void Speedometer::calibrationMeasurementStart() {
this->calibrationRunning = true;
this->encoder.clearCount();
}
uint16_t Speedometer::calibrationMeasurementStop() {
this->calibrationRunning = false;
uint16_t res = abs(this->encoder.getCount());
this->encoder.clearCount();
return res;
}
void Speedometer::initAvgBuf() {
this->buf = new int16_t[bufSize];
for (uint8_t i = 0; i < bufSize; i++)
@@ -92,14 +113,12 @@ void Speedometer::initAvgBuf() {
}
void Speedometer::addValToBuf(int16_t val) {
static uint8_t bufPos = 0;
this->buf[bufPos] = val;
bufPos++;
this->buf[this->bufPos] = val;
this->bufPos++;
if (bufPos == bufSize) {
if (bufPos == bufSize)
bufPos = 0;
}
}
void Speedometer::updateAvgBufSize() {
delete[] this->buf;
@@ -108,7 +127,7 @@ void Speedometer::updateAvgBufSize() {
int16_t Speedometer::calcAverage() {
int16_t sum = 0;
for (int i = 0; i < bufSize; i++)
for (int i = 0; i < this->bufSize; i++)
sum += this->buf[i];
return sum / bufSize;
return sum / this->bufSize;
}
lib/Speedometer/speedometer.h
+13 -6
View File
@@ -14,6 +14,7 @@
#include <Arduino.h>
#include <cstdint>
#include <iostream>
#include <ESP32Encoder.h>
/**
@@ -39,6 +40,7 @@
class Speedometer {
public:
Speedometer();
~Speedometer();
/**
* @brief Initialize the speedometer
@@ -55,7 +57,7 @@ class Speedometer {
/**
* @brief Calls runSpeedometer() to update all Values.
*
* This function should be called every mainloop. If the delay is not reached, than the
* This function should be called every mainloop. If the delayLoop is not reached, than the
* functions returns immediately.
* @see runSpeedometer()
* @see DELAY_SPEEDOMETER
@@ -85,11 +87,11 @@ class Speedometer {
void setEncFilter(uint16_t val);
/**
* @brief Set the min delay between each loop
* @brief Set the min delayLoop between each loop
*
* @param delay time in Milliseconds
* @param delayLoop time in Milliseconds
*/
void setDelay(uint8_t delay);
void setDelay(uint8_t delayLoop);
/**
* @brief Get the calculated speed of the Wheel
@@ -98,6 +100,9 @@ class Speedometer {
*/
double getSpeed();
void calibrationMeasurementStart();
uint16_t calibrationMeasurementStop();
private:
void initAvgBuf();
@@ -108,15 +113,17 @@ class Speedometer {
ESP32Encoder encoder;
bool isInit = false;
bool calibrationRunning = false;
double speed = 0;
double diameter;
uint8_t bufSize = BUFSIZE;
uint8_t delayLoop = DELAY_SPEEDOMETER;
uint8_t bufPos = 0;
uint16_t steps;
uint8_t delay = DELAY_SPEEDOMETER;
int16_t *buf;
int16_t *buf = nullptr;
uint32_t last_millis_loop = 0;
uint32_t last_millis_calc = 0;
src/SpecialMenus/Test/menuTest.cpp
+78
View File
@@ -0,0 +1,78 @@
/**
* @file menuTest.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @version 0.1
* @date 2023-01-24
*
* @copyright Copyright (c) 2023
*
*/
#include "menuTest.h"
MenuTest::MenuTest(Speedometer* speedometer) {
this->speedometer = speedometer;
}
void MenuTest::printMenu() {
switch (this->state) {
case State::Off :
this->print("Press Yes (O)", "to start");
break;
case State::Running :
this->print("Running, Yes (O)", "to stop");
break;
case State::Finished : {
String res = "";
res.concat(this->result);
this->print("Result: ", res);
}
break;
default:
this->print("Error in", "MenuTest.cpp");
break;
}
}
void MenuTest::left() {
this->no();
}
void MenuTest::no() {
if (this->state == State::Running)
speedometer->calibrationMeasurementStop();
this->state = State::Off;
this->parentMenu->printMenu();
}
void MenuTest::yes() {
switch (this->state) {
case State::Off :
this->state = State::Running;
this->speedometer->calibrationMeasurementStart();
this->printMenu();
break;
case State::Running :
this->state = State::Finished;
this->result = this->speedometer->calibrationMeasurementStop();
this->printMenu();
break;
case State::Finished :
this->state = State::Running;
this->speedometer->calibrationMeasurementStart();
this->printMenu();
break;
default:
break;
}
}
src/SpecialMenus/Test/menuTest.h
+37
View File
@@ -0,0 +1,37 @@
/**
* @file menuTest.h
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @version 0.1
* @date 2023-01-24
*
* @copyright Copyright (c) 2023
*
*/
#pragma once
#include "menuControl.h"
#include "speedometer.h"
class MenuTest : public MenuControl {
public:
enum State {
Off,
Running,
Finished
};
MenuTest(Speedometer* speedometer);
void printMenu() override;
void left() override;
void no() override;
void yes() override;
private:
Speedometer* speedometer;
State state = State::Off;
uint16_t result = 0;
};
src/main.cpp
+8 -3
View File
@@ -44,6 +44,7 @@
#include "SpecialMenus/PID/menuPidSettings.h"
#include "SpecialMenus/Route/menuRoute.h"
#include "SpecialMenus/GPS/menuGPS.h"
#include "SpecialMenus/Test/menuTest.h"
MoveControl moveController;
DriveManager* driveManager;
@@ -270,10 +271,11 @@ void makeMenu() {
MenuManualControl* man_m = new MenuManualControl(driveManager);
MenuCaptureRoute* cap_m = new MenuCaptureRoute(driveManager);
MenuAutopilot* auto_m = new MenuAutopilot(driveManager);
MenuTestMode* test_m = new MenuTestMode(driveManager);
MenuTestMode* testM_m = new MenuTestMode(driveManager);
MenuGPS* gps_m = new MenuGPS(driveManager);
MenuSysteminformation* sys_m = new MenuSysteminformation(mainBattery, gamepad);
MenuRoute* rout_m = new MenuRoute(driveManager->getNavigation()->getRoute());
MenuTest* test_m = new MenuTest(moveController.getSpeedometerLeft());
// Set Menus on LCD
main_m->setLcd(lcdWrapper);
@@ -284,10 +286,11 @@ void makeMenu() {
man_m->setLcd(lcdWrapper);
cap_m->setLcd(lcdWrapper);
auto_m->setLcd(lcdWrapper);
test_m->setLcd(lcdWrapper);
testM_m->setLcd(lcdWrapper);
gps_m->setLcd(lcdWrapper);
sys_m->setLcd(lcdWrapper);
rout_m->setLcd(lcdWrapper);
test_m->setLcd(lcdWrapper);
// Entry for the main menu
@@ -298,11 +301,13 @@ void makeMenu() {
MenuAction* contr_e = new MenuAction("Remove PS3", disconnectController);
MenuAction* sys_e = new MenuAction("Systeminfo", sys_m);
MenuAction* rout_e = new MenuAction("Route", rout_m);
MenuAction* test_e = new MenuAction("Test", test_m);
main_m->addEntry(mode_e);
main_m->addEntry(gps_e);
main_m->addEntry(rout_e);
main_m->addEntry(pid_e);
main_m->addEntry(sys_e);
main_m->addEntry(test_e);
main_m->addEntry(contr_e);
main_m->addEntry(restart_e);
@@ -311,7 +316,7 @@ void makeMenu() {
MenuAction* captureRoute_e = new MenuAction("Capture Route", cap_m);
MenuAction* consolControl_e = new MenuAction("Consol Control", dummy);
MenuAction* manualControl_e = new MenuAction("Manual Control", man_m);
MenuAction* testMode_e = new MenuAction("Test Mode", test_m);
MenuAction* testMode_e = new MenuAction("Test Mode", testM_m);
mode_m->addEntry(autopilot_e);
mode_m->addEntry(captureRoute_e);
mode_m->addEntry(manualControl_e);