diff --git a/doc/classdiagramm.uxf b/doc/classdiagramm.uxf new file mode 100644 index 0000000..943a1c7 --- /dev/null +++ b/doc/classdiagramm.uxf @@ -0,0 +1,108 @@ + + + 10 + + UMLClass + + 120 + 670 + 210 + 510 + + <<hardware>> +MotorControl +-- +- name: String +- debug: DebugMqtt* +- target_power: int8_t = 0 +- power: int8_t = 0 +- direction: uint8_t = 0 +- last_millis: uint64_t = 0; +- pwm_pin: uint8_t +- pwm_channel: uint8_t +- direction_pin_1: uint8_t +- direction_pin_2: uint8_t +-- ++ MotorControl() ++ init(pwm_pin: uint8_t, +    pwm_channel: uint8_t, +    direction_1: uint8_t, +   direction_2: uint8_t, +    name: String) ++ runMotorControl() ++ setTargetPower(power: int8_t) ++ stop() ++ emergencyStop() ++ toString() ++ getPower(): int8_t ++ getTargetPower(): int8_t ++ isTargetPowerReached(): bool ++ isAccelerationPositive(): bool ++ isAccelerationNegative(): bool +- setRealPower(power: int8_t) +- increasePower(power: int8_t) + + + + UMLClass + + 380 + 670 + 210 + 330 + + <<hardware>> +Speedometer +-- +- encoder: ESP32Encoder +- debug: DebugMqtt* +- name: String +- speed: double = 0 +- last_millis: uint64_t = 0; +- bufPos: uint8_t +- buf: int16_t[BUF_SIZE] +-- ++ Speedometer() ++ init(pinA: uint8_t, +    pinB: uint8_t, +    name: String) ++ runSpeedometer() ++ setTargetPower(power: int8_t) ++ getSpeed(): double ++ getDirection(): uint8_t +- addValToBuf(val: int16_t) +- getAverage(): int16_t + + + + UMLClass + + 900 + 660 + 210 + 330 + + <<hardware>> +Speedometer +-- +- encoder: ESP32Encoder +- debug: DebugMqtt* +- name: String +- speed: double = 0 +- last_millis: uint64_t = 0; +- bufPos: uint8_t +- buf: int16_t[BUF_SIZE] +-- ++ Speedometer() ++ init(pinA: uint8_t, +    pinB: uint8_t, +    name: String) ++ runSpeedometer() ++ setTargetPower(power: int8_t) ++ getSpeed(): double ++ getDirection(): uint8_t +- addValToBuf(val: int16_t) +- getAverage(): int16_t + + + diff --git a/src/config.h b/src/config.h index adf1fd2..c5e4463 100644 --- a/src/config.h +++ b/src/config.h @@ -46,13 +46,14 @@ #define RUN_MOVE_CONTROL_DELAY 150 // Normal 10 untested #define ACCELERATE_STEPS 1 +//ManualControl config +#define RUN_MANUALCONTROL_DELAY 10 +#define MANUALCONTROL_MAX_SPEED 1.0 +#define MANUALCONTROL_MAX_ROTATION 1.0 + // PS3 Controller // ESP32 MAC BL 24:62:AB:F2:4B:3A -//Max Values for speed and rotation -#define MAX_SPEED 1.0 -#define MAX_ROTATION 1.0 - //Network config #define WLAN_SSID "Kleiax2" #define WLAN_PASSWORD "Punica-699" diff --git a/src/driveModi/manualControl.cpp b/src/driveModi/manualControl.cpp index e69de29..0279a42 100644 --- a/src/driveModi/manualControl.cpp +++ b/src/driveModi/manualControl.cpp @@ -0,0 +1,83 @@ +#include "manualControl.h" + +ManualControl::ManualControl() { + this->debug = new DebugMqtt("ManualControl"); +} + +void ManualControl::init(MoveControl *moveControl, MotorControl *motorControlLeft, MotorControl *motorControlRight) { + debug->sendMsg(Loglevel::info, "Init..."); + + this->moveControl = moveControl; + this->motorControlLeft = motorControlLeft; + this->motorControlRight = motorControlRight; + + debug->sendMsg(Loglevel::info, "Init finished!"); +} + +void ManualControl::runManualControl() { + //Cancel if delay is not reached + if (millis() - this->last_millis < RUN_MANUALCONTROL_DELAY) { + return; + } + this->last_millis = millis(); + + switch (this->drive_mode) { + case DriveMode::stop: + this->reset(); + break; + + case DriveMode::joystick: + this->driveWithControllerJoystick(); + this->moveControl->runMoveControl(); + break; + + case DriveMode::trigger: + this->driveWithControllerShoulderTrigger(); + break; + + default: + break; + } +} + +void ManualControl::driveWithControllerJoystick() { + int8_t x = Ps3.data.analog.stick.lx; + int8_t y = Ps3.data.analog.stick.ly; + + double value_per_step = MANUALCONTROL_MAX_SPEED * 2 / 256; + this->moveControl->setSpeed((y * -1) * value_per_step); + + 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::driveWithControllerShoulderTrigger() { + uint8_t nowL = Ps3.data.analog.button.l2; + uint8_t nowR = Ps3.data.analog.button.r2; + + map(nowL, 0, 255, 0, 100); + map(nowR, 0, 255, 0, 100); + + if (nowL != this->oldL) { + this->motorControlLeft->setTargetPower(nowL); + this->oldL = nowL; + } + + if (nowR != this->oldR) { + this->motorControlRight->setTargetPower(nowR); + this->oldR = nowR; + } +} + +void ManualControl::reset() { + this->oldL = 0; + this->oldR = 0; + motorControlLeft->setTargetPower(0); + motorControlRight->setTargetPower(0); + moveControl->setRotationspeed(0); + moveControl->setSpeed(0); +} \ No newline at end of file diff --git a/src/driveModi/manualControl.h b/src/driveModi/manualControl.h index e69de29..468a8e3 100644 --- a/src/driveModi/manualControl.h +++ b/src/driveModi/manualControl.h @@ -0,0 +1,36 @@ +#ifndef MANUAL_CONTROL_H +#define MANUAL_CONTROL_H + +#include + +#include "moveControl.h" +#include "hardware/motorControl.h" +#include "debugMqtt.h" + +enum DriveMode {stop, joystick, trigger}; +class ManualControl { + public: + ManualControl(); + void init(MoveControl *moveControl, MotorControl *motorControlLeft, MotorControl *motorControlRight); + void runManualControl(); + void changeDriveMode(DriveMode drive_mode); + + private: + void driveWithControllerJoystick(); + void driveWithControllerShoulderTrigger(); + void reset(); + + MoveControl *moveControl; + MotorControl *motorControlLeft; + MotorControl *motorControlRight; + DebugMqtt *debug; + + DriveMode drive_mode = DriveMode::stop; + uint32_t last_millis = 0; + + // Is needed for driveWithControllerShoulderTrigger() + uint8_t oldL = 0; + uint8_t oldR = 0; +}; + +#endif // MANUAL_CONTROL_H \ No newline at end of file diff --git a/src/hardware/motorControl.cpp b/src/hardware/motorControl.cpp index cde41e9..8408169 100644 --- a/src/hardware/motorControl.cpp +++ b/src/hardware/motorControl.cpp @@ -8,9 +8,11 @@ MotorControl::MotorControl() { this->debug = new DebugMqtt("MotorControl"); } -void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_pin_1, uint8_t direction_pin_2) { +void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_pin_1, uint8_t direction_pin_2, String name) { debug->sendMsg(Loglevel::info, "Init..."); + this->name = name; + this->pwm_pin = pwm_pin; this->pwm_channel = pwm_channel; this->direction_pin_1 = direction_pin_1; @@ -83,7 +85,9 @@ void MotorControl::runMotorControl() { void MotorControl::setTargetPower(int8_t power) { if (power <= 100 && power >= -100) { this->target_power = power; - debug->writeToInflux("motorControl", "target_power_ASIDE", power); + char str[32]; + sprintf(str, "target_power_%s", this->name.c_str()); + debug->writeToInflux("motorControl", str, power); } else { debug->sendMsg(Loglevel::error, "Invalid Argument in setTargetPower!"); } @@ -91,7 +95,9 @@ void MotorControl::setTargetPower(int8_t power) { void MotorControl::stop() { this->target_power = 0; - debug->writeToInflux("motorControl", "target_power_ASIDE", this->target_power); + char str[32]; + sprintf(str, "target_power_%s", this->name.c_str()); + debug->writeToInflux("motorControl", str, this->target_power); } void MotorControl::emergencyStop() { @@ -158,7 +164,9 @@ void MotorControl::setRealPower(int8_t power) { } ledcWrite(this->pwm_channel, pwm_val); - debug->writeToInflux("motorControl", "real_power_ASIDE", power); + char str[32]; + sprintf(str, "real_power_%s", this->name.c_str()); + debug->writeToInflux("motorControl", str, power); // Serial.printf("pwm_val: %d, ", pwm_val); } diff --git a/src/hardware/motorControl.h b/src/hardware/motorControl.h index a7c8fe7..6dbd608 100644 --- a/src/hardware/motorControl.h +++ b/src/hardware/motorControl.h @@ -9,7 +9,7 @@ class MotorControl { public: MotorControl(); - void init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_1, uint8_t direction_2); + void init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_1, uint8_t direction_2, String name); void runMotorControl(); void setTargetPower(int8_t power); void stop(); @@ -26,6 +26,8 @@ class MotorControl { void setRealPower(int8_t power); void increasePower(int8_t power); + String name; + DebugMqtt *debug; int8_t target_power = 0; diff --git a/src/hardware/speedometer.cpp b/src/hardware/speedometer.cpp index 836a6fe..0f07972 100644 --- a/src/hardware/speedometer.cpp +++ b/src/hardware/speedometer.cpp @@ -7,8 +7,9 @@ Speedometer::Speedometer() { this->debug = new DebugMqtt("Speedometer"); } -void Speedometer::init(uint8_t pinA, uint8_t pinB) { +void Speedometer::init(uint8_t pinA, uint8_t pinB, String name) { debug->sendMsg(Loglevel::info, "Init..."); + this->name = name; ESP32Encoder::useInternalWeakPullResistors=DOWN; this->encoder.attachFullQuad(pinA, pinB); this->encoder.setFilter(ENC_FILTER); @@ -49,7 +50,9 @@ void Speedometer::runSpeedometer() { } else { this->speed = 0; } - debug->writeToInflux("speedometer", "speed_ASIDE", this->speed); + char str[32]; + sprintf(str, "speed_%s", this->name.c_str()); + debug->writeToInflux("speedometer", str, this->speed); // Serial.printf("time: %d, ", time); // Serial.printf("count: %d, ",count); diff --git a/src/hardware/speedometer.h b/src/hardware/speedometer.h index 37408f9..9a7b096 100644 --- a/src/hardware/speedometer.h +++ b/src/hardware/speedometer.h @@ -11,7 +11,7 @@ class Speedometer { public: Speedometer(); - void init(uint8_t pinA, uint8_t pinB); + void init(uint8_t pinA, uint8_t pinB, String name); void runSpeedometer(); double getSpeed(); uint8_t getDirection(); @@ -24,6 +24,7 @@ class Speedometer { ESP32Encoder encoder; DebugMqtt *debug; + String name; double speed = 0; uint32_t last_millis = 0; diff --git a/src/main.cpp b/src/main.cpp index 10ba911..d288f9d 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -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); -} - diff --git a/src/moveControl.cpp b/src/moveControl.cpp index e5a5ed3..98d6152 100644 --- a/src/moveControl.cpp +++ b/src/moveControl.cpp @@ -64,99 +64,7 @@ void MoveControl::regulateMotors() { this->right_motor->setTargetPower(0); break; - case DrivingStatus::straightForward : - // Left motor - // Too slow - if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target - && this->left_speedometer->getSpeed() <= this->right_speedometer->getSpeed()) { - - this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS); - - // Too fast - } else if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target - && this->left_speedometer->getSpeed() >= this->right_speedometer->getSpeed()) { - - this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS); - } - - // Right motor - // Too slow - if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target - && this->right_speedometer->getSpeed() <= this->left_speedometer->getSpeed()) { - - this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS); - - // Too fast - } else if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target - && this->right_speedometer->getSpeed() >= this->left_speedometer->getSpeed()) { - - this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS); - } - break; - - case DrivingStatus::straightBackward : - // Left motor - // Too slow - if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target - && this->left_speedometer->getSpeed() >= this->right_speedometer->getSpeed()) { - - this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS); - - // Too fast - } else if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target - && this->left_speedometer->getSpeed() <= this->right_speedometer->getSpeed()) { - - this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS); - } - - // Right motor - // Too slow - if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target - && this->right_speedometer->getSpeed() >= this->left_speedometer->getSpeed()) { - - this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS); - - // Too fast - } else if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target - && this->right_speedometer->getSpeed() <= this->left_speedometer->getSpeed()) { - - this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS); - } - break; - - case DrivingStatus::arcForwardLeft : // Identical with arcForwardRight - ratio = this->wheelspeed_left_target / this->wheelspeed_right_target; - - // Left motor - // Too slow - if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target - && this->left_speedometer->getSpeed() <= (this->right_speedometer->getSpeed()) * ratio) { - - this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS); - - // Too fast - } else if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target - && this->left_speedometer->getSpeed() >= (this->right_speedometer->getSpeed()) * ratio) { - - this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS); - } - - // Right motor - // Too slow - if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target - && (this->right_speedometer->getSpeed() * ratio) <= this->left_speedometer->getSpeed()) { - - this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS); - - // Too fast - } else if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target - && (this->right_speedometer->getSpeed() * ratio) >= this->left_speedometer->getSpeed()) { - - this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS); - } - break; - - case DrivingStatus::arcForwardRight : // Identical with arcForwardLeft + case DrivingStatus::forward : ratio = this->wheelspeed_left_target / this->wheelspeed_right_target; // Left motor @@ -188,39 +96,7 @@ void MoveControl::regulateMotors() { } break; - case DrivingStatus::arcBackwardLeft : // Identical with arcBackwardRight - ratio = this->wheelspeed_left_target / this->wheelspeed_right_target; - - // Left motor - // Too slow - if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target - && this->left_speedometer->getSpeed() >= (this->right_speedometer->getSpeed() * ratio)) { - - this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS); - - // Too fast - } else if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target - && this->left_speedometer->getSpeed() <= (this->right_speedometer->getSpeed() * ratio)) { - - this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS); - } - - // Right motor - // Too slow - if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target - && (this->right_speedometer->getSpeed() * ratio) >= this->left_speedometer->getSpeed()) { - - this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS); - - // Too fast - } else if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target - && (this->right_speedometer->getSpeed() * ratio) <= this->left_speedometer->getSpeed()) { - - this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS); - } - break; - - case DrivingStatus::arcBackwardRight : // Identical with arcBackwardLeft + case DrivingStatus::backward : ratio = this->wheelspeed_left_target / this->wheelspeed_right_target; // Left motor diff --git a/src/route.cpp b/src/route.cpp index 5c61b67..51a51d8 100644 --- a/src/route.cpp +++ b/src/route.cpp @@ -38,4 +38,11 @@ Point Route::getNextPoint() { uint16_t Route::getNumberOfPoints() { return this->count_points; +} + +double Route::getDis(Point point_1, Point point_2) { + double lat = (point_1.lat + point_2.lat) / 2 * 0.1745; + double dx = 111.3 * cos(lat) * (point_1.lon - point_2.lon); + double dy = 111.3 * (point_1.lat - point_2.lat); + return sqrt(dx * dx + dy * dy); } \ No newline at end of file diff --git a/src/route.h b/src/route.h index f392308..b784c39 100644 --- a/src/route.h +++ b/src/route.h @@ -9,8 +9,8 @@ #include "config.h" struct Point{ - float N = 0; - float E = 0; + float lat = 0; + float lon = 0; }; class Route { @@ -25,6 +25,8 @@ class Route { Point getNextPoint(); uint16_t getNumberOfPoints(); + static double getDis(Point point_1, Point point_2); + private: std::list points;