clean main.cpp, clean moveControl.cpp
added classdiagramm and ManualControl
This commit is contained in:
+74
-148
@@ -12,7 +12,6 @@
|
||||
|
||||
void callbackControllerAction();
|
||||
void callbackControllerConnect();
|
||||
void callbackControllerDisconnect();
|
||||
void controllerPrintBattery();
|
||||
|
||||
void reconnectMqtt();
|
||||
@@ -32,182 +31,109 @@ IPAddress mqtt_server(MQTT_SERVER);
|
||||
WiFiClient wifi_client;
|
||||
PubSubClient mqtt_client(wifi_client);
|
||||
|
||||
uint64_t last_millis = 0;
|
||||
int controller_battery = -1;
|
||||
|
||||
// Temp code
|
||||
double speed = 0;
|
||||
uint8_t drive_mode = 0;
|
||||
void driveWithControllerJoystick(int8_t x, int8_t y);
|
||||
void driveWithControllerShoulderTrigger(uint8_t left, uint8_t right);
|
||||
void changeDriveMode();
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
Serial.begin(115200);
|
||||
|
||||
// Configures static IP address
|
||||
if (!WiFi.config(local_IP, gateway, subnet)) {
|
||||
Serial.println("STA Failed to configure");
|
||||
}
|
||||
// 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(".");
|
||||
}
|
||||
// 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());
|
||||
// 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();
|
||||
mqtt_client.setServer(mqtt_server, MQTT_PORT);
|
||||
reconnectMqtt();
|
||||
|
||||
configTime(3600, 3600, "192.168.1.2");
|
||||
configTime(3600, 3600, "192.168.1.2");
|
||||
|
||||
DebugMqtt::init(&mqtt_client, Loglevel::debug);
|
||||
DebugMqtt::initRealMillis();
|
||||
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();
|
||||
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);
|
||||
right_motor.init(M_PWM_2, PWM_CHANNEL_M2, M_DIR_21, M_DIR_22);
|
||||
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);
|
||||
speedometer_right.init(M_ENCODE_2A, M_ENCODE_2B);
|
||||
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);
|
||||
moveController.init(&left_motor, &right_motor, &speedometer_left, &speedometer_right);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
if (!mqtt_client.connected()) {
|
||||
reconnectMqtt();
|
||||
}
|
||||
mqtt_client.loop();
|
||||
if (!mqtt_client.connected())
|
||||
reconnectMqtt();
|
||||
mqtt_client.loop();
|
||||
|
||||
if (millis() - last_millis > 1000) {
|
||||
debugger.sendTime();
|
||||
|
||||
last_millis = millis();
|
||||
}
|
||||
|
||||
left_motor.runMotorControl();
|
||||
right_motor.runMotorControl();
|
||||
speedometer_left.runSpeedometer();
|
||||
speedometer_right.runSpeedometer();
|
||||
|
||||
if (drive_mode == 1)
|
||||
moveController.runMoveControl();
|
||||
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);
|
||||
// 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.r3) { controllerPrintBattery(); }
|
||||
if (Ps3.event.button_down.l1) { moveController.setSpeed(speed -= 0.1); }
|
||||
if (Ps3.event.button_down.r1) { moveController.setSpeed(speed += 0.1); }
|
||||
if (Ps3.event.button_down.start) { changeDriveMode(); }
|
||||
if (abs(Ps3.event.analog_changed.stick.lx) || abs(Ps3.event.analog_changed.stick.ly))
|
||||
driveWithControllerJoystick(Ps3.data.analog.stick.lx, Ps3.data.analog.stick.ly);
|
||||
|
||||
if (abs(Ps3.event.analog_changed.button.l2) || abs(Ps3.event.analog_changed.button.r2)) {
|
||||
driveWithControllerShoulderTrigger(Ps3.data.analog.button.l2, Ps3.data.analog.button.r2);
|
||||
Serial.println(Ps3.data.analog.button.l2, DEC);
|
||||
}
|
||||
if (Ps3.event.button_down.start) {
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void callbackControllerConnect() {
|
||||
Serial.println("Controller connected to ESP32");
|
||||
|
||||
delay(400);
|
||||
Serial.print("Setting LEDs to Status "); Serial.println(drive_mode, DEC);
|
||||
Ps3.setPlayer(drive_mode);
|
||||
}
|
||||
|
||||
void callbackControllerDisconnect() {
|
||||
Serial.println("Controller disconnected from ESP32");
|
||||
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;
|
||||
}
|
||||
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");
|
||||
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");
|
||||
}
|
||||
|
||||
void driveWithControllerJoystick(int8_t x, int8_t y) {
|
||||
if (drive_mode != 1) return;
|
||||
|
||||
double value_per_step = MAX_SPEED * 2 / 256;
|
||||
moveController.setSpeed((y * -1) * value_per_step);
|
||||
|
||||
value_per_step = MAX_ROTATION * 2 / 256;
|
||||
moveController.setRotationspeed(x * value_per_step);
|
||||
}
|
||||
|
||||
void driveWithControllerShoulderTrigger(uint8_t left, uint8_t right) {
|
||||
if (drive_mode != 2) return;
|
||||
|
||||
map(left, 0, 255, 0, 100);
|
||||
map(right, 0, 255, 0, 100);
|
||||
|
||||
left_motor.setTargetPower(left);
|
||||
right_motor.setTargetPower(right);
|
||||
}
|
||||
|
||||
void changeDriveMode() {
|
||||
drive_mode++;
|
||||
if (drive_mode > 2) {
|
||||
drive_mode = 1;
|
||||
}
|
||||
Ps3.setPlayer(drive_mode);
|
||||
|
||||
if (drive_mode == 2) {
|
||||
moveController.setSpeed(0);
|
||||
}
|
||||
}
|
||||
|
||||
void getDis(double lat1, double lon1, double lat2, double lon2) {
|
||||
double lat = (lat1 + lat2) / 2 * 0.01745;
|
||||
double dx = 111.3 * cos(lat) * (lon1 - lon2);
|
||||
double dy = 111.3 * (lat1 - lat2);
|
||||
double erg = sqrt(dx * dx + dy * dy);
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user