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

restart the work

Browse Source
This commit is contained in:
kleiax
2021-07-02 08:03:00 +02:00
parent 3b9b2f19f2
commit 4df275ddc0
8 changed files with 99 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/classdiagramm.uxf
+9 -9
View File
@@ -4,8 +4,8 @@
<element>
<id>UMLClass</id>
<coordinates>
<x>120</x>
<y>670</y>
<x>460</x>
<y>60</y>
<w>220</w>
<h>510</h>
</coordinates>
@@ -46,8 +46,8 @@ MotorControl
<element>
<id>UMLClass</id>
<coordinates>
<x>360</x>
<y>670</y>
<x>80</x>
<y>480</y>
<w>220</w>
<h>330</h>
</coordinates>
@@ -77,8 +77,8 @@ Speedometer
<element>
<id>UMLClass</id>
<coordinates>
<x>120</x>
<y>270</y>
<x>70</x>
<y>60</y>
<w>270</w>
<h>370</h>
</coordinates>
@@ -111,13 +111,13 @@ Speedometer
<element>
<id>UMLClass</id>
<coordinates>
<x>1110</x>
<y>670</y>
<x>700</x>
<y>60</y>
<w>220</w>
<h>330</h>
</coordinates>
<panel_attributes>&lt;&lt;driveModi&gt;&gt;
ManualContro
ManualControl
--
- encoder: ESP32Encoder
- debug: DebugMqtt*
src/debugMqtt.cpp
+3 -1
View File
@@ -41,7 +41,7 @@ void DebugMqtt::writeToInflux(String measurement_name, String field_set, float m
unsigned long long int nanos = DebugMqtt::getUpdatedRealMillis() * 1000000;
snprintf(DebugMqtt::msg, MQTT_BUFFER_SITE, "%s %s=%f %llu", measurement_name.c_str(), field_set.c_str(), measurement, nanos);
this->sendData(Loglevel::debug, DebugMqtt::msg);
this->sendData(Loglevel::influx, DebugMqtt::msg);
}
void DebugMqtt::sendTime() {
@@ -93,6 +93,8 @@ String DebugMqtt::enum_to_string(Loglevel loglevel) {
return "Rover/Info";
case Loglevel::debug :
return "Rover/Debug";
case Loglevel::influx :
return "Rover/Influx";
default:
return "INVALID ENUM";
}
src/debugMqtt.h
+2 -1
View File
@@ -9,7 +9,8 @@ enum Loglevel { none,
error,
warn,
info,
debug};
debug,
influx};
class DebugMqtt {
public:
src/driveModi/manualControl.cpp
+43 -9
View File
@@ -19,19 +19,44 @@ void ManualControl::runManualControl() {
if (millis() - this->last_millis < RUN_MANUALCONTROL_DELAY) {
return;
}
//Reset if this function was called a long time ago
if (millis() - this->last_millis > 1000) {
this->reset();
controlMode = ControMode::halt;
debug->sendMsg(Loglevel::info, "Changed ControlMode to halt by Timereset");
}
this->last_millis = millis();
switch (this->drive_mode) {
case DriveMode::stop:
//Change controlMode
if (this->controlMode == ControMode::halt && Ps3.data.button.l3) {
this->reset();
controlMode = ControMode::joystick;
debug->sendMsg(Loglevel::info, "Changed ControlMode to Joystick by Gamepad");
} else if (this->controlMode == ControMode::halt && Ps3.data.button.r3) {
this->reset();
controlMode = ControMode::trigger;
debug->sendMsg(Loglevel::info, "Changed ControlMode to ShoulderTrigger by Gamepad");
} else if (this->controlMode == ControMode::joystick && Ps3.data.button.r3) {
this->reset();
controlMode = ControMode::trigger;
debug->sendMsg(Loglevel::info, "Changed ControlMode to ShoulderTrigger by Gamepad");
} else if (this->controlMode == ControMode::trigger && Ps3.data.button.l3) {
this->reset();
controlMode = ControMode::joystick;
debug->sendMsg(Loglevel::info, "Changed ControlMode to Joystick by Gamepad");
}
switch (this->controlMode) {
case ControMode::halt:
this->reset();
break;
case DriveMode::joystick:
case ControMode::joystick:
this->driveWithControllerJoystick();
this->moveControl->runMoveControl();
break;
case DriveMode::trigger:
case ControMode::trigger:
this->driveWithControllerShoulderTrigger();
break;
@@ -46,21 +71,30 @@ void ManualControl::driveWithControllerJoystick() {
double value_per_step = MANUALCONTROL_MAX_SPEED * 2 / 256;
this->moveControl->setSpeed((y * -1) * value_per_step);
char str[64];
sprintf(str, "%f", (y * -1) * value_per_step);
debug->sendMsg(Loglevel::debug, str);
value_per_step = MANUALCONTROL_MAX_ROTATION * 2 / 256;
this->moveControl->setRotationspeed(x * value_per_step);
}
void ManualControl::changeDriveMode(DriveMode drive_mode) {
this->drive_mode = drive_mode;
void ManualControl::changeDriveMode(ControMode drive_mode) {
this->reset();
controlMode = drive_mode;
debug->sendMsg(Loglevel::info, "Changed ControlMode by function");
}
void ManualControl::driveWithControllerShoulderTrigger() {
uint8_t nowL = Ps3.data.analog.button.l2;
uint8_t nowR = Ps3.data.analog.button.r2;
uint8_t nowR = Ps3.data.analog.button.r2;;
map(nowL, 0, 255, 0, 100);
map(nowR, 0, 255, 0, 100);
nowL = map(nowL, 0, 255, 0, 100);
nowR = map(nowR, 0, 255, 0, 100);
// char str[64];
// sprintf(str, "nachher: nowL: %u, nowR: %u", nowL, nowR);
// debug->sendMsg(Loglevel::debug, str);
if (nowL != this->oldL) {
this->motorControlLeft->setTargetPower(nowL);
src/driveModi/manualControl.h
+3 -3
View File
@@ -7,13 +7,13 @@
#include "hardware/motorControl.h"
#include "debugMqtt.h"
enum DriveMode {stop, joystick, trigger};
enum ControMode {halt, joystick, trigger};
class ManualControl {
public:
ManualControl();
void init(MoveControl *moveControl, MotorControl *motorControlLeft, MotorControl *motorControlRight);
void runManualControl();
void changeDriveMode(DriveMode drive_mode);
void changeDriveMode(ControMode drive_mode);
private:
void driveWithControllerJoystick();
@@ -25,7 +25,7 @@ class ManualControl {
MotorControl *motorControlRight;
DebugMqtt *debug;
DriveMode drive_mode = DriveMode::stop;
ControMode controlMode = ControMode::halt;
uint32_t last_millis = 0;
// Is needed for driveWithControllerShoulderTrigger()
src/hardware/motorControl.cpp
+3 -1
View File
@@ -89,7 +89,9 @@ void MotorControl::setTargetPower(int8_t power) {
sprintf(str, "target_power_%s", this->name.c_str());
debug->writeToInflux("motorControl", str, power);
} else {
debug->sendMsg(Loglevel::error, "Invalid Argument in setTargetPower!");
char str[64];
sprintf(str, "Invalid Argument (power: %d) in setTargetPower!", power);
debug->sendMsg(Loglevel::error, str);
}
}
src/main.cpp
+33 -3
View File
@@ -5,9 +5,13 @@
#include <time.h>
#include "config.h"
#include "hardware/motorControl.h"
#include "moveControl.h"
#include "hardware/speedometer.h"
#include "driveModi/autopilot.h"
#include "driveModi/captureRoute.h"
#include "driveModi/manualControl.h"
#include "moveControl.h"
#include "debugMqtt.h"
void callbackControllerAction();
@@ -23,6 +27,8 @@ Speedometer speedometer_right;
MoveControl moveController;
DebugMqtt debugger("main");
ManualControl manualControl;
IPAddress local_IP(WLAN_IP);
IPAddress gateway(WLAN_GATEWAY);
IPAddress subnet(WLAN_SUBNETMASK);
@@ -32,6 +38,9 @@ WiFiClient wifi_client;
PubSubClient mqtt_client(wifi_client);
int controller_battery = -1;
enum DriveMode {autopilot_e, captureRoute_e, manualControl_e};
DriveMode driveMode = manualControl_e;
void setup() {
Serial.begin(115200);
@@ -66,10 +75,11 @@ void setup() {
Ps3.attach(callbackControllerAction);
Ps3.attachOnConnect(callbackControllerConnect);
Ps3.attachOnDisconnect(callbackControllerDisconnect);
// 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, "left");
right_motor.init(M_PWM_2, PWM_CHANNEL_M2, M_DIR_21, M_DIR_22, "right");
@@ -77,6 +87,9 @@ void setup() {
speedometer_right.init(M_ENCODE_2A, M_ENCODE_2B, "right");
moveController.init(&left_motor, &right_motor, &speedometer_left, &speedometer_right);
// Init driveModi
manualControl.init(&moveController, &left_motor, &right_motor);
}
void loop() {
@@ -88,6 +101,23 @@ void loop() {
right_motor.runMotorControl();
speedometer_left.runSpeedometer();
speedometer_right.runSpeedometer();
switch (driveMode) {
case manualControl_e:
manualControl.runManualControl();
break;
case captureRoute_e:
/* code */
break;
case autopilot_e:
/* code */
break;
default:
break;
}
}
void reconnectMqtt() {
@@ -120,7 +150,7 @@ void callbackControllerAction() {
void callbackControllerConnect() {
Serial.println("Controller connected to ESP32");
debugger.sendMsg(Loglevel::info, "Controller connected to ESP32")
debugger.sendMsg(Loglevel::info, "Controller connected to ESP32");
}
void controllerPrintBattery() {
src/moveControl.cpp
+3 -3
View File
@@ -41,9 +41,9 @@ void MoveControl::setRotationspeed(double speed) {
}
void MoveControl::calcWheelSpeed() {
// original formula:
// (1 / r) / 1 b \ / x \ = / Xl \
// \ 1 -b / \ T / \ Xr /
/* original formula:
(1 / r) / 1 b \ / x \ = / Xl \
\ 1 -b / \ T / \ Xr / */
// (1 / r) * 1
const static double A = 15.82278481;