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> <element>
<id>UMLClass</id> <id>UMLClass</id>
<coordinates> <coordinates>
<x>120</x> <x>460</x>
<y>670</y> <y>60</y>
<w>220</w> <w>220</w>
<h>510</h> <h>510</h>
</coordinates> </coordinates>
@@ -46,8 +46,8 @@ MotorControl
<element> <element>
<id>UMLClass</id> <id>UMLClass</id>
<coordinates> <coordinates>
<x>360</x> <x>80</x>
<y>670</y> <y>480</y>
<w>220</w> <w>220</w>
<h>330</h> <h>330</h>
</coordinates> </coordinates>
@@ -77,8 +77,8 @@ Speedometer
<element> <element>
<id>UMLClass</id> <id>UMLClass</id>
<coordinates> <coordinates>
<x>120</x> <x>70</x>
<y>270</y> <y>60</y>
<w>270</w> <w>270</w>
<h>370</h> <h>370</h>
</coordinates> </coordinates>
@@ -111,13 +111,13 @@ Speedometer
<element> <element>
<id>UMLClass</id> <id>UMLClass</id>
<coordinates> <coordinates>
<x>1110</x> <x>700</x>
<y>670</y> <y>60</y>
<w>220</w> <w>220</w>
<h>330</h> <h>330</h>
</coordinates> </coordinates>
<panel_attributes>&lt;&lt;driveModi&gt;&gt; <panel_attributes>&lt;&lt;driveModi&gt;&gt;
ManualContro ManualControl
-- --
- encoder: ESP32Encoder - encoder: ESP32Encoder
- debug: DebugMqtt* - 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; 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); 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() { void DebugMqtt::sendTime() {
@@ -93,6 +93,8 @@ String DebugMqtt::enum_to_string(Loglevel loglevel) {
return "Rover/Info"; return "Rover/Info";
case Loglevel::debug : case Loglevel::debug :
return "Rover/Debug"; return "Rover/Debug";
case Loglevel::influx :
return "Rover/Influx";
default: default:
return "INVALID ENUM"; return "INVALID ENUM";
} }
src/debugMqtt.h
+2 -1
View File
@@ -9,7 +9,8 @@ enum Loglevel { none,
error, error,
warn, warn,
info, info,
debug}; debug,
influx};
class DebugMqtt { class DebugMqtt {
public: public:
src/driveModi/manualControl.cpp
+43 -9
View File
@@ -19,19 +19,44 @@ void ManualControl::runManualControl() {
if (millis() - this->last_millis < RUN_MANUALCONTROL_DELAY) { if (millis() - this->last_millis < RUN_MANUALCONTROL_DELAY) {
return; 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(); this->last_millis = millis();
switch (this->drive_mode) { //Change controlMode
case DriveMode::stop: 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(); this->reset();
break; break;
case DriveMode::joystick: case ControMode::joystick:
this->driveWithControllerJoystick(); this->driveWithControllerJoystick();
this->moveControl->runMoveControl(); this->moveControl->runMoveControl();
break; break;
case DriveMode::trigger: case ControMode::trigger:
this->driveWithControllerShoulderTrigger(); this->driveWithControllerShoulderTrigger();
break; break;
@@ -46,21 +71,30 @@ void ManualControl::driveWithControllerJoystick() {
double value_per_step = MANUALCONTROL_MAX_SPEED * 2 / 256; double value_per_step = MANUALCONTROL_MAX_SPEED * 2 / 256;
this->moveControl->setSpeed((y * -1) * value_per_step); 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; value_per_step = MANUALCONTROL_MAX_ROTATION * 2 / 256;
this->moveControl->setRotationspeed(x * value_per_step); this->moveControl->setRotationspeed(x * value_per_step);
} }
void ManualControl::changeDriveMode(DriveMode drive_mode) { void ManualControl::changeDriveMode(ControMode drive_mode) {
this->drive_mode = drive_mode; this->reset();
controlMode = drive_mode;
debug->sendMsg(Loglevel::info, "Changed ControlMode by function");
} }
void ManualControl::driveWithControllerShoulderTrigger() { void ManualControl::driveWithControllerShoulderTrigger() {
uint8_t nowL = Ps3.data.analog.button.l2; 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); nowL = map(nowL, 0, 255, 0, 100);
map(nowR, 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) { if (nowL != this->oldL) {
this->motorControlLeft->setTargetPower(nowL); this->motorControlLeft->setTargetPower(nowL);
src/driveModi/manualControl.h
+3 -3
View File
@@ -7,13 +7,13 @@
#include "hardware/motorControl.h" #include "hardware/motorControl.h"
#include "debugMqtt.h" #include "debugMqtt.h"
enum DriveMode {stop, joystick, trigger}; enum ControMode {halt, joystick, trigger};
class ManualControl { class ManualControl {
public: public:
ManualControl(); ManualControl();
void init(MoveControl *moveControl, MotorControl *motorControlLeft, MotorControl *motorControlRight); void init(MoveControl *moveControl, MotorControl *motorControlLeft, MotorControl *motorControlRight);
void runManualControl(); void runManualControl();
void changeDriveMode(DriveMode drive_mode); void changeDriveMode(ControMode drive_mode);
private: private:
void driveWithControllerJoystick(); void driveWithControllerJoystick();
@@ -25,7 +25,7 @@ class ManualControl {
MotorControl *motorControlRight; MotorControl *motorControlRight;
DebugMqtt *debug; DebugMqtt *debug;
DriveMode drive_mode = DriveMode::stop; ControMode controlMode = ControMode::halt;
uint32_t last_millis = 0; uint32_t last_millis = 0;
// Is needed for driveWithControllerShoulderTrigger() // 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()); sprintf(str, "target_power_%s", this->name.c_str());
debug->writeToInflux("motorControl", str, power); debug->writeToInflux("motorControl", str, power);
} else { } 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 <time.h>
#include "config.h" #include "config.h"
#include "hardware/motorControl.h" #include "hardware/motorControl.h"
#include "moveControl.h"
#include "hardware/speedometer.h" #include "hardware/speedometer.h"
#include "driveModi/autopilot.h"
#include "driveModi/captureRoute.h"
#include "driveModi/manualControl.h"
#include "moveControl.h"
#include "debugMqtt.h" #include "debugMqtt.h"
void callbackControllerAction(); void callbackControllerAction();
@@ -23,6 +27,8 @@ Speedometer speedometer_right;
MoveControl moveController; MoveControl moveController;
DebugMqtt debugger("main"); DebugMqtt debugger("main");
ManualControl manualControl;
IPAddress local_IP(WLAN_IP); IPAddress local_IP(WLAN_IP);
IPAddress gateway(WLAN_GATEWAY); IPAddress gateway(WLAN_GATEWAY);
IPAddress subnet(WLAN_SUBNETMASK); IPAddress subnet(WLAN_SUBNETMASK);
@@ -32,6 +38,9 @@ WiFiClient wifi_client;
PubSubClient mqtt_client(wifi_client); PubSubClient mqtt_client(wifi_client);
int controller_battery = -1; int controller_battery = -1;
enum DriveMode {autopilot_e, captureRoute_e, manualControl_e};
DriveMode driveMode = manualControl_e;
void setup() { void setup() {
Serial.begin(115200); Serial.begin(115200);
@@ -66,10 +75,11 @@ void setup() {
Ps3.attach(callbackControllerAction); Ps3.attach(callbackControllerAction);
Ps3.attachOnConnect(callbackControllerConnect); Ps3.attachOnConnect(callbackControllerConnect);
Ps3.attachOnDisconnect(callbackControllerDisconnect); // Ps3.attachOnDisconnect(callbackControllerDisconnect);
Serial.println("\nReady to connect a PS3 Controller... \n"); Serial.println("\nReady to connect a PS3 Controller... \n");
Ps3.begin(); Ps3.begin();
// Init hardware
left_motor.init(M_PWM_1, PWM_CHANNEL_M1, M_DIR_11, M_DIR_12, "left"); 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"); 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"); 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);
// Init driveModi
manualControl.init(&moveController, &left_motor, &right_motor);
} }
void loop() { void loop() {
@@ -88,6 +101,23 @@ void loop() {
right_motor.runMotorControl(); right_motor.runMotorControl();
speedometer_left.runSpeedometer(); speedometer_left.runSpeedometer();
speedometer_right.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() { void reconnectMqtt() {
@@ -120,7 +150,7 @@ void callbackControllerAction() {
void callbackControllerConnect() { void callbackControllerConnect() {
Serial.println("Controller connected to ESP32"); Serial.println("Controller connected to ESP32");
debugger.sendMsg(Loglevel::info, "Controller connected to ESP32") debugger.sendMsg(Loglevel::info, "Controller connected to ESP32");
} }
void controllerPrintBattery() { void controllerPrintBattery() {
src/moveControl.cpp
+3 -3
View File
@@ -41,9 +41,9 @@ void MoveControl::setRotationspeed(double speed) {
} }
void MoveControl::calcWheelSpeed() { void MoveControl::calcWheelSpeed() {
// original formula: /* original formula:
// (1 / r) / 1 b \ / x \ = / Xl \ (1 / r) / 1 b \ / x \ = / Xl \
// \ 1 -b / \ T / \ Xr / \ 1 -b / \ T / \ Xr / */
// (1 / r) * 1 // (1 / r) * 1
const static double A = 15.82278481; const static double A = 15.82278481;