a lot of bullshit
This commit is contained in:
+11
-170
@@ -1,208 +1,49 @@
|
||||
#include <Arduino.h>
|
||||
#include <Ps3Controller.h>
|
||||
#include <WiFi.h>
|
||||
#include <PubSubClient.h>
|
||||
#include <time.h>
|
||||
|
||||
#include "config.h"
|
||||
|
||||
#include "motorControl.h"
|
||||
#include "speedometer.h"
|
||||
#include "driveModi/autopilot.h"
|
||||
#include "driveModi/captureRoute.h"
|
||||
#include "driveModi/manualControl.h"
|
||||
#include "driveModi/consolControl.h"
|
||||
|
||||
#include "driveModi/Modi/ManualControl/manualControl.h"
|
||||
#include "driveModi/driveManager.h"
|
||||
#include "moveControl.h"
|
||||
#include "route.h"
|
||||
#include "debugMqtt.h"
|
||||
#include "debugTimes.h"
|
||||
#include "network.h"
|
||||
|
||||
void callbackControllerAction();
|
||||
void callbackControllerConnect();
|
||||
void controllerPrintBattery();
|
||||
|
||||
bool reconnectMqtt();
|
||||
|
||||
MotorControl left_motor;
|
||||
MotorControl right_motor;
|
||||
Speedometer speedometer_left;
|
||||
Speedometer speedometer_right;
|
||||
MoveControl moveController;
|
||||
Route route;
|
||||
DebugMqtt debugger("main");
|
||||
|
||||
ManualControl manualControl;
|
||||
CaptureRoute captureRoute;
|
||||
Autopilot autopilot;
|
||||
ConsolControl consolControl;
|
||||
|
||||
IPAddress local_IP(WLAN_IP);
|
||||
IPAddress gateway(WLAN_GATEWAY);
|
||||
IPAddress subnet(WLAN_SUBNETMASK);
|
||||
IPAddress mqtt_server(MQTT_SERVER);
|
||||
|
||||
WiFiClient wifi_client;
|
||||
PubSubClient mqtt_client(wifi_client);
|
||||
|
||||
uint64_t lastReconnectAttempt = 0;
|
||||
int controller_battery = -1;
|
||||
enum DriveMode {autopilot_e, captureRoute_e, manualControl_e, consolControl_e};
|
||||
DriveMode driveMode = manualControl_e;
|
||||
|
||||
DriveManager driveManager(&moveController);
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
|
||||
// Configures static IP address
|
||||
if (!WiFi.config(local_IP, gateway, subnet)) {
|
||||
Serial.println("STA Failed to configure");
|
||||
}
|
||||
|
||||
// Connect to Wi-Fi network with SSID and password
|
||||
Serial.print("Connecting to ");
|
||||
Serial.println(WLAN_SSID);
|
||||
WiFi.begin(WLAN_SSID, WLAN_PASSWORD);
|
||||
while (WiFi.status() != WL_CONNECTED) {
|
||||
delay(500);
|
||||
Serial.print(".");
|
||||
}
|
||||
|
||||
// Print local IP address and start web server
|
||||
Serial.println("");
|
||||
Serial.println("WiFi connected.");
|
||||
Serial.println("IP address: ");
|
||||
Serial.println(WiFi.localIP());
|
||||
|
||||
mqtt_client.setServer(mqtt_server, MQTT_PORT);
|
||||
// mqtt_client.setServer(MQTT_SERVER, MQTT_PORT);
|
||||
mqtt_client.setSocketTimeout(1);
|
||||
reconnectMqtt();
|
||||
|
||||
//FIXME: make it from config.h
|
||||
configTime(3600, 3600, "2.de.pool.ntp.org");
|
||||
|
||||
DebugMqtt::init(&mqtt_client, Loglevel::debug);
|
||||
setIps();
|
||||
connectWifi();
|
||||
setupMQTT();
|
||||
|
||||
Ps3.attach(callbackControllerAction);
|
||||
Ps3.attachOnConnect(callbackControllerConnect);
|
||||
// Ps3.attachOnDisconnect(callbackControllerDisconnect);
|
||||
Serial.println("\nReady to connect a PS3 Controller... \n");
|
||||
Ps3.begin();
|
||||
|
||||
// Init hardware
|
||||
left_motor.init(M_PWM_1, PWM_CHANNEL_M1, M_DIR_11, M_DIR_12);
|
||||
right_motor.init(M_PWM_2, PWM_CHANNEL_M2, M_DIR_21, M_DIR_22);
|
||||
|
||||
speedometer_left.init(M_ENCODE_1A, M_ENCODE_1B, WHEEL_DIAMETER, ENC_STEPS);
|
||||
speedometer_right.init(M_ENCODE_2A, M_ENCODE_2B, WHEEL_DIAMETER, ENC_STEPS);
|
||||
|
||||
moveController.init(&left_motor, &right_motor, &speedometer_left, &speedometer_right);
|
||||
|
||||
// Init driveModi
|
||||
manualControl.init(&moveController, &left_motor, &right_motor);
|
||||
captureRoute.init(&route, &moveController, &left_motor, &right_motor);
|
||||
autopilot.init(&route, &moveController);
|
||||
consolControl.init(&moveController);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
|
||||
|
||||
if (!mqtt_client.connected()) {
|
||||
long now = millis();
|
||||
if (now - lastReconnectAttempt > MQTT_TIME_RECONNECT) {
|
||||
lastReconnectAttempt = now;
|
||||
// Attempt to reconnect
|
||||
if (reconnectMqtt()) {
|
||||
lastReconnectAttempt = 0;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// Client connected
|
||||
mqtt_client.loop();
|
||||
}
|
||||
|
||||
left_motor.runMotorControl();
|
||||
right_motor.runMotorControl();
|
||||
|
||||
speedometer_left.runSpeedometer();
|
||||
speedometer_right.runSpeedometer();
|
||||
// TODO: Auskommentiert weil macht vielleicht komische Sachen
|
||||
// route.runRoute();
|
||||
|
||||
switch (driveMode) {
|
||||
case manualControl_e:
|
||||
manualControl.runManualControl();
|
||||
break;
|
||||
|
||||
case captureRoute_e:
|
||||
captureRoute.runCaptureRoute();
|
||||
break;
|
||||
|
||||
case autopilot_e:
|
||||
autopilot.runAutopilot();
|
||||
break;
|
||||
|
||||
case consolControl_e:
|
||||
consolControl.run();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
bool reconnectMqtt() {
|
||||
// Create a random client ID
|
||||
String clientId = "ESP32Rover-";
|
||||
clientId += String(random(0xffff), HEX);
|
||||
#ifdef MQTT_AUTH
|
||||
if (mqtt_client.connect(clientId.c_str(), MQTT_USER, MQTT_PASSWORD)) {
|
||||
#endif //MQTT_AUTH
|
||||
#ifndef MQTT_AUTH
|
||||
if (mqtt_client.connect(clientId.c_str())) {
|
||||
#endif //MQTT_AUTH
|
||||
// Once connected, publish an announcement...
|
||||
mqtt_client.publish("Rover/Info", "Connected to Mqtt-Broker");
|
||||
}
|
||||
return mqtt_client.connected();
|
||||
checkMQTT();
|
||||
driveManager.loop();
|
||||
}
|
||||
|
||||
void callbackControllerAction() {
|
||||
if (Ps3.event.button_down.ps) {
|
||||
switch (driveMode) {
|
||||
case manualControl_e:
|
||||
driveMode = DriveMode::captureRoute_e;
|
||||
debugger.sendMsg(Loglevel::info, "New driveMode = CaptureRoute");
|
||||
break;
|
||||
|
||||
case captureRoute_e:
|
||||
driveMode = DriveMode::autopilot_e;
|
||||
debugger.sendMsg(Loglevel::info, "New driveMode = Autopilot");
|
||||
break;
|
||||
|
||||
case autopilot_e:
|
||||
driveMode = DriveMode::consolControl_e;
|
||||
debugger.sendMsg(Loglevel::info, "New driveMode = ConsolControl");
|
||||
break;
|
||||
|
||||
case consolControl_e:
|
||||
driveMode = DriveMode::manualControl_e;
|
||||
debugger.sendMsg(Loglevel::info, "New driveMode = ManualControl");
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void callbackControllerConnect() {
|
||||
Serial.println("Controller connected to ESP32");
|
||||
debugger.sendMsg(Loglevel::info, "Controller connected to ESP32");
|
||||
}
|
||||
|
||||
void controllerPrintBattery() {
|
||||
static uint8_t controller_battery = -1;
|
||||
if( controller_battery != Ps3.data.status.battery ){
|
||||
controller_battery = Ps3.data.status.battery;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user