304 lines
9.8 KiB
C++
304 lines
9.8 KiB
C++
/**
|
|
* @file main.cpp
|
|
* @author Alexander Klein (alex@kleiax.de)
|
|
* @brief The main file.
|
|
*
|
|
* Sets up Network stuff, PS3-Controller, Menu and Lcd
|
|
* Handles Controller Input
|
|
*
|
|
* @version 0.1
|
|
* @date 2022-02-15
|
|
*
|
|
* @copyright Copyright (c) 2022
|
|
*
|
|
*/
|
|
|
|
#include <Arduino.h>
|
|
|
|
#include "config.h"
|
|
|
|
#include <iostream>
|
|
#include <Wire.h>
|
|
#include <LiquidCrystal_I2C.h>
|
|
#include <SPI.h>
|
|
|
|
#include "Version.h"
|
|
|
|
#include "driveModi/driveManager.h"
|
|
#include "OutputBuf/outputBuf.h"
|
|
#include "LcdWrapper.h"
|
|
#include "moveControl.h"
|
|
#include "network.h"
|
|
#include "networkConfig.h"
|
|
#include "debugMqtt.h"
|
|
#include "battery.h"
|
|
#include "sensorData.h"
|
|
#include "debugTimes.h"
|
|
#include "controlPad.h"
|
|
|
|
#include "menu.h"
|
|
#include "menuAction.h"
|
|
#include "SpecialMenus/driveModi/ManualDrive/menuManualDrive.h"
|
|
#include "SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h"
|
|
#include "SpecialMenus/driveModi/Autopilot/menuAutopilot.h"
|
|
#include "SpecialMenus/driveModi/TestMode/menuTestMode.h"
|
|
#include "SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h"
|
|
#include "SpecialMenus/Systeminformation/menuSysteminformation.h"
|
|
#include "SpecialMenus/PID/menuPidSettings.h"
|
|
#include "SpecialMenus/Route/menuRoute.h"
|
|
#include "SpecialMenus/SensorData/menuSensorData.h"
|
|
|
|
MoveControl moveController;
|
|
Network* network;
|
|
DriveManager* driveManager;
|
|
Menu* main_m;
|
|
LiquidCrystal_I2C* lcd;
|
|
LcdWrapper* lcdWrapper;
|
|
OutputBuf* outputBuf;
|
|
DebugMqtt* debugMqtt = nullptr;
|
|
Battery* mainBattery;
|
|
SPIClass* spiPort;
|
|
SensorData* sensorData;
|
|
ControlPad* controlPad;
|
|
|
|
bool wifiIsActive;
|
|
|
|
void i2cScanner(void);
|
|
void makeMenu(void);
|
|
void restart(void);
|
|
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 lcdWrapperCallback (const char data[][LcdWrapper::totalRows], uint8_t lines, uint8_t rows);
|
|
NetworkAdresses setIPs(void);
|
|
|
|
|
|
void setup() {
|
|
Serial.begin(115200);
|
|
// DebugTimes::setConsolOutput(true);
|
|
DebugTimes setupTime;
|
|
|
|
spiPort = new SPIClass(HSPI);
|
|
spiPort->begin(PinNumbers::spiSck, PinNumbers::spiCipo, PinNumbers::spiCopi, PinNumbers::gnssSpiCs);
|
|
|
|
mainBattery = new Battery(35);
|
|
controlPad = new ControlPad();
|
|
|
|
Wire.begin(21, 19);
|
|
Wire.setClock(400000);
|
|
i2cScanner();
|
|
lcd = new LiquidCrystal_I2C(0x3F,16,2);
|
|
lcd->init();
|
|
lcd->clear();
|
|
lcd->noBacklight();
|
|
|
|
network = new Network(NetworkConfig::ssid, NetworkConfig::password, setIPs());
|
|
network->activateEspNow(receiveCallback, sendCallback);
|
|
if (NetworkConfig::mqtt) {
|
|
if (network->activateMqtt(MqttConfig::user, MqttConfig::password)) {
|
|
DebugMqtt::init(network->getMqttClient(), Loglevel::debug);
|
|
debugMqtt = new DebugMqtt("Console");
|
|
outputBuf = new OutputBuf(debugMqtt);
|
|
outputBuf->activateMqtt(true);
|
|
} else {
|
|
outputBuf = new OutputBuf();
|
|
}
|
|
} else
|
|
outputBuf = new OutputBuf();
|
|
std::cout.rdbuf(outputBuf);
|
|
|
|
std::cout << "Welcome to Kleiax-Rover" << std::endl;
|
|
std::cout << "Project verion: " << VERSION << std::endl;
|
|
std::cout << "Build timestamp: " << BUILD_TIMESTAMP << std::endl;
|
|
std::cout << "All actions from the main program run on Core -> " << xPortGetCoreID() << std::endl;
|
|
|
|
sensorData = new SensorData();
|
|
sensorData->enableGnss(spiPort, PinNumbers::gnssSpiCs);
|
|
sensorData->enableRealCompass();
|
|
// sensorData->enableGyroskop();
|
|
driveManager = new DriveManager(&moveController, sensorData, controlPad->getControlPadDataPtr());
|
|
|
|
char wifiIndicator = 'X';
|
|
if (network->isWifiConnected())
|
|
wifiIndicator = '-';
|
|
lcd->backlight();
|
|
lcd->printf("%c Kleiax-Rover %c", wifiIndicator, wifiIndicator);
|
|
lcd->setCursor(0, 1);
|
|
lcd->printf("WiFi channel %u", Network::getCurrentChannel());
|
|
lcdWrapper = new LcdWrapper(lcd);
|
|
lcdWrapper->setCallback(lcdWrapperCallback);
|
|
|
|
makeMenu();
|
|
|
|
setupTime.stopConsol("Setup");
|
|
}
|
|
|
|
void loop() {
|
|
network->loop();
|
|
sensorData->loop();
|
|
driveManager->loop();
|
|
controlPad->loop();
|
|
main_m->update();
|
|
lcdWrapper->loop();
|
|
mainBattery->loop();
|
|
|
|
// new Value ervery 0.5s
|
|
if (mainBattery->isNewValue()) {
|
|
static uint8_t batteryLowCounter = 0;
|
|
if (mainBattery->isBatteryLow(10))
|
|
batteryLowCounter++;
|
|
else
|
|
batteryLowCounter = 0;
|
|
|
|
if (batteryLowCounter >= 10) {
|
|
moveController.emergencyStop();
|
|
uint16_t voltage = (uint16_t) (mainBattery->getBatteryVoltage() * 100);
|
|
lcd->setCursor(0, 0);
|
|
lcd->printf("Low Battery: %u", voltage);
|
|
lcd->setCursor(0, 1);
|
|
lcd->print("Please turn off.");
|
|
|
|
while (true);
|
|
}
|
|
}
|
|
}
|
|
|
|
void i2cScanner() {
|
|
std::cout << "\nI2C Scanner" << std::endl;
|
|
|
|
byte error, address;
|
|
int nDevices;
|
|
std::cout << "Scanning..." << std::endl;
|
|
nDevices = 0;
|
|
for(address = 1; address < 127; address++ ) {
|
|
Wire.beginTransmission(address);
|
|
error = Wire.endTransmission();
|
|
if (error == 0) {
|
|
std::cout << "I2C device found at address 0x";
|
|
if (address<16)
|
|
std::cout << "0";
|
|
std::cout << std::hex << (int) address << std::dec << std::endl;
|
|
// std::cout << (int) address << std::endl;
|
|
nDevices++;
|
|
}
|
|
else if (error==4) {
|
|
std::cout << "Unknow error at address 0x";
|
|
if (address<16)
|
|
std::cout << "0";
|
|
std::cout << std::hex << (int) address << std::dec << std::endl;
|
|
}
|
|
}
|
|
if (nDevices == 0)
|
|
std::cout << "No I2C devices found\n" << std::endl;
|
|
else
|
|
std::cout << "done\n" << std::endl;
|
|
}
|
|
|
|
void makeMenu() {
|
|
auto dummy = []() {
|
|
std::cout << "Dummy in Action" <<std::endl;
|
|
};
|
|
|
|
// Create Menu
|
|
main_m = new Menu();
|
|
main_m->setLcd(lcdWrapper);
|
|
Menu* mode_m = new Menu();
|
|
Menu* set_m = new Menu();
|
|
Menu* pid_m = new Menu();
|
|
MenuIntInput* pidl_m = new MenuIntInput(3, new MenuPidSettings(moveController.getPID(0)));
|
|
MenuIntInput* pidr_m = new MenuIntInput(3, new MenuPidSettings(moveController.getPID(1)));
|
|
MenuManualControl* man_m = new MenuManualControl(driveManager);
|
|
MenuCaptureRoute* cap_m = new MenuCaptureRoute(driveManager);
|
|
MenuAutopilot* auto_m = new MenuAutopilot(driveManager);
|
|
MenuTestMode* testM_m = new MenuTestMode(driveManager);
|
|
MenuCalibrateCompass* comp_m = new MenuCalibrateCompass(driveManager);
|
|
MenuSysteminformation* sys_m = new MenuSysteminformation(mainBattery);
|
|
MenuSensorData* sen_m = new MenuSensorData(sensorData);
|
|
//TODO: Wie bekommt jeder die dumme Route?
|
|
MenuRoute* rout_m = new MenuRoute(new Route());
|
|
|
|
auto_m->setUpdateDelay(1000);
|
|
sys_m->setUpdateDelay(1500);
|
|
sen_m->setUpdateDelay(500);
|
|
|
|
// Entry for the main menu
|
|
main_m->addEntry(new MenuAction("Mode", mode_m));
|
|
main_m->addEntry(new MenuAction("Sensor", sen_m));
|
|
main_m->addEntry(new MenuAction("Route", rout_m));
|
|
main_m->addEntry(new MenuAction("Settings", set_m));
|
|
main_m->addEntry(new MenuAction("Systeminfo", sys_m));
|
|
main_m->addEntry(new MenuAction("Restart", restart));
|
|
|
|
// Entry for the mode Menu
|
|
mode_m->addEntry(new MenuAction("Manual Control", man_m));
|
|
mode_m->addEntry(new MenuAction("Capture Route", cap_m));
|
|
mode_m->addEntry(new MenuAction("Autopilot", auto_m));
|
|
mode_m->addEntry(new MenuAction("Gauge Compass", comp_m));
|
|
mode_m->addEntry(new MenuAction("Test Mode", testM_m));
|
|
mode_m->addEntry(new MenuAction("Consol Control", dummy));
|
|
|
|
// Entry for the setting menu
|
|
set_m->addEntry(new MenuAction("PID", pid_m));
|
|
set_m->addEntry(new MenuAction("Speed", dummy));
|
|
set_m->addEntry(new MenuAction("Distance", dummy));
|
|
set_m->addEntry(new MenuAction("WiFi", dummy));
|
|
|
|
// Entry for the PID Menu
|
|
pid_m->addEntry(new MenuAction("Left", pidl_m));
|
|
pid_m->addEntry(new MenuAction("Right", pidr_m));
|
|
|
|
// Other config
|
|
pidl_m->setMinMax(0, UINT8_MAX);
|
|
pidl_m->setEntry(0, "P-Part", (uint8_t) moveController.getPID(0)->GetKp());
|
|
pidl_m->setEntry(1, "I-Part", (uint8_t) moveController.getPID(0)->GetKi());
|
|
pidl_m->setEntry(2, "D-Part", (uint8_t) moveController.getPID(0)->GetKd());
|
|
|
|
pidr_m->setMinMax(0, UINT8_MAX);
|
|
pidr_m->setEntry(0, "P-Part", (uint8_t) moveController.getPID(1)->GetKp());
|
|
pidr_m->setEntry(1, "I-Part", (uint8_t) moveController.getPID(1)->GetKi());
|
|
pidr_m->setEntry(2, "D-Part", (uint8_t) moveController.getPID(1)->GetKd());
|
|
|
|
controlPad->setMenuControl(main_m);
|
|
}
|
|
|
|
void restart(void) {
|
|
lcdWrapper->clear();
|
|
lcdWrapper->setCursor(0, 0);
|
|
lcdWrapper->print("Rebooting ...");
|
|
ESP.restart();
|
|
}
|
|
|
|
void receiveCallback (const uint8_t * mac, const uint8_t *incomingData, int len) {
|
|
if (len != sizeof(ControlPadInput))
|
|
return;
|
|
controlPad->insertData(incomingData);
|
|
}
|
|
|
|
void sendCallback (const uint8_t *mac_addr, esp_now_send_status_t status) {
|
|
if (status != ESP_NOW_SEND_SUCCESS)
|
|
std::cout << "sendCallback - Delivery Fail" << std::endl;
|
|
}
|
|
|
|
void lcdWrapperCallback (const char data[][LcdWrapper::totalRows], uint8_t lines, uint8_t rows) {
|
|
if (!controlPad->isControlPadConnected())
|
|
return;
|
|
|
|
esp_err_t result = esp_now_send(network->getBroadcastAddress(), (uint8_t *) data, lines * rows);
|
|
|
|
if (result != ESP_OK)
|
|
Serial.println("Error sending the data");
|
|
}
|
|
|
|
NetworkAdresses setIPs() {
|
|
NetworkAdresses adresses;
|
|
adresses.localIP.fromString(NetworkConfig::ip);
|
|
adresses.subnet.fromString(NetworkConfig::subnet);
|
|
adresses.gateway.fromString(NetworkConfig::gateway);
|
|
adresses.dnsServer.fromString(NetworkConfig::dns);
|
|
if (NetworkConfig::mqtt) {
|
|
adresses.mqttServer.fromString(MqttConfig::server);
|
|
adresses.mqttPort = MqttConfig::port;
|
|
}
|
|
|
|
return adresses;
|
|
}
|