From 7d8ceba7bf32140c642feb5907609265ebbcd680 Mon Sep 17 00:00:00 2001 From: Linux_Hat Date: Sat, 11 May 2024 21:38:01 +0200 Subject: [PATCH] fixes and optimisations --- .../jacobot-pio-project/.vscode/settings.json | 73 ++++++++++++++ .../src/{Car.cpp => car.cpp} | 21 ++-- .../jacobot-pio-project/src/{Car.h => car.h} | 2 +- .../arduino/jacobot-pio-project/src/main.cpp | 10 +- .../jacobot-pio-project/src/motorEncoder.cpp | 5 +- .../jacobot-pio-project/src/motorEncoder.h | 95 +++++-------------- 6 files changed, 116 insertions(+), 90 deletions(-) create mode 100644 jacobot/arduino/jacobot-pio-project/.vscode/settings.json rename jacobot/arduino/jacobot-pio-project/src/{Car.cpp => car.cpp} (81%) rename jacobot/arduino/jacobot-pio-project/src/{Car.h => car.h} (95%) diff --git a/jacobot/arduino/jacobot-pio-project/.vscode/settings.json b/jacobot/arduino/jacobot-pio-project/.vscode/settings.json new file mode 100644 index 0000000..1af0236 --- /dev/null +++ b/jacobot/arduino/jacobot-pio-project/.vscode/settings.json @@ -0,0 +1,73 @@ +{ + "C_Cpp_Runner.cCompilerPath": "/home/adrien/.platformio/packages/toolchain-atmelavr/bin/avr-gcc", + "C_Cpp_Runner.cppCompilerPath": "/home/adrien/.platformio/packages/toolchain-atmelavr/bin/avr-g++", + "C_Cpp_Runner.debuggerPath": "/home/adrien/.platformio/packages/toolchain-atmelavr/bin/avr-gdb", + "C_Cpp_Runner.cStandard": "gnu11", + "C_Cpp_Runner.cppStandard": "gnu++11", + "C_Cpp_Runner.msvcBatchPath": "", + "C_Cpp_Runner.useMsvc": false, + "C_Cpp_Runner.warnings": [ + "-Wall", + "-Wextra", + "-Wpedantic", + "-Wshadow", + "-Wformat=2", + "-Wcast-align", + "-Wconversion", + "-Wsign-conversion", + "-Wnull-dereference" + ], + "C_Cpp_Runner.msvcWarnings": [ + "/W4", + "/permissive-", + "/w14242", + "/w14287", + "/w14296", + "/w14311", + "/w14826", + "/w44062", + "/w44242", + "/w14905", + "/w14906", + "/w14263", + "/w44265", + "/w14928" + ], + "C_Cpp_Runner.enableWarnings": true, + "C_Cpp_Runner.warningsAsError": false, + "C_Cpp_Runner.compilerArgs": [], + "C_Cpp_Runner.linkerArgs": [], + "C_Cpp_Runner.includePaths": [ + "/home/adrien/dev/LPH/JacoBot/jacobot/arduino/jacobot-pio-project/include", + "/home/adrien/dev/LPH/JacoBot/jacobot/arduino/jacobot-pio-project/src", + "/home/adrien/dev/LPH/JacoBot/jacobot/arduino/jacobot-pio-project/.pio/libdeps/uno/PID", + "/home/adrien/dev/LPH/JacoBot/jacobot/arduino/jacobot-pio-project/.pio/libdeps/uno/RF24", + "/home/adrien/dev/LPH/JacoBot/jacobot/arduino/jacobot-pio-project/.pio/libdeps/uno/RF24/utility", + "/home/adrien/.platformio/packages/framework-arduino-avr/libraries/SPI/src", + "/home/adrien/.platformio/packages/framework-arduino-avr/cores/arduino", + "/home/adrien/.platformio/packages/framework-arduino-avr/variants/standard", + "/home/adrien/.platformio/packages/framework-arduino-avr/libraries/EEPROM/src", + "/home/adrien/.platformio/packages/framework-arduino-avr/libraries/HID/src", + "/home/adrien/.platformio/packages/framework-arduino-avr/libraries/SoftwareSerial/src", + "/home/adrien/.platformio/packages/framework-arduino-avr/libraries/Wire/src", + "" + ], + "C_Cpp_Runner.includeSearch": [ + "*", + "**/*" + ], + "C_Cpp_Runner.excludeSearch": [ + "**/build", + "**/build/**", + "**/.*", + "**/.*/**", + "**/.vscode", + "**/.vscode/**" + ], + "C_Cpp_Runner.useAddressSanitizer": false, + "C_Cpp_Runner.useUndefinedSanitizer": false, + "C_Cpp_Runner.useLeakSanitizer": false, + "C_Cpp_Runner.showCompilationTime": false, + "C_Cpp_Runner.useLinkTimeOptimization": false, + "C_Cpp_Runner.msvcSecureNoWarnings": false +} \ No newline at end of file diff --git a/jacobot/arduino/jacobot-pio-project/src/Car.cpp b/jacobot/arduino/jacobot-pio-project/src/car.cpp similarity index 81% rename from jacobot/arduino/jacobot-pio-project/src/Car.cpp rename to jacobot/arduino/jacobot-pio-project/src/car.cpp index 47133a9..7c1542f 100644 --- a/jacobot/arduino/jacobot-pio-project/src/Car.cpp +++ b/jacobot/arduino/jacobot-pio-project/src/car.cpp @@ -1,6 +1,6 @@ // Made by Linux_Hat -#include "Car.h" -#include "MotorEncoder.h" +#include "car.h" +#include "motorEncoder.h" #define pi 3.1415 Car::Car(MotorEncoder* left, MotorEncoder* right, float distance_beetween_motors) @@ -16,16 +16,13 @@ void Car::forward(float distance, double setPoint) { //distance : mm m_right->setPoint = setPoint/m2_mmp*m1_mmp; while(m1_durationduration; - m_left->compute(0); + m1_duration+=m_left->compute(0); m_left->advance(); - m_left->duration = 0; } else {m_left->stop();} + if(m2_durationduration; - m_right->compute(0); + m2_duration+m_right->compute(0); m_right->advance(); - m_right->duration = 0; } else {m_right->stop();} } m_left->stop(); @@ -42,8 +39,7 @@ void Car::turn(float angle, double setPoint) { //angle : degre if(angle>0) { const double pulse_goal = distance/m1_mmp; while(durationduration; - m_left->compute(0); + duration+=m_left->compute(0); m_left->advance(); m_right->compute(0); m_right->back(); @@ -51,11 +47,10 @@ void Car::turn(float angle, double setPoint) { //angle : degre } else if(angle<0) { const double pulse_goal = distance/m2_mmp; while(durationduration; + duration+=m_right->compute(0); + m_right->advance(); m_left->compute(0); m_left->back(); - m_right->compute(0); - m_right->advance(); } } m_left->stop(); diff --git a/jacobot/arduino/jacobot-pio-project/src/Car.h b/jacobot/arduino/jacobot-pio-project/src/car.h similarity index 95% rename from jacobot/arduino/jacobot-pio-project/src/Car.h rename to jacobot/arduino/jacobot-pio-project/src/car.h index d382e3b..7126f60 100644 --- a/jacobot/arduino/jacobot-pio-project/src/Car.h +++ b/jacobot/arduino/jacobot-pio-project/src/car.h @@ -1,6 +1,6 @@ // Made by Linux_Hat #pragma once -#include "MotorEncoder.h" +#include "motorEncoder.h" /** * @brief Create a new instance of car diff --git a/jacobot/arduino/jacobot-pio-project/src/main.cpp b/jacobot/arduino/jacobot-pio-project/src/main.cpp index b4fbe6a..607c878 100644 --- a/jacobot/arduino/jacobot-pio-project/src/main.cpp +++ b/jacobot/arduino/jacobot-pio-project/src/main.cpp @@ -1,14 +1,13 @@ // Made by Linux_Hat #include #include "radio.h" -#include "MotorEncoder.h" -#include "Car.h" +#include "motorEncoder.h" +#include "car.h" #define MLEFT_ENC_PIN_A 3 #define MLEFT_ENC_PIN_B A4 #define MLEFT_E_MOTOR 5 #define MLEFT_M_MOTOR 4 -#define MLEFT_ORIENTATION false #define MLEFT_RATIO 120 #define MLEFT_RADIUS 3.3 @@ -16,7 +15,6 @@ #define MRIGHT_ENC_PIN_B A3 #define MRIGHT_E_MOTOR 10 #define MRIGHT_M_MOTOR 8 -#define MRIGHT_ORIENTATION false #define MRIGHT_RATIO 120 #define MRIGHT_RADIUS 3.3 @@ -25,8 +23,8 @@ #define SIZE 500 #define SPEED 50 -MotorEncoder m1(MLEFT_ENC_PIN_A, MLEFT_ENC_PIN_B, MLEFT_E_MOTOR, MLEFT_M_MOTOR, MLEFT_ORIENTATION, MLEFT_RATIO, MLEFT_RADIUS); // Initialise motor -MotorEncoder m2(MRIGHT_ENC_PIN_A, MRIGHT_ENC_PIN_B, MRIGHT_E_MOTOR, MRIGHT_M_MOTOR, MRIGHT_ORIENTATION, MRIGHT_RATIO, MRIGHT_RADIUS); // Initialise motor with pins +MotorEncoder m1(MLEFT_ENC_PIN_A, MLEFT_ENC_PIN_B, MLEFT_E_MOTOR, MLEFT_M_MOTOR, MLEFT_RATIO, MLEFT_RADIUS); // Initialise motor +MotorEncoder m2(MRIGHT_ENC_PIN_A, MRIGHT_ENC_PIN_B, MRIGHT_E_MOTOR, MRIGHT_M_MOTOR, MRIGHT_RATIO, MRIGHT_RADIUS); // Initialise motor with pins Car car(&m1, &m2, 110); diff --git a/jacobot/arduino/jacobot-pio-project/src/motorEncoder.cpp b/jacobot/arduino/jacobot-pio-project/src/motorEncoder.cpp index 3ecba97..8b3480f 100644 --- a/jacobot/arduino/jacobot-pio-project/src/motorEncoder.cpp +++ b/jacobot/arduino/jacobot-pio-project/src/motorEncoder.cpp @@ -59,7 +59,7 @@ void MotorEncoder::stop() { analogWrite(m_E_left, 0); } -void MotorEncoder::compute(int debug) { +double MotorEncoder::compute(int debug) { abs_duration = abs(duration); result = myPID->Compute(); if (result) { @@ -70,6 +70,9 @@ void MotorEncoder::compute(int debug) { Serial.println(val_output); } + double old_duration = duration; duration = 0; + return old_duration; } + return duration; } \ No newline at end of file diff --git a/jacobot/arduino/jacobot-pio-project/src/motorEncoder.h b/jacobot/arduino/jacobot-pio-project/src/motorEncoder.h index 3ecba97..a3a4c6b 100644 --- a/jacobot/arduino/jacobot-pio-project/src/motorEncoder.h +++ b/jacobot/arduino/jacobot-pio-project/src/motorEncoder.h @@ -1,75 +1,32 @@ +#pragma once #include #include -#include "motorEncoder.h" -MotorEncoder::MotorEncoder(uint8_t encoder0pinA, uint8_t encoder0pinB, int E_left, int M_left, float ratio, float radius) { - m_encoder0pinA = encoder0pinA; // pin 2 or 3 - m_encoder0pinB = encoder0pinB; - m_E_left = E_left;// pwm pin ! - m_M_left = M_left; - m_ratio = ratio; - m_radius = radius; - setPoint = 0; - direction = true; // Default -> Forward - pinMode(m_encoder0pinB, INPUT); - pinMode(m_M_left, OUTPUT); // L298P Control port settings DC motor driver board for the output mode - pinMode(m_E_left, OUTPUT); +class MotorEncoder { + private: + uint8_t encoder0PinALast; //the number of the pulses + boolean direction; //the rotation direction + boolean result; + PID* myPID; + double val_output; //Power supplied to the motor PWM value. + double Kp = 0.6, Ki = 5, Kd = 0; + double abs_duration; + uint8_t m_encoder0pinA; //A pin + uint8_t m_encoder0pinB; //B pin + int m_E_left; //The enabling of L298PDC motor driver board connection to the digital interface port 5 + int m_M_left; - // Initialize PID controller - myPID = new PID(&abs_duration, &val_output, &setPoint, Kp, Ki, Kd, DIRECT); - myPID->SetMode(AUTOMATIC); // PID is set to automatic mode - myPID->SetSampleTime(100); // Set PID sampling frequency to 100ms -} + public: + float m_radius, m_ratio; + double duration; + double setPoint; -void MotorEncoder::wheelSpeed() { - int Lstate = digitalRead(m_encoder0pinA); - if((encoder0PinALast == LOW) && Lstate==HIGH){ - int val = digitalRead(m_encoder0pinB); - if(val == LOW && direction){ - direction = false; //Reverse - } - else if(val == HIGH && !direction){ - direction = true; //Forward - } - } - encoder0PinALast = Lstate; + MotorEncoder(uint8_t encoder0pinA, uint8_t encoder0pinB, int E_left, int M_left, float ratio, float radius); - if(!direction) duration++; - else duration--; - -} - -void MotorEncoder::EncoderHandlerInit(void (*ISR)(void)) { - direction = true; // Default -> Forward - pinMode(m_encoder0pinB, INPUT); // Ensure encoder pin A is set as INPUT - attachInterrupt(m_encoder0pinA-2, ISR, CHANGE); -} - -void MotorEncoder::advance() { - digitalWrite(m_M_left, LOW); - analogWrite(m_E_left, abs(val_output)); -} - -void MotorEncoder::back() { - digitalWrite(m_M_left, HIGH); - analogWrite(m_E_left, abs(val_output)); -} - -void MotorEncoder::stop() { - analogWrite(m_E_left, 0); -} - -void MotorEncoder::compute(int debug) { - abs_duration = abs(duration); - result = myPID->Compute(); - if (result) { - if (debug) { - Serial.print("Pluse: "); - Serial.print(duration); - Serial.print(" Power: "); - Serial.println(val_output); - - } - duration = 0; - } -} \ No newline at end of file + void EncoderHandlerInit(void (*ISR)(void)); + void wheelSpeed(); + void advance(); + void back(); + void stop(); + double compute(int debug); +}; \ No newline at end of file