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

bug fix static in motor und speed

Browse Source
This commit is contained in:
kleiax
2022-01-29 12:25:06 +01:00
parent 1265cfda5c
commit 9c4c82f8b3
18 changed files with 197 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/SpecialMenus/menuManualDrive.cpp
+20
View File
@@ -0,0 +1,20 @@
#include "menuManualDrive.h"
MenuManualControl::MenuManualControl(DriveManager* driveManager) {
this->driveManager = driveManager;
}
void MenuManualControl::left() {
this->driveManager->changeModus(Modi::Off);
if (parentMenu)
this->parentMenu->printMenu();
}
void MenuManualControl::no() {
this->left();
}
void MenuManualControl::printMenu() {
this->driveManager->changeModus(Modi::ManualControl);
std::cout << "Jetzt kannst du\n fahren. :-)" << std::endl;
}
src/SpecialMenus/menuManualDrive.h
+42
View File
@@ -0,0 +1,42 @@
/**
* @file menuManualDrive.h
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @version 0.1
* @date 2022-01-20
*
* @copyright Copyright (c) 2022
*
*/
#ifndef MENU_MANUAL_DRIVE_H
#define MENU_MANUAL_DRIVE_H
#include "menuControl.h"
#include "driveModi/driveManager.h"
// TODO: delete after installtion of display
#include <Arduino.h>
class MenuManualControl : public MenuControl {
public:
MenuManualControl(DriveManager* driveManager);
void down(){}
void up(){}
void right(){}
void left();
void no();
void yes(){}
void printMenu();
private:
DriveManager* driveManager;
};
#endif // MENU_MANUAL_DRIVE_H
src/SpecialMenus/menuPidSettings.cpp
+84
View File
@@ -0,0 +1,84 @@
/**
* @file pidSettings.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @version 0.1
* @date 2022-01-19
*
* @copyright Copyright (c) 2022
*
*/
#include "menuPidSettings.h"
MenuPidSettings::MenuPidSettings(PID* pid) {
this->pid = pid;
this->values[0] = (uint8_t) pid->GetKp();
this->values[1] = (uint8_t) pid->GetKi();
this->values[2] = (uint8_t) pid->GetKd();
}
void MenuPidSettings::down() {
if (values[curPos] > 0)
values[curPos]--;
this->printMenu();
}
void MenuPidSettings::up() {
if (values[curPos] < UINT8_MAX)
values[curPos]++;
this->printMenu();
}
void MenuPidSettings::right() {
if (curPos < NUM_VAL - 1)
curPos++;
else
curPos = 0;
this->printMenu();
}
void MenuPidSettings::left() {
if (curPos > 0)
curPos--;
else
curPos = NUM_VAL - 1;
this->printMenu();
}
void MenuPidSettings::no() {
if (parentMenu)
this->parentMenu->printMenu();
}
void MenuPidSettings::yes() {
this->pid->SetTunings(this->values[0], this->values[1], this->values[2]);
if (parentMenu)
this->parentMenu->printMenu();
}
void MenuPidSettings::printMenu() {
std::cout << " P I D " << std::endl;
switch (this->curPos) {
case 0:
printf("_%3.d_ %3.d %3.d \n", this->values[0], this->values[1], this->values[2]);
break;
case 1:
printf(" %3.d _%3.d_ %3.d \n", this->values[0], this->values[1], this->values[2]);
break;
case 2:
printf(" %3.d %3.d _%3.d_\n", this->values[0], this->values[1], this->values[2]);
break;
default:
break;
}
}
src/SpecialMenus/menuPidSettings.h
+44
View File
@@ -0,0 +1,44 @@
/**
* @file pidSettings.h
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @version 0.1
* @date 2022-01-19
*
* @copyright Copyright (c) 2022
*
*/
#ifndef PID_SETTINGS_H
#define PID_SETTINGS_H
#include "menuControl.h"
#include <PID_v1.h>
#include <stdint.h>
// TODO: delete after installtion of display
#include <Arduino.h>
#define NUM_VAL 3
class MenuPidSettings : public MenuControl {
public:
MenuPidSettings(PID* pid);
void down();
void up();
void right();
void left();
void no();
void yes();
void printMenu();
private:
PID* pid;
uint8_t values[NUM_VAL];
uint8_t curPos = 0;
};
#endif // PID_SETTINGS_H
src/driveModi/Modi/ManualControl/manualControl.cpp
-2
View File
@@ -24,8 +24,6 @@ void ManualControl::init(MoveControl *moveControl) {
}
void ManualControl::loop() {
this->moveControl->loop();
static uint32_t last_millis = 0;
if (millis() - last_millis < delay) {
return;
src/driveModi/driveManager.cpp
+4 -2
View File
@@ -16,6 +16,7 @@ DriveManager::DriveManager(MoveControl *moveControl) {
}
void DriveManager::loop() {
this->moveControl->loop();
if (currentModus)
this->currentModus->loop();
}
@@ -31,11 +32,12 @@ void DriveManager::nextModus() {
void DriveManager::changeModus(Modi modus) {
//Set moveControl to a safe state
delete this->currentModus;
this->moveControl->setSpeed(0);
this->moveControl->setRotationspeed(0);
this->moveControl->setDrivingStatus(DrivingStatus::stop);
delete this->currentModus;
switch (modus) {
case Modi::Off:
this->currentModus = nullptr;
src/main.cpp
+29 -27
View File
@@ -1,5 +1,6 @@
#include <Arduino.h>
#include <Ps3Controller.h>
#include <iostream>
#include "config.h"
@@ -11,26 +12,26 @@
#include "menu.h"
#include "menuAction.h"
#include "menuPidSettings.h"
// Platzhalter, muss irgendwann weg
void dummy(){Serial.println("Dummy in Action");}
void callbackControllerAction();
void callbackControllerConnect();
void controllerPrintBattery();
void p_f();
void i_f();
void d_f();
int32_t displaySelectValue();
#include "SpecialMenus/menuPidSettings.h"
#include "SpecialMenus/menuManualDrive.h"
MoveControl moveController;
DriveManager driveManager(&moveController);
Menu* main_m;
// TODO: Platzhalter, muss irgendwann weg
void dummy(){std::cout << "Dummy in Action" <<std::endl;}
void callbackControllerAction();
void callbackControllerConnect();
void controllerPrintBattery();
void exitDriveMode() {driveManager.changeModus(Modi::Off);}
void setup() {
Serial.begin(115200);
Serial.println("Welcome to Kleiax-Rover");
std::cout << "Welcome to Kleiax-Rover" << std::endl;
Network::setIps();
Network::connectWifi();
@@ -38,7 +39,7 @@ void setup() {
Ps3.attach(callbackControllerAction);
Ps3.attachOnConnect(callbackControllerConnect);
Serial.println("\nReady to connect a PS3 Controller... \n");
std::cout << "\nReady to connect a PS3 Controller... \n";
Ps3.begin();
Serial1.begin(GPS_BAUD, SERIAL_8N1, GPS_RX, GPS_TX);
@@ -49,6 +50,7 @@ void setup() {
Menu* pid_m = new Menu();
MenuPidSettings* pidl_m = new MenuPidSettings(moveController.getPID(0));
MenuPidSettings* pidr_m = new MenuPidSettings(moveController.getPID(1));
MenuManualControl* man_m = new MenuManualControl(&driveManager);
// Entry for the main menu
MenuAction* mode_e = new MenuAction("Mode", mode_m);
@@ -62,7 +64,7 @@ void setup() {
MenuAction* autopilot_e = new MenuAction("Autopilot", dummy);
MenuAction* captureRoute_e = new MenuAction("Capture Route", dummy);
MenuAction* consolControl_e = new MenuAction("Consol Control", dummy);
MenuAction* manualControl_e = new MenuAction("Manual Control", dummy);
MenuAction* manualControl_e = new MenuAction("Manual Control", man_m);
MenuAction* testMode_e = new MenuAction("Test Mode", dummy);
mode_m->addEntry(autopilot_e);
mode_m->addEntry(captureRoute_e);
@@ -82,7 +84,8 @@ void loop() {
Network::checkMQTT();
driveManager.loop();
while (Serial1.available() > 0)
Serial.print(Serial1.read());
std::cout << "In while von gps in loop in main.cpp" << std::endl;
// Serial.print(Serial1.read());
}
bool isDelayReached() {
@@ -112,16 +115,15 @@ void callbackControllerAction() {
else if (Ps3.data.button.circle)
main_m->yes();
else if (Ps3.data.button.cross)
main_m->no();
else if (Ps3.data.button.select)
main_m->printMenu();
main_m->no();
else if (Ps3.data.button.ps)
controllerPrintBattery();
}
}
void callbackControllerConnect() {
Serial.println("Controller connected to ESP32");
std::cout << "Controller connected to ESP32" << std::endl;
main_m->printMenu();
}
void controllerPrintBattery() {
@@ -131,11 +133,11 @@ void controllerPrintBattery() {
}
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");
if( controller_battery == ps3_status_battery_charging ) printf("charging\n");
else if( controller_battery == ps3_status_battery_full ) printf("FULL\n");
else if( controller_battery == ps3_status_battery_high ) printf("HIGH\n");
else if( controller_battery == ps3_status_battery_low) printf("LOW\n");
else if( controller_battery == ps3_status_battery_dying ) printf("DYING\n");
else if( controller_battery == ps3_status_battery_shutdown ) printf("SHUTDOWN\n");
else printf("UNDEFINED\n");
}
src/moveControl.cpp
+44 -15
View File
@@ -38,10 +38,17 @@ MoveControl::~MoveControl() {
}
void MoveControl::loop() {
this->left_motor->loop();
this->right_motor->loop();
this->left_speedometer->loop();
this->right_speedometer->loop();
uint16_t left_motor_time = this->left_motor->loop();
uint16_t right_motor_time = this->right_motor->loop();
uint16_t left_speed_time = this->left_speedometer->loop();
uint16_t right_speed_time = this->right_speedometer->loop();
if (left_motor_time > 50
|| right_motor_time > 50
|| left_speed_time > 50
|| right_speed_time > 50) {
Serial.printf("left M: %d, right M %d, left S %d, right S %d in moveControl::loop\n", left_motor_time, right_motor_time, left_speed_time, right_speed_time);
}
static uint64_t last_millis = 0;
if (millis() - last_millis < delay)
@@ -63,6 +70,19 @@ void MoveControl::runMoveControl() {
void MoveControl::setDrivingStatus(DrivingStatus status) {
this->driving_status = status;
// switch (this->driving_status) {
// case DrivingStatus::stop :
// Serial.println("New drivingState = stop in MoveControl::setDrivingStatus");
// break;
// case DrivingStatus::drive :
// Serial.println("New drivingState = drive in MoveControl::setDrivingStatus");
// break;
// default:
// Serial.println("Wrong drivingState in MoveControl::regulateMotors");
// break;
// }
}
void MoveControl::setSpeed(double speed) {
@@ -114,19 +134,28 @@ void MoveControl::calcTargetWheelSpeed() {
void MoveControl::regulateMotors() {
switch (this->driving_status) {
case DrivingStatus::stop :
this->left_motor->setTargetPower(0);
this->right_motor->setTargetPower(0);
break;
case DrivingStatus::stop :
this->left_motor->setTargetPower(0);
this->right_motor->setTargetPower(0);
// Serial.println("in stop MoveControl::regulateMotors");
// if (abs(left_speedometer->getSpeed()) > 0.2) {
// Serial.printf("Left Motor Target: %d, Ist: %d\n", this->left_motor->getTargetPower(), this->left_motor->getPower());
// Serial.printf("Left Speedometer speed: %f\n", this->left_speedometer->getSpeed());
// }
// if (abs(right_speedometer->getSpeed()) > 0.2) {
// Serial.printf("Right Motor Target: %d, Ist: %d\n", this->right_motor->getTargetPower(), this->right_motor->getPower());
// Serial.printf("Right Speedometer speed: %f\n", this->right_speedometer->getSpeed());
// }
break;
case DrivingStatus::drive :
this->left_motor->setTargetPower( (int8_t) this->left_pid_out);
this->right_motor->setTargetPower( (int8_t) this->right_pid_out);
break;
case DrivingStatus::drive :
this->left_motor->setTargetPower( (int8_t) this->left_pid_out);
this->right_motor->setTargetPower( (int8_t) this->right_pid_out);
break;
default:
Serial.println("Wrong drivingState in MoveControl::regulateMotors");
break;
default:
Serial.println("Wrong drivingState in MoveControl::regulateMotors");
break;
}
}