140 lines
4.2 KiB
C++
140 lines
4.2 KiB
C++
#include <Arduino.h>
|
|
#include <Ps3Controller.h>
|
|
#include <WiFi.h>
|
|
#include <PubSubClient.h>
|
|
#include <time.h>
|
|
|
|
#include "config.h"
|
|
#include "hardware/motorControl.h"
|
|
#include "moveControl.h"
|
|
#include "hardware/speedometer.h"
|
|
#include "debugMqtt.h"
|
|
|
|
void callbackControllerAction();
|
|
void callbackControllerConnect();
|
|
void controllerPrintBattery();
|
|
|
|
void reconnectMqtt();
|
|
|
|
MotorControl left_motor;
|
|
MotorControl right_motor;
|
|
Speedometer speedometer_left;
|
|
Speedometer speedometer_right;
|
|
MoveControl moveController;
|
|
DebugMqtt debugger("main");
|
|
|
|
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);
|
|
|
|
int controller_battery = -1;
|
|
|
|
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("Kleiax2", "Punica-699");
|
|
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);
|
|
reconnectMqtt();
|
|
|
|
configTime(3600, 3600, "192.168.1.2");
|
|
|
|
DebugMqtt::init(&mqtt_client, Loglevel::debug);
|
|
DebugMqtt::initRealMillis();
|
|
|
|
Ps3.attach(callbackControllerAction);
|
|
Ps3.attachOnConnect(callbackControllerConnect);
|
|
Ps3.attachOnDisconnect(callbackControllerDisconnect);
|
|
Serial.println("\nReady to connect a PS3 Controller... \n");
|
|
Ps3.begin();
|
|
|
|
left_motor.init(M_PWM_1, PWM_CHANNEL_M1, M_DIR_11, M_DIR_12, "left");
|
|
right_motor.init(M_PWM_2, PWM_CHANNEL_M2, M_DIR_21, M_DIR_22, "right");
|
|
|
|
speedometer_left.init(M_ENCODE_1A, M_ENCODE_1B, "left");
|
|
speedometer_right.init(M_ENCODE_2A, M_ENCODE_2B, "right");
|
|
|
|
moveController.init(&left_motor, &right_motor, &speedometer_left, &speedometer_right);
|
|
}
|
|
|
|
void loop() {
|
|
if (!mqtt_client.connected())
|
|
reconnectMqtt();
|
|
mqtt_client.loop();
|
|
|
|
left_motor.runMotorControl();
|
|
right_motor.runMotorControl();
|
|
speedometer_left.runSpeedometer();
|
|
speedometer_right.runSpeedometer();
|
|
}
|
|
|
|
void reconnectMqtt() {
|
|
// Loop until reconnection
|
|
while (!mqtt_client.connected()) {
|
|
Serial.print("Attempting MQTT connection...");
|
|
// Create a random client ID
|
|
String clientId = "ESP32Rover-";
|
|
clientId += String(random(0xffff), HEX);
|
|
// Attempt to connect
|
|
if (mqtt_client.connect(clientId.c_str())) {
|
|
Serial.println("connected");
|
|
// Once connected, publish an announcement...
|
|
mqtt_client.publish("Rover/Info", "Connected to Mqtt-Broker");
|
|
} else {
|
|
Serial.print("failed, rc=");
|
|
Serial.print(mqtt_client.state());
|
|
Serial.println(" try again in 5 seconds");
|
|
// Wait 5 seconds before retrying
|
|
delay(5000);
|
|
}
|
|
}
|
|
}
|
|
|
|
void callbackControllerAction() {
|
|
if (Ps3.event.button_down.start) {
|
|
|
|
}
|
|
}
|
|
|
|
void callbackControllerConnect() {
|
|
Serial.println("Controller connected to ESP32");
|
|
debugger.sendMsg(Loglevel::info, "Controller connected to ESP32")
|
|
}
|
|
|
|
void controllerPrintBattery() {
|
|
if( controller_battery != Ps3.data.status.battery ){
|
|
controller_battery = Ps3.data.status.battery;
|
|
}
|
|
|
|
Serial.print("The controller battery is ");
|
|
if( controller_battery == ps3_status_battery_charging ) Serial.println("charging");
|
|
else if( controller_battery == ps3_status_battery_full ) Serial.println("FULL");
|
|
else if( controller_battery == ps3_status_battery_high ) Serial.println("HIGH");
|
|
else if( controller_battery == ps3_status_battery_low) Serial.println("LOW");
|
|
else if( controller_battery == ps3_status_battery_dying ) Serial.println("DYING");
|
|
else if( controller_battery == ps3_status_battery_shutdown ) Serial.println("SHUTDOWN");
|
|
else Serial.println("UNDEFINED");
|
|
}
|