From 6276b632efa661a258744f4326c5867ba1aeba3c Mon Sep 17 00:00:00 2001 From: Tim Korjakow Date: Thu, 15 Jun 2023 15:09:34 +0200 Subject: [PATCH] Added new arduino code --- .../include/communication/UCommands.hpp | 7 ++++ src/dcaitirobot/embedded/include/constants.h | 4 +-- .../embedded/src/communication/UARTCom.cpp | 12 ++++++- .../embedded/src/communication/UCommands.cpp | 33 +++++++++++++++++++ src/dcaitirobot/embedded/src/main.cpp | 7 ---- .../src/diffdrive_arduino.cpp | 19 ++--------- 6 files changed, 55 insertions(+), 27 deletions(-) diff --git a/src/dcaitirobot/embedded/include/communication/UCommands.hpp b/src/dcaitirobot/embedded/include/communication/UCommands.hpp index 5967670..79e068b 100644 --- a/src/dcaitirobot/embedded/include/communication/UCommands.hpp +++ b/src/dcaitirobot/embedded/include/communication/UCommands.hpp @@ -31,6 +31,13 @@ public: * @param c motor to apply the value to */ static void setSetpoint (uint8_t* d, U_Component c); + /** + * @brief sets the PID parameters + * + * @param d uart data package to read the pid parameters from + * @param c motor to apply the value to + */ + static void setPIDParameter(uint8_t* d, U_Component c); }; #endif \ No newline at end of file diff --git a/src/dcaitirobot/embedded/include/constants.h b/src/dcaitirobot/embedded/include/constants.h index b852a59..0b47a93 100644 --- a/src/dcaitirobot/embedded/include/constants.h +++ b/src/dcaitirobot/embedded/include/constants.h @@ -6,7 +6,7 @@ * */ -#define DCAITI_DEBUG 0 +//#define DCAITI_DEBUG 0 /* 1 is on the right, 2 on the left */ @@ -89,7 +89,7 @@ typedef enum components { } U_Component; typedef enum requests { - DEFAULT_REQUEST, DRIVE_MOTOR, RESET_TIMESTAMP + DEFAULT_REQUEST, DRIVE_MOTOR, RESET_TIMESTAMP, DRIVE_ENCODER, PID_PARAMETER } U_Request; #endif \ No newline at end of file diff --git a/src/dcaitirobot/embedded/src/communication/UARTCom.cpp b/src/dcaitirobot/embedded/src/communication/UARTCom.cpp index be10a18..975ba38 100644 --- a/src/dcaitirobot/embedded/src/communication/UARTCom.cpp +++ b/src/dcaitirobot/embedded/src/communication/UARTCom.cpp @@ -1,5 +1,6 @@ #include "communication/UARTCom.hpp" #include "communication/UCommands.hpp" +#include "communication/UValue.hpp" #include "misc/Utils.hpp" #include "constants.h" @@ -7,6 +8,7 @@ uint8_t UARTCom::data[MAX_PACKET_LENGTH]; uint8_t currentParseIndex; int payloadLength; uint32_t currentTimestamp = 0; +extern R2WD _2WD; /** * @brief extracts frametype from data package @@ -115,7 +117,7 @@ uint16_t computeChecksum (uint8_t* d, uint8_t pl) int correctChecksum (uint8_t* d) { uint8_t payloadLength = d[PAYLOAD_LENGTH_POS]; - uint16_t checksumRead = d[PACKAGE_LENGTH_NO_P + payloadLength - 2] | d[PACKAGE_LENGTH_NO_P + payloadLength - 1] << 8; + uint16_t checksumRead = d[PACKAGE_LENGTH_NO_P + payloadLength - 1] | d[PACKAGE_LENGTH_NO_P + payloadLength - 2] << 8; return checksumRead == computeChecksum(d, payloadLength); } @@ -166,6 +168,14 @@ void handleRequest (uint8_t* d) currentTimestamp = 0; break; } + case DRIVE_ENCODER: { + UValue::sendBothEncoderValues(_2WD.wheelLeftGetSpeedMMPS()/1000.0, _2WD.wheelRightGetSpeedMMPS()/1000.0); + break; + } + case PID_PARAMETER: { + UCommands::setPIDParameter(d, parseComponent(d)); + break; + } default: { Serial.println("UNKNOWN REQUEST"); break; diff --git a/src/dcaitirobot/embedded/src/communication/UCommands.cpp b/src/dcaitirobot/embedded/src/communication/UCommands.cpp index 5c7c218..17201f0 100644 --- a/src/dcaitirobot/embedded/src/communication/UCommands.cpp +++ b/src/dcaitirobot/embedded/src/communication/UCommands.cpp @@ -31,4 +31,37 @@ void UCommands::setSetpoint (uint8_t* d, U_Component c) break; } } +} + +void UCommands::setPIDParameter (uint8_t* d, U_Component c) +{ + float P = Utils::bytesToFloat(d[COMMAND_POS + 1], + d[COMMAND_POS + 2], + d[COMMAND_POS + 3], + d[COMMAND_POS + 4]); + float I = Utils::bytesToFloat(d[COMMAND_POS + 5], + d[COMMAND_POS + 6], + d[COMMAND_POS + 7], + d[COMMAND_POS + 8]); + float D = Utils::bytesToFloat(d[COMMAND_POS + 9], + d[COMMAND_POS + 10], + d[COMMAND_POS + 11], + d[COMMAND_POS + 12]); + switch (c) { + case ALL_MOTORS: { + _2WD.PIDTune(P,I,D,10); + break; + } + case RIGHT_MOTOR: { + _2WD.wheelLeftPIDTune(P,I,D,10); + break; + } + case LEFT_MOTOR: { + _2WD.wheelRightPIDTune(P,I,D,10); + break; + } + default: { + break; + } + } } \ No newline at end of file diff --git a/src/dcaitirobot/embedded/src/main.cpp b/src/dcaitirobot/embedded/src/main.cpp index 9d0cd40..cabab23 100644 --- a/src/dcaitirobot/embedded/src/main.cpp +++ b/src/dcaitirobot/embedded/src/main.cpp @@ -14,7 +14,6 @@ int speed = 0; #ifndef MICROS_PER_SEC #define MICROS_PER_SEC 1000000 #endif -# define DEBUG true irqISR(irq1, isr1); irqISR(irq2, isr2); MotorWheel wheelLeft(9, 8, 4, 5, &irq1, REDUCTION_RATIO, 300); // Motor PWM:Pin9, DIR:Pin8, Encoder A:Pin6, B:Pin7 @@ -35,16 +34,10 @@ void setup(void) uint8_t pl[1]; pl[0] = REQUEST_RESET_TIMESTAMP; UARTCom::send(REQUEST, 1, DEFAULT_COMPONENT, pl); - _2WD.setCarAdvance(50); } void loop(void) { _2WD.PIDRegulate(); UARTCom::read(UARTCom::data); - - if (millis() - uartTransmitTimer > 2 * UART_TRANSMIT_MS) { - UValue::sendBothEncoderValues(_2WD.wheelLeftGetSpeedMMPS()/1000.0, _2WD.wheelRightGetSpeedMMPS()/1000.0); - uartTransmitTimer = millis(); - } } \ No newline at end of file diff --git a/src/diffdrive_arduino/src/diffdrive_arduino.cpp b/src/diffdrive_arduino/src/diffdrive_arduino.cpp index 8551774..ea8192c 100644 --- a/src/diffdrive_arduino/src/diffdrive_arduino.cpp +++ b/src/diffdrive_arduino/src/diffdrive_arduino.cpp @@ -101,7 +101,6 @@ return_type DiffDriveArduino::read(const rclcpp::Time & /* time */, const rclcpp double deltaSeconds = diff.count(); time_ = new_time; - if (!arduino_.connected()) { return return_type::ERROR; @@ -112,31 +111,17 @@ return_type DiffDriveArduino::read(const rclcpp::Time & /* time */, const rclcpp // Force the wheel position l_wheel_.pos = l_wheel_.pos + l_wheel_.vel * deltaSeconds; r_wheel_.pos = r_wheel_.pos + r_wheel_.vel * deltaSeconds; - - - - return return_type::OK; - - + return return_type::OK; } return_type DiffDriveArduino::write(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) { - if (!arduino_.connected()) { return return_type::ERROR; } - arduino_.setMotorValues(l_wheel_.cmd, r_wheel_.cmd); - - - - - return return_type::OK; - - - + return return_type::OK; } } // namespace diffdrive_arduino