From e148ac6466c4805a9f7f819a8adaada1147efc13 Mon Sep 17 00:00:00 2001 From: mimi980 <90112270+mimi980@users.noreply.github.com> Date: Sun, 15 Jan 2023 15:15:51 +0100 Subject: [PATCH 01/36] manque le gyro --- src/main/cpp/Robot.cpp | 44 +++- src/main/include/Robot.h | 28 ++- vendordeps/Phoenix.json | 423 +++++++++++++++++++++++++++++++++++++++ vendordeps/REVLib.json | 73 +++++++ 4 files changed, 561 insertions(+), 7 deletions(-) create mode 100644 vendordeps/Phoenix.json create mode 100644 vendordeps/REVLib.json diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 98059c5..0bc2e81 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -3,7 +3,16 @@ // the WPILib BSD license file in the root directory of this project. #include "Robot.h" -//cc + +double Robot::signe(double x) { + if (x > 0) { + return 1; + } else if (x < 0) { + return -1; + } else { + return 0; + } +} void Robot::RobotInit() {} void Robot::RobotPeriodic() {} @@ -11,8 +20,35 @@ void Robot::RobotPeriodic() {} void Robot::AutonomousInit() {} void Robot::AutonomousPeriodic() {} -void Robot::TeleopInit() {} -void Robot::TeleopPeriodic() {} +void Robot::TeleopInit() { + frc::SmartDashboard::PutNumber("P", 0.5); + frc::SmartDashboard::PutNumber("I", 0); + frc::SmartDashboard::PutNumber("D", 0); +} +void Robot::TeleopPeriodic() { + + + frc::SmartDashboard::PutNumber("Setpoint", 1); + double z = std::abs(m_accelerometer.GetZ()); // -1/1 + double y =m_accelerometer.GetY(); + + double output = m_pidController.Calculate(z, 1); + + // double angle = m_gyro.GetAngle(); + + frc::SmartDashboard::PutNumber("Z", z); + frc::SmartDashboard::PutNumber("Y", y); + frc::SmartDashboard::PutNumber("Output", output); + frc::SmartDashboard::PutNumber("Angle", 0); + + double speed = signe(y)*output; + frc::SmartDashboard::PutNumber("speed",speed); + m_MotorRight.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); + m_MotorRightFollower.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); + m_MotorRightFollower2.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); + m_MotorLeft.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); + +} void Robot::DisabledInit() {} void Robot::DisabledPeriodic() {} @@ -20,8 +56,6 @@ void Robot::DisabledPeriodic() {} void Robot::TestInit() {} void Robot::TestPeriodic() {} -void Robot::SimulationInit() {} -void Robot::SimulationPeriodic() {} #ifndef RUNNING_FRC_TESTS int main() { diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 062a198..28a441f 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -5,6 +5,15 @@ #pragma once #include +#include +#include +#include +#include +#include +#include +#include +//gyro +// #include class Robot : public frc::TimedRobot { public: @@ -23,6 +32,21 @@ class Robot : public frc::TimedRobot { void TestInit() override; void TestPeriodic() override; - void SimulationInit() override; - void SimulationPeriodic() override; + + double signe(double x); + + private: + + frc::Joystick m_joystick{0}; + frc::BuiltInAccelerometer m_accelerometer{}; + // frc::ADXRS450_Gyro m_gyro{}; + frc::PIDController m_pidController{frc::SmartDashboard::GetNumber("P",0.5), 0, 0}; + + ctre::phoenix::motorcontrol::can::TalonFX m_MotorRight{1}; + ctre::phoenix::motorcontrol::can::TalonFX m_MotorRightFollower{2}; + ctre::phoenix::motorcontrol::can::TalonFX m_MotorRightFollower2{3}; + ctre::phoenix::motorcontrol::can::TalonFX m_MotorLeft{4}; + ctre::phoenix::motorcontrol::can::TalonFX m_MotorLeftFollower{5}; + ctre::phoenix::motorcontrol::can::TalonFX m_MotorLeftFollower2{6}; + }; diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json new file mode 100644 index 0000000..72371c0 --- /dev/null +++ b/vendordeps/Phoenix.json @@ -0,0 +1,423 @@ +{ + "fileName": "Phoenix.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.30.3", + "frcYear": 2023, + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2023-latest.json", + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.30.3" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.30.3" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.30.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.30.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro", + "artifactId": "tools", + "version": "23.0.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "tools-sim", + "version": "23.0.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonSRX", + "version": "23.0.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonFX", + "version": "23.0.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simVictorSPX", + "version": "23.0.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simPigeonIMU", + "version": "23.0.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simCANCoder", + "version": "23.0.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProTalonFX", + "version": "23.0.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProCANcoder", + "version": "23.0.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProPigeon2", + "version": "23.0.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.30.3", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.30.3", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.30.3", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenixpro", + "artifactId": "tools", + "version": "23.0.2", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.30.3", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.30.3", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.30.3", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "tools-sim", + "version": "23.0.2", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonSRX", + "version": "23.0.2", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonFX", + "version": "23.0.2", + "libName": "CTRE_SimTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simVictorSPX", + "version": "23.0.2", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simPigeonIMU", + "version": "23.0.2", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simCANCoder", + "version": "23.0.2", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProTalonFX", + "version": "23.0.2", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProCANcoder", + "version": "23.0.2", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProPigeon2", + "version": "23.0.2", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..cdbbe6e --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,73 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2023.1.2", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2023.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2023.1.2" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2023.1.2", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2023.1.2", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2023.1.2", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file From 516038d18195405f6107f8f7f78fe9951ac44997 Mon Sep 17 00:00:00 2001 From: mimi980 <90112270+mimi980@users.noreply.github.com> Date: Fri, 20 Jan 2023 12:37:52 +0100 Subject: [PATCH 02/36] =?UTF-8?q?gyro=20=C3=A0=20tester?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/cpp/Robot.cpp | 13 +++++++------ src/main/include/Robot.h | 6 +++--- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 0bc2e81..4f772f7 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -24,22 +24,23 @@ void Robot::TeleopInit() { frc::SmartDashboard::PutNumber("P", 0.5); frc::SmartDashboard::PutNumber("I", 0); frc::SmartDashboard::PutNumber("D", 0); + frc::SmartDashboard::PutNumber("Setpoint", 1); } void Robot::TeleopPeriodic() { - - - frc::SmartDashboard::PutNumber("Setpoint", 1); + m_pidController.SetP(frc::SmartDashboard::GetNumber("P", 0.5)); + m_pidController.SetI(frc::SmartDashboard::GetNumber("I", 0)); + m_pidController.SetD(frc::SmartDashboard::GetNumber("D", 0)); double z = std::abs(m_accelerometer.GetZ()); // -1/1 double y =m_accelerometer.GetY(); - double output = m_pidController.Calculate(z, 1); + double output = m_pidController.Calculate(z, frc::SmartDashboard::GetNumber("Setpoint", 1)); - // double angle = m_gyro.GetAngle(); + double angle = m_gyro.GetAngle(); frc::SmartDashboard::PutNumber("Z", z); frc::SmartDashboard::PutNumber("Y", y); frc::SmartDashboard::PutNumber("Output", output); - frc::SmartDashboard::PutNumber("Angle", 0); + frc::SmartDashboard::PutNumber("Angle", angle); double speed = signe(y)*output; frc::SmartDashboard::PutNumber("speed",speed); diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 28a441f..edeeeca 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -13,7 +13,7 @@ #include #include //gyro -// #include +#include class Robot : public frc::TimedRobot { public: @@ -39,8 +39,8 @@ class Robot : public frc::TimedRobot { frc::Joystick m_joystick{0}; frc::BuiltInAccelerometer m_accelerometer{}; - // frc::ADXRS450_Gyro m_gyro{}; - frc::PIDController m_pidController{frc::SmartDashboard::GetNumber("P",0.5), 0, 0}; + frc::ADXRS450_Gyro m_gyro{}; + frc::PIDController m_pidController{0, 0, 0}; ctre::phoenix::motorcontrol::can::TalonFX m_MotorRight{1}; ctre::phoenix::motorcontrol::can::TalonFX m_MotorRightFollower{2}; From 2d579f56234dcea76de5e78b07272a6af6ac8990 Mon Sep 17 00:00:00 2001 From: Gamtnt Date: Sat, 21 Jan 2023 12:46:34 +0100 Subject: [PATCH 03/36] Pour entrer les valeurs du PID --- gradlew | 0 src/main/include/Robot.h | 3 +-- 2 files changed, 1 insertion(+), 2 deletions(-) mode change 100644 => 100755 gradlew diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 28a441f..00a27eb 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -40,8 +40,7 @@ class Robot : public frc::TimedRobot { frc::Joystick m_joystick{0}; frc::BuiltInAccelerometer m_accelerometer{}; // frc::ADXRS450_Gyro m_gyro{}; - frc::PIDController m_pidController{frc::SmartDashboard::GetNumber("P",0.5), 0, 0}; - + frc::PIDController m_pidController{(frc::SmartDashboard::PutNumber("P", frc::SmartDashboard::GetNumber("P", 0.0))), (frc::SmartDashboard::PutNumber("I", frc::SmartDashboard::GetNumber("I", 0.0))), (frc::SmartDashboard::PutNumber("D", frc::SmartDashboard::GetNumber("D", 0.0))) }; ctre::phoenix::motorcontrol::can::TalonFX m_MotorRight{1}; ctre::phoenix::motorcontrol::can::TalonFX m_MotorRightFollower{2}; ctre::phoenix::motorcontrol::can::TalonFX m_MotorRightFollower2{3}; From 23ba7297e27fc27260c3c0b5d1b0c35c11c5d281 Mon Sep 17 00:00:00 2001 From: mimi980 <90112270+mimi980@users.noreply.github.com> Date: Wed, 25 Jan 2023 15:32:39 +0100 Subject: [PATCH 04/36] save --- .vscode/settings.json | 5 ++++- src/main/cpp/Robot.cpp | 35 ++++++++++++++++++++++++++--------- src/main/include/Robot.h | 5 +++-- 3 files changed, 33 insertions(+), 12 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 5e795ff..da44c62 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -14,5 +14,8 @@ "**/.factorypath": true, "**/*~": true }, - "C_Cpp.default.configurationProvider": "vscode-wpilib" + "C_Cpp.default.configurationProvider": "vscode-wpilib", + "files.associations": { + "*.inc": "cpp" + } } diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 4f772f7..dd74295 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -21,33 +21,50 @@ void Robot::AutonomousInit() {} void Robot::AutonomousPeriodic() {} void Robot::TeleopInit() { - frc::SmartDashboard::PutNumber("P", 0.5); + frc::SmartDashboard::PutNumber("P", 1.5); frc::SmartDashboard::PutNumber("I", 0); frc::SmartDashboard::PutNumber("D", 0); frc::SmartDashboard::PutNumber("Setpoint", 1); } void Robot::TeleopPeriodic() { - m_pidController.SetP(frc::SmartDashboard::GetNumber("P", 0.5)); + m_pidController.SetP(frc::SmartDashboard::GetNumber("P", 1.5)); m_pidController.SetI(frc::SmartDashboard::GetNumber("I", 0)); m_pidController.SetD(frc::SmartDashboard::GetNumber("D", 0)); - double z = std::abs(m_accelerometer.GetZ()); // -1/1 + double z = std::abs(m_accelerometer.GetX()); // -1/1 double y =m_accelerometer.GetY(); double output = m_pidController.Calculate(z, frc::SmartDashboard::GetNumber("Setpoint", 1)); - double angle = m_gyro.GetAngle(); frc::SmartDashboard::PutNumber("Z", z); frc::SmartDashboard::PutNumber("Y", y); frc::SmartDashboard::PutNumber("Output", output); - frc::SmartDashboard::PutNumber("Angle", angle); + frc::SmartDashboard::PutNumber("Angle", y); + // double angle = m_gyro.GetAngle(); + // frc::SmartDashboard::PutNumber("Angle", angle); + // frc::SmartDashboard::PutNumber("arcos accélé",std::acos(y)/3.14159265*180.0); double speed = signe(y)*output; frc::SmartDashboard::PutNumber("speed",speed); - m_MotorRight.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); - m_MotorRightFollower.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); - m_MotorRightFollower2.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); - m_MotorLeft.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); + if (m_joystick.GetRawButton(1)) + { + m_MotorRight.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); + m_MotorRightFollower.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); + m_MotorRightFollower2.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); + m_MotorLeft.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); + m_MotorLeftFollower.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); + m_MotorLeftFollower2.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); + } + else + { + m_MotorRight.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, 0.0); + m_MotorRightFollower.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, 0.0); + m_MotorRightFollower2.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, 0.0); + m_MotorLeft.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, 0.0); + m_MotorLeftFollower.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, 0.0); + m_MotorLeftFollower2.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, 0.0); + } + } diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index edeeeca..e3df0b7 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -12,9 +12,10 @@ #include #include #include -//gyro #include +//gyro + class Robot : public frc::TimedRobot { public: void RobotInit() override; @@ -39,8 +40,8 @@ class Robot : public frc::TimedRobot { frc::Joystick m_joystick{0}; frc::BuiltInAccelerometer m_accelerometer{}; - frc::ADXRS450_Gyro m_gyro{}; frc::PIDController m_pidController{0, 0, 0}; + frc::ADXRS450_Gyro m_gyro{}; ctre::phoenix::motorcontrol::can::TalonFX m_MotorRight{1}; ctre::phoenix::motorcontrol::can::TalonFX m_MotorRightFollower{2}; From 29ceef7c8ae0496513d2e62d372edef83c3bc1d8 Mon Sep 17 00:00:00 2001 From: mimi980 <90112270+mimi980@users.noreply.github.com> Date: Thu, 26 Jan 2023 15:59:29 +0100 Subject: [PATCH 05/36] merci constantin --- src/main/cpp/Robot.cpp | 50 ++++++++++++++++++++++------------------ src/main/include/Robot.h | 3 ++- 2 files changed, 30 insertions(+), 23 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index dd74295..f5207ff 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -14,7 +14,11 @@ double Robot::signe(double x) { } } -void Robot::RobotInit() {} +void Robot::RobotInit() { + // frc::ADXRS450_Gyro(frc::SPI::Port::kOnboardCS0); + m_gyro.Reset(); + m_gyro.Calibrate(); +} void Robot::RobotPeriodic() {} void Robot::AutonomousInit() {} @@ -40,35 +44,37 @@ void Robot::TeleopPeriodic() { frc::SmartDashboard::PutNumber("Y", y); frc::SmartDashboard::PutNumber("Output", output); frc::SmartDashboard::PutNumber("Angle", y); - // double angle = m_gyro.GetAngle(); - // frc::SmartDashboard::PutNumber("Angle", angle); + double angle = m_gyro.GetAngle(); + frc::SmartDashboard::PutNumber("Angle", angle); // frc::SmartDashboard::PutNumber("arcos accélé",std::acos(y)/3.14159265*180.0); double speed = signe(y)*output; frc::SmartDashboard::PutNumber("speed",speed); - if (m_joystick.GetRawButton(1)) - { - m_MotorRight.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); - m_MotorRightFollower.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); - m_MotorRightFollower2.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); - m_MotorLeft.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); - m_MotorLeftFollower.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); - m_MotorLeftFollower2.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); - } - else - { - m_MotorRight.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, 0.0); - m_MotorRightFollower.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, 0.0); - m_MotorRightFollower2.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, 0.0); - m_MotorLeft.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, 0.0); - m_MotorLeftFollower.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, 0.0); - m_MotorLeftFollower2.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, 0.0); - } + // if (m_joystick.GetRawButton(1)) + // { + // m_MotorRight.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); + // m_MotorRightFollower.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); + // m_MotorRightFollower2.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); + // m_MotorLeft.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); + // m_MotorLeftFollower.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); + // m_MotorLeftFollower2.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); + // } + // else + // { + // m_MotorRight.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, 0.0); + // m_MotorRightFollower.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, 0.0); + // m_MotorRightFollower2.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, 0.0); + // m_MotorLeft.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, 0.0); + // m_MotorLeftFollower.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, 0.0); + // m_MotorLeftFollower2.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, 0.0); + // } } -void Robot::DisabledInit() {} +void Robot::DisabledInit() { + m_gyro.Reset(); +} void Robot::DisabledPeriodic() {} void Robot::TestInit() {} diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index e3df0b7..9baa813 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -13,6 +13,7 @@ #include #include #include +#include //gyro @@ -41,7 +42,7 @@ class Robot : public frc::TimedRobot { frc::Joystick m_joystick{0}; frc::BuiltInAccelerometer m_accelerometer{}; frc::PIDController m_pidController{0, 0, 0}; - frc::ADXRS450_Gyro m_gyro{}; + frc::ADXRS450_Gyro m_gyro; ctre::phoenix::motorcontrol::can::TalonFX m_MotorRight{1}; ctre::phoenix::motorcontrol::can::TalonFX m_MotorRightFollower{2}; From 90030b6330a58276739b0e130be913502f744b69 Mon Sep 17 00:00:00 2001 From: mimi980 <90112270+mimi980@users.noreply.github.com> Date: Thu, 26 Jan 2023 16:51:53 +0100 Subject: [PATCH 06/36] gradle --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index 2451599..313fab4 100644 --- a/build.gradle +++ b/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2023.1.1" + id "edu.wpi.first.GradleRIO" version "2023.2.1" } // Define my targets (RoboRIO) and artifacts (deployable files) From 8adf637c66228dd51d5808d6021004cae959ef7b Mon Sep 17 00:00:00 2001 From: mimi980 <90112270+mimi980@users.noreply.github.com> Date: Fri, 27 Jan 2023 12:22:53 +0100 Subject: [PATCH 07/36] cc --- src/main/include/Robot.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 9baa813..5b8d57a 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -42,7 +42,7 @@ class Robot : public frc::TimedRobot { frc::Joystick m_joystick{0}; frc::BuiltInAccelerometer m_accelerometer{}; frc::PIDController m_pidController{0, 0, 0}; - frc::ADXRS450_Gyro m_gyro; + frc::ADXRS450_Gyro m_gyro{frc::SPI::Port::kOnboardCS0}; ctre::phoenix::motorcontrol::can::TalonFX m_MotorRight{1}; ctre::phoenix::motorcontrol::can::TalonFX m_MotorRightFollower{2}; From 2d6ae95f8fcf692de6127771423b01cb27790ed9 Mon Sep 17 00:00:00 2001 From: Gamtnt Date: Tue, 7 Feb 2023 15:07:49 +0100 Subject: [PATCH 08/36] Le gyro marche avec la maj --- build.gradle | 2 +- src/main/cpp/Robot.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/build.gradle b/build.gradle index 313fab4..6d5a1cf 100644 --- a/build.gradle +++ b/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2023.2.1" + id "edu.wpi.first.GradleRIO" version "2023.3.2" } // Define my targets (RoboRIO) and artifacts (deployable files) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index f5207ff..a7fceff 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -15,8 +15,8 @@ double Robot::signe(double x) { } void Robot::RobotInit() { - // frc::ADXRS450_Gyro(frc::SPI::Port::kOnboardCS0); - m_gyro.Reset(); + //frc::ADXRS450_Gyro(frc::SPI::Port::kOnboardCS0); + m_gyro.Reset(); m_gyro.Calibrate(); } void Robot::RobotPeriodic() {} From 1dcd6a00691671b2a1b5a1d3a1823cece8e69b57 Mon Sep 17 00:00:00 2001 From: Gamtnt Date: Thu, 9 Feb 2023 00:21:36 +0100 Subject: [PATCH 09/36] bonne nuit --- src/main/cpp/Robot.cpp | 64 +++++++++++++++++++++++++++++----------- src/main/include/Robot.h | 19 +++++++++++- 2 files changed, 64 insertions(+), 19 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index a7fceff..abb3661 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -14,6 +14,25 @@ double Robot::signe(double x) { } } +double Robot::Calcul_De_Notre_Brave_JM(double forward, double turn, bool wheelSide) // calcule la vitesse des roues +{ + double m_forward = forward; + double m_turn = turn; + + double left_wheel = m_forward + m_turn * SIGMA; + double right_wheel = m_forward - m_turn * SIGMA; + + double k; + k = 1.0 / (NMAX(1, NMAX(NABS(left_wheel), NABS(right_wheel)))); + left_wheel *= k; + right_wheel *= k; + + if (wheelSide == 0) + return right_wheel; + else + return left_wheel; +} + void Robot::RobotInit() { //frc::ADXRS450_Gyro(frc::SPI::Port::kOnboardCS0); m_gyro.Reset(); @@ -29,6 +48,21 @@ void Robot::TeleopInit() { frc::SmartDashboard::PutNumber("I", 0); frc::SmartDashboard::PutNumber("D", 0); frc::SmartDashboard::PutNumber("Setpoint", 1); + + m_MotorLeft.SetInverted(true); + m_MotorLeftFollower.SetInverted(true); + m_MotorLeftFollower2.SetInverted(true); + + m_MotorRight.SetInverted(false); + m_MotorRightFollower.SetInverted(false); + m_MotorRightFollower2.SetInverted(false); + + m_MotorLeftFollower.Follow(m_MotorLeft); + m_MotorLeftFollower2.Follow(m_MotorLeft); + + m_MotorRightFollower.Follow(m_MotorRight); + m_MotorRightFollower2.Follow(m_MotorRight); + } void Robot::TeleopPeriodic() { m_pidController.SetP(frc::SmartDashboard::GetNumber("P", 1.5)); @@ -50,24 +84,18 @@ void Robot::TeleopPeriodic() { double speed = signe(y)*output; frc::SmartDashboard::PutNumber("speed",speed); - // if (m_joystick.GetRawButton(1)) - // { - // m_MotorRight.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); - // m_MotorRightFollower.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); - // m_MotorRightFollower2.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); - // m_MotorLeft.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); - // m_MotorLeftFollower.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); - // m_MotorLeftFollower2.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); - // } - // else - // { - // m_MotorRight.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, 0.0); - // m_MotorRightFollower.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, 0.0); - // m_MotorRightFollower2.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, 0.0); - // m_MotorLeft.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, 0.0); - // m_MotorLeftFollower.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, 0.0); - // m_MotorLeftFollower2.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, 0.0); - // } + if (m_joystickRight.GetRawButton(1)) + { + m_MotorRight.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); + m_MotorLeft.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); + + } + else + { + m_MotorRight.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, Calcul_De_Notre_Brave_JM(-m_joystickLeft.GetY(), m_joystickRight.GetZ(), 1)); + m_MotorLeft.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, Calcul_De_Notre_Brave_JM(-m_joystickLeft.GetY(), m_joystickRight.GetZ(), 0)); + + } } diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 9baa813..964b786 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -3,6 +3,21 @@ // the WPILib BSD license file in the root directory of this project. #pragma once +#define SIGMA 0.5 + +#define AMAX 5.1 // Acceleration Max au PIF .. à définir aux encodeurs +#define VMAX 3.4 // vitesse Max théorique (3,395472 sur JVN-DT) .. à vérifier aux encodeurs +#define WMAX \ + (((2.0 * VMAX) / AXLETRACK) / \ + 1.7) // vitesse angulaire Max theorique .. à modifier avec Garice + +#define NABS(a) (((a) < 0) ? -(a) : (a)) // VALEUR ABSOLUE +#define NMAX(a, b) (((a) > (b)) ? (a) : (b)) // Max +#define NMIN(a, b) (((a) < (b)) ? (a) : (b)) // Min +#define NROUND(fval) ( ( (fval) >= 0.0f ) ? ((Ns32)((fval) + 0.5f)) : ((Ns32)((fval) - 0.5f)) ) +#define NSIGN(a) (((a)<0) ? -1:1) +#define NCLAMP(mn,a,mx) ( ((a)<(mn)) ? (mn) : ((a)>(mx)) ? (mx) : (a) ) +#define NLERP(a,b,t) ( a + (b - a)*t ) #include #include @@ -36,10 +51,12 @@ class Robot : public frc::TimedRobot { double signe(double x); + double Calcul_De_Notre_Brave_JM(double forward, double turn, bool wheelSide); private: - frc::Joystick m_joystick{0}; + frc::Joystick m_joystickRight{0}; + frc::Joystick m_joystickLeft{1}; frc::BuiltInAccelerometer m_accelerometer{}; frc::PIDController m_pidController{0, 0, 0}; frc::ADXRS450_Gyro m_gyro; From da5a9e09043b11b8cf9b39ca589167aa6da00d2d Mon Sep 17 00:00:00 2001 From: mimi980 <90112270+mimi980@users.noreply.github.com> Date: Thu, 9 Feb 2023 14:46:25 +0100 Subject: [PATCH 10/36] =?UTF-8?q?=C3=A7a=20marche=20mais=20pas=20les=20boi?= =?UTF-8?q?tes?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/cpp/Robot.cpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index abb3661..712b7ee 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -63,15 +63,23 @@ void Robot::TeleopInit() { m_MotorRightFollower.Follow(m_MotorRight); m_MotorRightFollower2.Follow(m_MotorRight); + m_MotorLeft.SetNeutralMode(ctre::phoenix::motorcontrol::NeutralMode::Brake); + m_MotorLeftFollower.SetNeutralMode(ctre::phoenix::motorcontrol::NeutralMode::Brake); + m_MotorLeftFollower2.SetNeutralMode(ctre::phoenix::motorcontrol::NeutralMode::Brake); + + m_MotorRight.SetNeutralMode(ctre::phoenix::motorcontrol::NeutralMode::Brake); + m_MotorRightFollower.SetNeutralMode(ctre::phoenix::motorcontrol::NeutralMode::Brake); + m_MotorRightFollower2.SetNeutralMode(ctre::phoenix::motorcontrol::NeutralMode::Brake); + } void Robot::TeleopPeriodic() { - m_pidController.SetP(frc::SmartDashboard::GetNumber("P", 1.5)); + m_pidController.SetP(frc::SmartDashboard::GetNumber("P", 0.1)); m_pidController.SetI(frc::SmartDashboard::GetNumber("I", 0)); m_pidController.SetD(frc::SmartDashboard::GetNumber("D", 0)); double z = std::abs(m_accelerometer.GetX()); // -1/1 double y =m_accelerometer.GetY(); - double output = m_pidController.Calculate(z, frc::SmartDashboard::GetNumber("Setpoint", 1)); + double output = m_pidController.Calculate(y, frc::SmartDashboard::GetNumber("Setpoint", 1)); frc::SmartDashboard::PutNumber("Z", z); @@ -82,7 +90,7 @@ void Robot::TeleopPeriodic() { frc::SmartDashboard::PutNumber("Angle", angle); // frc::SmartDashboard::PutNumber("arcos accélé",std::acos(y)/3.14159265*180.0); - double speed = signe(y)*output; + double speed = signe(-angle)*output; frc::SmartDashboard::PutNumber("speed",speed); if (m_joystickRight.GetRawButton(1)) { @@ -92,14 +100,15 @@ void Robot::TeleopPeriodic() { } else { - m_MotorRight.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, Calcul_De_Notre_Brave_JM(-m_joystickLeft.GetY(), m_joystickRight.GetZ(), 1)); - m_MotorLeft.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, Calcul_De_Notre_Brave_JM(-m_joystickLeft.GetY(), m_joystickRight.GetZ(), 0)); + m_MotorRight.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, Calcul_De_Notre_Brave_JM(-m_joystickLeft.GetY(), m_joystickRight.GetZ(), 0)); + m_MotorLeft.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, Calcul_De_Notre_Brave_JM(-m_joystickLeft.GetY(), m_joystickRight.GetZ(), 1)); } } + void Robot::DisabledInit() { m_gyro.Reset(); } From b3e572b7008b070cdd618322b8894f0f9659bfd1 Mon Sep 17 00:00:00 2001 From: mimi980 <90112270+mimi980@users.noreply.github.com> Date: Thu, 9 Feb 2023 17:02:48 +0100 Subject: [PATCH 11/36] save --- src/main/include/Robot.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 37c6bfa..4b0d55c 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -29,6 +29,7 @@ #include #include #include +#include //gyro From 9ed32f97f1961e569f894e4c5a13cd4407a9a47b Mon Sep 17 00:00:00 2001 From: mimi980 <90112270+mimi980@users.noreply.github.com> Date: Thu, 9 Feb 2023 17:03:00 +0100 Subject: [PATCH 12/36] save --- src/main/cpp/Robot.cpp | 28 ++++++++++++++++++---------- 1 file changed, 18 insertions(+), 10 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 712b7ee..6362c79 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -44,10 +44,10 @@ void Robot::AutonomousInit() {} void Robot::AutonomousPeriodic() {} void Robot::TeleopInit() { - frc::SmartDashboard::PutNumber("P", 1.5); + frc::SmartDashboard::PutNumber("P", 0); frc::SmartDashboard::PutNumber("I", 0); frc::SmartDashboard::PutNumber("D", 0); - frc::SmartDashboard::PutNumber("Setpoint", 1); + frc::SmartDashboard::PutNumber("Setpoint", 0); m_MotorLeft.SetInverted(true); m_MotorLeftFollower.SetInverted(true); @@ -63,6 +63,8 @@ void Robot::TeleopInit() { m_MotorRightFollower.Follow(m_MotorRight); m_MotorRightFollower2.Follow(m_MotorRight); + + m_MotorLeft.SetNeutralMode(ctre::phoenix::motorcontrol::NeutralMode::Brake); m_MotorLeftFollower.SetNeutralMode(ctre::phoenix::motorcontrol::NeutralMode::Brake); m_MotorLeftFollower2.SetNeutralMode(ctre::phoenix::motorcontrol::NeutralMode::Brake); @@ -73,24 +75,26 @@ void Robot::TeleopInit() { } void Robot::TeleopPeriodic() { - m_pidController.SetP(frc::SmartDashboard::GetNumber("P", 0.1)); + m_pidController.SetP(frc::SmartDashboard::GetNumber("P", 0)); m_pidController.SetI(frc::SmartDashboard::GetNumber("I", 0)); m_pidController.SetD(frc::SmartDashboard::GetNumber("D", 0)); - double z = std::abs(m_accelerometer.GetX()); // -1/1 - double y =m_accelerometer.GetY(); + double x = m_accelerometer.GetX(); // -1/1 + double y =std::abs(m_accelerometer.GetY()); + double z =m_accelerometer.GetZ(); - double output = m_pidController.Calculate(y, frc::SmartDashboard::GetNumber("Setpoint", 1)); + double output = m_pidController.Calculate(std::abs(x), frc::SmartDashboard::GetNumber("Setpoint", 1)); frc::SmartDashboard::PutNumber("Z", z); frc::SmartDashboard::PutNumber("Y", y); + frc::SmartDashboard::PutNumber("X",x); frc::SmartDashboard::PutNumber("Output", output); - frc::SmartDashboard::PutNumber("Angle", y); + frc::SmartDashboard::PutNumber("error",m_pidController.GetPositionError()); double angle = m_gyro.GetAngle(); frc::SmartDashboard::PutNumber("Angle", angle); - // frc::SmartDashboard::PutNumber("arcos accélé",std::acos(y)/3.14159265*180.0); + frc::SmartDashboard::PutNumber("arcos accélé",std::acos(x)/3.14159265*180.0); - double speed = signe(-angle)*output; + double speed = signe(angle)*std::clamp(output,-0.3,0.3); frc::SmartDashboard::PutNumber("speed",speed); if (m_joystickRight.GetRawButton(1)) { @@ -104,7 +108,11 @@ void Robot::TeleopPeriodic() { m_MotorLeft.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, Calcul_De_Notre_Brave_JM(-m_joystickLeft.GetY(), m_joystickRight.GetZ(), 1)); } - + if (m_joystickLeft.GetRawButton(1)) + { + m_pidController.Reset(); + m_gyro.Reset(); + } } From 78124a9f671ebba229fceccd25fd6c56d63ca715 Mon Sep 17 00:00:00 2001 From: mimi980 <90112270+mimi980@users.noreply.github.com> Date: Thu, 9 Feb 2023 22:34:58 +0100 Subject: [PATCH 13/36] add rollingaverage --- .vscode/settings.json | 73 ++++++++++- src/main/cpp/Robot.cpp | 77 +++++++++--- src/main/cpp/lib/NRollingAverage.cpp | 165 +++++++++++++++++++++++++ src/main/cpp/lib/rate_limiter.cpp | 58 +++++++++ src/main/include/Robot.h | 18 ++- src/main/include/lib/Dynamic.h | 25 ++++ src/main/include/lib/NRollingAverage.h | 69 +++++++++++ src/main/include/lib/rate_limiter.h | 25 ++++ 8 files changed, 492 insertions(+), 18 deletions(-) create mode 100644 src/main/cpp/lib/NRollingAverage.cpp create mode 100644 src/main/cpp/lib/rate_limiter.cpp create mode 100644 src/main/include/lib/Dynamic.h create mode 100644 src/main/include/lib/NRollingAverage.h create mode 100644 src/main/include/lib/rate_limiter.h diff --git a/.vscode/settings.json b/.vscode/settings.json index da44c62..5113d54 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -16,6 +16,77 @@ }, "C_Cpp.default.configurationProvider": "vscode-wpilib", "files.associations": { - "*.inc": "cpp" + "*.inc": "cpp", + "array": "cpp", + "atomic": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "cctype": "cpp", + "cfenv": "cpp", + "chrono": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "codecvt": "cpp", + "compare": "cpp", + "complex": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdint": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "deque": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "regex": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "fstream": "cpp", + "future": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "numbers": "cpp", + "ostream": "cpp", + "semaphore": "cpp", + "span": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cinttypes": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "variant": "cpp" } } diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 6362c79..96b644f 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -44,10 +44,10 @@ void Robot::AutonomousInit() {} void Robot::AutonomousPeriodic() {} void Robot::TeleopInit() { - frc::SmartDashboard::PutNumber("P", 0); - frc::SmartDashboard::PutNumber("I", 0); - frc::SmartDashboard::PutNumber("D", 0); - frc::SmartDashboard::PutNumber("Setpoint", 0); + // frc::SmartDashboard::PutNumber("P", 0); + // frc::SmartDashboard::PutNumber("I", 0); + // frc::SmartDashboard::PutNumber("D", 0); + frc::SmartDashboard::PutNumber("Setpoint", 1); m_MotorLeft.SetInverted(true); m_MotorLeftFollower.SetInverted(true); @@ -75,27 +75,61 @@ void Robot::TeleopInit() { } void Robot::TeleopPeriodic() { + m_pidController.SetP(frc::SmartDashboard::GetNumber("P", 0)); m_pidController.SetI(frc::SmartDashboard::GetNumber("I", 0)); m_pidController.SetD(frc::SmartDashboard::GetNumber("D", 0)); + double x = m_accelerometer.GetX(); // -1/1 - double y =std::abs(m_accelerometer.GetY()); + double y =m_accelerometer.GetY(); double z =m_accelerometer.GetZ(); - double output = m_pidController.Calculate(std::abs(x), frc::SmartDashboard::GetNumber("Setpoint", 1)); - - frc::SmartDashboard::PutNumber("Z", z); - frc::SmartDashboard::PutNumber("Y", y); - frc::SmartDashboard::PutNumber("X",x); - frc::SmartDashboard::PutNumber("Output", output); - frc::SmartDashboard::PutNumber("error",m_pidController.GetPositionError()); double angle = m_gyro.GetAngle(); - frc::SmartDashboard::PutNumber("Angle", angle); - frc::SmartDashboard::PutNumber("arcos accélé",std::acos(x)/3.14159265*180.0); + m_AccelerometerX_Average.add(x); + double arcos_X = std::acos(m_AccelerometerX_Average.get())/3.14159265*180.0; + m_AccelerometerX_Arcos_Average.add(arcos_X); + + m_AccelerometerX.set(x); + m_AccelerometerY.set(y); + m_Gyro_Angle.set(angle); + + if (m_Gyro_Angle.m_delta > bruit) + { + m_Sum_Delta_Gyro_Angle += m_Gyro_Angle.m_delta; + } + else if (m_Gyro_Angle.m_delta < bruit) + { + m_Sum_Delta_Gyro_Angle += m_Gyro_Angle.m_delta; + } + + +frc::SmartDashboard::PutNumber("Accel_X",x); +frc::SmartDashboard::PutNumber("Accel_Y",y); +frc::SmartDashboard::PutNumber("Accel_Z",z); +frc::SmartDashboard::PutNumber("Angle_Gyro",angle); +frc::SmartDashboard::PutNumber("Accel_X_Average",m_AccelerometerX_Average.get()); +frc::SmartDashboard::PutNumber("Accel_Arcos_X",arcos_X); +frc::SmartDashboard::PutNumber("Accel_Arcos_X_Average",m_AccelerometerX_Arcos_Average.get()); +frc::SmartDashboard::PutNumber("Current_Dynamic_Accel_X",m_AccelerometerX.m_current); +frc::SmartDashboard::PutNumber("Current_Dynamic_Accel_Y",m_AccelerometerY.m_current); +frc::SmartDashboard::PutNumber("Current_Dynamic_Gryo",m_Gyro_Angle.m_current); +frc::SmartDashboard::PutNumber("Delta_Dynamic_Accel_X",m_AccelerometerX.m_delta); +frc::SmartDashboard::PutNumber("Delta_Dynamic_Accel_Y",m_AccelerometerY.m_delta); +frc::SmartDashboard::PutNumber("Delta_Dynamic_Gryo",m_Gyro_Angle.m_delta); +frc::SmartDashboard::PutNumber("Sum_Delta_Gyro_Angle",m_Sum_Delta_Gyro_Angle); + + + + + + + + + double output = m_pidController.Calculate(std::abs(x), frc::SmartDashboard::GetNumber("Setpoint", 1)); + double speed = signe(m_Sum_Delta_Gyro_Angle)*std::clamp(output,-0.3,0.3); + - double speed = signe(angle)*std::clamp(output,-0.3,0.3); - frc::SmartDashboard::PutNumber("speed",speed); if (m_joystickRight.GetRawButton(1)) { m_MotorRight.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); @@ -114,6 +148,17 @@ void Robot::TeleopPeriodic() { m_gyro.Reset(); } + frc::SmartDashboard::PutNumber("speed",speed); + // frc::SmartDashboard::PutNumber("Z", z); + // frc::SmartDashboard::PutNumber("Y", y); + // frc::SmartDashboard::PutNumber("X",x); + // frc::SmartDashboard::PutNumber("m_AccelerometerX_Average",m_AccelerometerX_Average.get()); + // frc::SmartDashboard::PutNumber("m_AccelerometerX_arcos_Average",m_AccelerometerX_Arcos_Average.get()); + frc::SmartDashboard::PutNumber("Output", output); + frc::SmartDashboard::PutNumber("error",m_pidController.GetPositionError()); + // frc::SmartDashboard::PutNumber("Angle", angle); + // frc::SmartDashboard::PutNumber("arcos accelerometer X average",arcos_X); + } diff --git a/src/main/cpp/lib/NRollingAverage.cpp b/src/main/cpp/lib/NRollingAverage.cpp new file mode 100644 index 0000000..2884a4f --- /dev/null +++ b/src/main/cpp/lib/NRollingAverage.cpp @@ -0,0 +1,165 @@ +#include "lib/NRollingAverage.h" + +NdoubleRollingAverage::NdoubleRollingAverage() +{ + m_last = 0; + m_pdouble = NULL; + m_sum = 0.0; + m_average = 0.0; + m_index = 0; +} + +NdoubleRollingAverage::NdoubleRollingAverage(const int table_size, const double initial_average) +{ + m_last = table_size - 1; + m_pdouble = (double*)malloc(sizeof(double)*table_size); + m_sum = initial_average*table_size; + m_average = initial_average; + m_index = 0; + + double *pd = m_pdouble; + for(int i= 0;i + + +double RateLimiter::Update(double target) { // on update le current + m_target = target; + + if(m_current < m_target - m_speed) // si le current est plus petit que le target - la vitesse + { + m_current += m_speed; // on ajoute la vitesse au current + } + else if (m_current > m_target + m_speed) // si le current est plus grand que le target + la vitesse + { + m_current -= m_speed; // on enleve la vitesse au current + } + else + { + m_current = m_target; // sinon le current est egal au target + } + return m_current; // on retourne le current +} + +double RateLimiter::Update() { + if(m_current < m_target - m_speed) // si le current est plus petit que le target - la vitesse + { + m_current += m_speed; // on ajoute la vitesse au current + } + else if (m_current > m_target + m_speed) // si le current est plus grand que le target + la vitesse + { + m_current -= m_speed; // on enleve la vitesse au current + } + else + { + m_current = m_target; // sinon le current est egal au target + } + return m_current; // on retourne le current +} +void RateLimiter::SetTarget(double target) { // on set le target + m_target = target; +} + +void RateLimiter::SetCurrent(double current) { // on set le current + m_current = current; +} + +void RateLimiter::SetSpeed (double speed) { // on set la vitesse + m_speed = speed; +} + +void RateLimiter::Reset(double target, double current, double speed) { // on reset tout + m_target = target; + m_current = current; + m_speed = speed; +} + +// void RateLimiter::SetRecul(double current, double speed_1, double speed_2) { // on set le recul +// m_current = current; +// m_recul = ((m_current - m_speedSwitch)/speed_2 )*speed_1; // on calcule le current du recul qui sera soustrait à une switch définie ? +// } \ No newline at end of file diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 4b0d55c..7466c81 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -19,6 +19,8 @@ #define NCLAMP(mn,a,mx) ( ((a)<(mn)) ? (mn) : ((a)>(mx)) ? (mx) : (a) ) #define NLERP(a,b,t) ( a + (b - a)*t ) +#define bruit 0.1 + #include #include #include @@ -29,7 +31,9 @@ #include #include #include -#include +#include +#include +#include //gyro @@ -69,4 +73,16 @@ class Robot : public frc::TimedRobot { ctre::phoenix::motorcontrol::can::TalonFX m_MotorLeftFollower{5}; ctre::phoenix::motorcontrol::can::TalonFX m_MotorLeftFollower2{6}; + + NdoubleRollingAverage m_AccelerometerX_Average{25}; + NdoubleRollingAverage m_AccelerometerX_Arcos_Average{25}; + + Dynamic m_AccelerometerX; + Dynamic m_AccelerometerY; + Dynamic m_Gyro_Angle; + + double m_Sum_Delta_Gyro_Angle=0.0; + + + }; diff --git a/src/main/include/lib/Dynamic.h b/src/main/include/lib/Dynamic.h new file mode 100644 index 0000000..72922f7 --- /dev/null +++ b/src/main/include/lib/Dynamic.h @@ -0,0 +1,25 @@ +#pragma once + +typedef struct Dynamic Dynamic; +struct Dynamic +{ + + Dynamic() : m_current(0.0), m_delta(0.0) {}; + Dynamic(double initialvalue) : m_current(initialvalue), m_delta(0.0) {}; + ~Dynamic() {}; + double m_current; + double m_delta; + + void set(double value) + { + m_delta = (value - m_current); + m_current = value; + }; + void reset(double initialvalue ) + { + m_current = initialvalue; + m_delta = 0.0; + }; + + +}; \ No newline at end of file diff --git a/src/main/include/lib/NRollingAverage.h b/src/main/include/lib/NRollingAverage.h new file mode 100644 index 0000000..1bd4caa --- /dev/null +++ b/src/main/include/lib/NRollingAverage.h @@ -0,0 +1,69 @@ +#ifndef __NROLLINGAVERAGE_H +#define __NROLLINGAVERAGE_H + +#include +// *************************************************************************************** +// *************************************************************************************** +// ** ** +// ** NRollingAverage.h ** +// ** ** +// *************************************************************************************** +// *************************************************************************************** +class NdoubleRollingAverage +{ +public: + NdoubleRollingAverage(); + NdoubleRollingAverage(const int table_size, const double initial_average = 0.0 ); + ~NdoubleRollingAverage(); + + void reset(const double initial_average = 0.0); + const double add(const double value); + inline const double get(){return m_average;} + + int m_last; + int m_index; + double m_average; + double m_sum; + double* m_pdouble; +}; + +class NfloatRollingAverage +{ +public: + NfloatRollingAverage(const unsigned short table_size, const float initial_average = 0.0f ); + ~NfloatRollingAverage(); + + void reset(const float initial_average = 0.0f); + const float add(const float value); + inline const float get(){return m_average;} + + unsigned short m_last; + unsigned short m_index; + float m_average; + float m_sum; + float *m_pfloat; +}; + +class NlongRollingAverage +{ +public: + NlongRollingAverage(const unsigned short table_size, const long initial_average = 0.0f ); + ~NlongRollingAverage(); + + void reset(const long initial_average = 0.0f); + const long add(const long value); + inline const long get(){return m_average;} + + unsigned short m_last; + unsigned short m_index; + long m_average; + long m_sum; + long *m_plong; +}; + + + + + +#endif // __NROLLINGAVERAGE_H + diff --git a/src/main/include/lib/rate_limiter.h b/src/main/include/lib/rate_limiter.h new file mode 100644 index 0000000..edc93ac --- /dev/null +++ b/src/main/include/lib/rate_limiter.h @@ -0,0 +1,25 @@ + + +class RateLimiter { + + public: + RateLimiter(): m_target(0.0), m_current(0.0), m_speed(0.0){}; + + double Update(double target); // redefini la target et Update la valeur de current + double Update(); // Update la valeur de current + void SetTarget(double target); // Redefini la valeur de la target + void SetCurrent(double current); // "force" la valeur de current + void SetSpeed (double speed); // defini la vitesse + void Reset(double target, double current, double speed); // Reset tout avec des valeurs que l'on choisit je sais pas si c'est ce que tu pensais +// void SetRecul(double current, double speed_1, double speed_2); // Set le recul + + double m_target; + double m_current; + double m_speed; + private: + +// double m_speedSwitch; +// double m_recul; + + +}; \ No newline at end of file From 931665a28cffb17d573b7de6aa280701995eb395 Mon Sep 17 00:00:00 2001 From: mimi980 <90112270+mimi980@users.noreply.github.com> Date: Fri, 10 Feb 2023 10:36:30 +0100 Subject: [PATCH 14/36] sumDelta --- src/main/cpp/Robot.cpp | 29 +++++++++++++++-------------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 96b644f..556a15f 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -104,20 +104,21 @@ void Robot::TeleopPeriodic() { } -frc::SmartDashboard::PutNumber("Accel_X",x); -frc::SmartDashboard::PutNumber("Accel_Y",y); -frc::SmartDashboard::PutNumber("Accel_Z",z); -frc::SmartDashboard::PutNumber("Angle_Gyro",angle); -frc::SmartDashboard::PutNumber("Accel_X_Average",m_AccelerometerX_Average.get()); -frc::SmartDashboard::PutNumber("Accel_Arcos_X",arcos_X); -frc::SmartDashboard::PutNumber("Accel_Arcos_X_Average",m_AccelerometerX_Arcos_Average.get()); -frc::SmartDashboard::PutNumber("Current_Dynamic_Accel_X",m_AccelerometerX.m_current); -frc::SmartDashboard::PutNumber("Current_Dynamic_Accel_Y",m_AccelerometerY.m_current); -frc::SmartDashboard::PutNumber("Current_Dynamic_Gryo",m_Gyro_Angle.m_current); -frc::SmartDashboard::PutNumber("Delta_Dynamic_Accel_X",m_AccelerometerX.m_delta); -frc::SmartDashboard::PutNumber("Delta_Dynamic_Accel_Y",m_AccelerometerY.m_delta); -frc::SmartDashboard::PutNumber("Delta_Dynamic_Gryo",m_Gyro_Angle.m_delta); -frc::SmartDashboard::PutNumber("Sum_Delta_Gyro_Angle",m_Sum_Delta_Gyro_Angle); + frc::SmartDashboard::PutNumber("Accel_X",x); + frc::SmartDashboard::PutNumber("Accel_Y",y); + frc::SmartDashboard::PutNumber("Accel_Z",z); + frc::SmartDashboard::PutNumber("Angle_Gyro",angle); + frc::SmartDashboard::PutNumber("Accel_X_Average",m_AccelerometerX_Average.get()); + frc::SmartDashboard::PutNumber("Accel_Arcos_X",arcos_X); + frc::SmartDashboard::PutNumber("Accel_Arcos_X_Average",m_AccelerometerX_Arcos_Average.get()); + frc::SmartDashboard::PutNumber("Current_Dynamic_Accel_X",m_AccelerometerX.m_current); + frc::SmartDashboard::PutNumber("Current_Dynamic_Accel_Y",m_AccelerometerY.m_current); + frc::SmartDashboard::PutNumber("Current_Dynamic_Gryo",m_Gyro_Angle.m_current); + frc::SmartDashboard::PutNumber("Delta_Dynamic_Accel_X",m_AccelerometerX.m_delta); + frc::SmartDashboard::PutNumber("Delta_Dynamic_Accel_Y",m_AccelerometerY.m_delta); + frc::SmartDashboard::PutNumber("Delta_Dynamic_Gryo",m_Gyro_Angle.m_delta); + frc::SmartDashboard::PutNumber("Sum_Delta_Gyro_Angle",m_Sum_Delta_Gyro_Angle); + frc::SmartDashboard::PutNumber("Signe",signe(m_Sum_Delta_Gyro_Angle)); From 9097826e57921761f7725ce761e4e9c4c9d01645 Mon Sep 17 00:00:00 2001 From: mimi980 <90112270+mimi980@users.noreply.github.com> Date: Fri, 10 Feb 2023 13:44:04 +0100 Subject: [PATCH 15/36] on a faim --- src/main/cpp/Robot.cpp | 50 ++++++++++++++++++++------------- src/main/cpp/lib/Angle_AG.cpp | 37 ++++++++++++++++++++++++ src/main/include/Robot.h | 9 ++++++ src/main/include/lib/Angle_AG.h | 28 ++++++++++++++++++ 4 files changed, 104 insertions(+), 20 deletions(-) create mode 100644 src/main/cpp/lib/Angle_AG.cpp create mode 100644 src/main/include/lib/Angle_AG.h diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 556a15f..9de7071 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -44,9 +44,9 @@ void Robot::AutonomousInit() {} void Robot::AutonomousPeriodic() {} void Robot::TeleopInit() { - // frc::SmartDashboard::PutNumber("P", 0); - // frc::SmartDashboard::PutNumber("I", 0); - // frc::SmartDashboard::PutNumber("D", 0); + frc::SmartDashboard::PutNumber("P", 0); + frc::SmartDashboard::PutNumber("I", 0); + frc::SmartDashboard::PutNumber("D", 0); frc::SmartDashboard::PutNumber("Setpoint", 1); m_MotorLeft.SetInverted(true); @@ -94,31 +94,42 @@ void Robot::TeleopPeriodic() { m_AccelerometerY.set(y); m_Gyro_Angle.set(angle); + m_FusAngle.Update(m_gyro.GetRate()/180.0*3.14159265,m_accelerometer.GetY(),m_accelerometer.GetX()); + + frc::SmartDashboard::PutNumber("FuseAngle",m_FusAngle.GetAngle()*180.0/3.14159265); + frc::SmartDashboard::PutNumber("m_K",m_FusAngle.m_k); + frc::SmartDashboard::PutNumber("m_dt",m_FusAngle.m_dt); + frc::SmartDashboard::PutNumber("m_tau",m_FusAngle.m_tau); + frc::SmartDashboard::PutNumber("m_a",m_FusAngle.m_a); + frc::SmartDashboard::PutNumber("m_angleAccel",m_FusAngle.m_angleAccel); + + + if (m_Gyro_Angle.m_delta > bruit) { m_Sum_Delta_Gyro_Angle += m_Gyro_Angle.m_delta; } - else if (m_Gyro_Angle.m_delta < bruit) + else if (m_Gyro_Angle.m_delta < -bruit) { m_Sum_Delta_Gyro_Angle += m_Gyro_Angle.m_delta; } - frc::SmartDashboard::PutNumber("Accel_X",x); - frc::SmartDashboard::PutNumber("Accel_Y",y); - frc::SmartDashboard::PutNumber("Accel_Z",z); - frc::SmartDashboard::PutNumber("Angle_Gyro",angle); - frc::SmartDashboard::PutNumber("Accel_X_Average",m_AccelerometerX_Average.get()); - frc::SmartDashboard::PutNumber("Accel_Arcos_X",arcos_X); - frc::SmartDashboard::PutNumber("Accel_Arcos_X_Average",m_AccelerometerX_Arcos_Average.get()); - frc::SmartDashboard::PutNumber("Current_Dynamic_Accel_X",m_AccelerometerX.m_current); - frc::SmartDashboard::PutNumber("Current_Dynamic_Accel_Y",m_AccelerometerY.m_current); - frc::SmartDashboard::PutNumber("Current_Dynamic_Gryo",m_Gyro_Angle.m_current); - frc::SmartDashboard::PutNumber("Delta_Dynamic_Accel_X",m_AccelerometerX.m_delta); - frc::SmartDashboard::PutNumber("Delta_Dynamic_Accel_Y",m_AccelerometerY.m_delta); - frc::SmartDashboard::PutNumber("Delta_Dynamic_Gryo",m_Gyro_Angle.m_delta); - frc::SmartDashboard::PutNumber("Sum_Delta_Gyro_Angle",m_Sum_Delta_Gyro_Angle); - frc::SmartDashboard::PutNumber("Signe",signe(m_Sum_Delta_Gyro_Angle)); + frc::SmartDashboard::PutNumber("Accel_X",x); + frc::SmartDashboard::PutNumber("Accel_Y",y); + frc::SmartDashboard::PutNumber("Accel_Z",z); + frc::SmartDashboard::PutNumber("Angle_Gyro",angle); + frc::SmartDashboard::PutNumber("Accel_X_Average",m_AccelerometerX_Average.get()); + frc::SmartDashboard::PutNumber("Accel_Arcos_X",arcos_X); + frc::SmartDashboard::PutNumber("Accel_Arcos_X_Average",m_AccelerometerX_Arcos_Average.get()); + // frc::SmartDashboard::PutNumber("Current_Dynamic_Accel_X",m_AccelerometerX.m_current); + // frc::SmartDashboard::PutNumber("Current_Dynamic_Accel_Y",m_AccelerometerY.m_current); + // frc::SmartDashboard::PutNumber("Current_Dynamic_Gryo",m_Gyro_Angle.m_current); + // frc::SmartDashboard::PutNumber("Delta_Dynamic_Accel_X",m_AccelerometerX.m_delta); + // frc::SmartDashboard::PutNumber("Delta_Dynamic_Accel_Y",m_AccelerometerY.m_delta); + frc::SmartDashboard::PutNumber("Delta_Dynamic_Gryo",m_Gyro_Angle.m_delta); + frc::SmartDashboard::PutNumber("Sum_Delta_Gyro_Angle",m_Sum_Delta_Gyro_Angle); + frc::SmartDashboard::PutNumber("Signe",signe(m_Sum_Delta_Gyro_Angle)); @@ -146,7 +157,6 @@ void Robot::TeleopPeriodic() { if (m_joystickLeft.GetRawButton(1)) { m_pidController.Reset(); - m_gyro.Reset(); } frc::SmartDashboard::PutNumber("speed",speed); diff --git a/src/main/cpp/lib/Angle_AG.cpp b/src/main/cpp/lib/Angle_AG.cpp new file mode 100644 index 0000000..4905875 --- /dev/null +++ b/src/main/cpp/lib/Angle_AG.cpp @@ -0,0 +1,37 @@ +#include + +Angle_AG::Angle_AG(double dt,double tau,double angle) : m_angle(angle), m_dt(dt),m_tau(tau) { + m_k=tau/(tau+dt); +} + +//rate_gyro -> rate du gyro en radian/sec +//xaccel et zaccel -> base ortonormée directe avec z pointant vers le haut et x axe horizontale +double Angle_AG::Update(double rate_gyro, double xaccel, double zaccel) +{ + m_a =NCLAMP(-1.0,zaccel,1.0); + m_b=NCLAMP(-1.0,xaccel,1.0); + // m_angleAccel = acos(m_a); + m_angleAccel =atan2(m_b,m_a); + m_angle=m_k *(m_angle+rate_gyro*m_dt)+(1.0-m_k)*(m_angleAccel); + return m_angle; + + +//doc : +// a=tau / (tau + loop time) +// newAngle = angle measured with atan2 using the accelerometer +// newRate = angle measured using the gyro +// looptime = loop time in millis() +// float tau=0.075; +// a=0.0; +// float Complementary(float newAngle, float newRate,int looptime) { +// float dtC = float(looptime)/1000.0; +// a=tau/(tau+dtC); +// x_angleC= a* (x_angleC + newRate * dtC) + (1-a) * (newAngle); +// return x_angleC; + +} + +void Angle_AG::Reset(double angle) +{ + m_angle=angle; +} \ No newline at end of file diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 7466c81..301de55 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -34,6 +34,8 @@ #include #include #include +#include +#include //gyro @@ -58,6 +60,8 @@ class Robot : public frc::TimedRobot { double signe(double x); double Calcul_De_Notre_Brave_JM(double forward, double turn, bool wheelSide); + double GetAngle(); + private: frc::Joystick m_joystickRight{0}; @@ -82,6 +86,11 @@ class Robot : public frc::TimedRobot { Dynamic m_Gyro_Angle; double m_Sum_Delta_Gyro_Angle=0.0; + + frc::Encoder m_EncoderRight{1,2}; + frc::Encoder m_EncoderLeft{3,4}; + + Angle_AG m_FusAngle{0.02,0.075}; diff --git a/src/main/include/lib/Angle_AG.h b/src/main/include/lib/Angle_AG.h new file mode 100644 index 0000000..ab948be --- /dev/null +++ b/src/main/include/lib/Angle_AG.h @@ -0,0 +1,28 @@ +#include +#include +#include + +#define NCLAMP(mn,a,mx) ((a)<(mn)) ? (mn) : ((a)>(mx)) ? (mx) :(a) + +class Angle_AG +{ + public : + Angle_AG(double dt,double tau,double angle=0.0); + double Update(double rate_gyro, double xaccel,double zaccel); // x correspond à la verticale vers le haut et y à l'horizontale vers l'avant et z l'ordonée + double GetAngle(){return m_angle;}; + void Reset(double angle =0.0); + double m_k; // coef calculé grace à m_tau et dt + double m_tau; // temp en seconde + double m_dt; // dela t en seconde + double m_a; + double m_b; + double m_angleAccel; + + + + + private : + double m_angle; // angle en radian combinant accel et gyro + + +}; \ No newline at end of file From 73ac7472dec93a19161f44d614c4cb37966ed97a Mon Sep 17 00:00:00 2001 From: mimi980 <90112270+mimi980@users.noreply.github.com> Date: Fri, 10 Feb 2023 14:26:05 +0100 Subject: [PATCH 16/36] m_tau devient autonome --- src/main/cpp/Robot.cpp | 48 ++++----------------------------- src/main/include/Robot.h | 3 ++- src/main/include/lib/Angle_AG.h | 1 + 3 files changed, 8 insertions(+), 44 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 9de7071..05c0b9e 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -4,16 +4,6 @@ #include "Robot.h" -double Robot::signe(double x) { - if (x > 0) { - return 1; - } else if (x < 0) { - return -1; - } else { - return 0; - } -} - double Robot::Calcul_De_Notre_Brave_JM(double forward, double turn, bool wheelSide) // calcule la vitesse des roues { double m_forward = forward; @@ -34,7 +24,7 @@ double Robot::Calcul_De_Notre_Brave_JM(double forward, double turn, bool wheelSi } void Robot::RobotInit() { - //frc::ADXRS450_Gyro(frc::SPI::Port::kOnboardCS0); + frc::ADXRS450_Gyro(frc::SPI::Port::kOnboardCS0); m_gyro.Reset(); m_gyro.Calibrate(); } @@ -48,6 +38,7 @@ void Robot::TeleopInit() { frc::SmartDashboard::PutNumber("I", 0); frc::SmartDashboard::PutNumber("D", 0); frc::SmartDashboard::PutNumber("Setpoint", 1); + frc::SmartDashboard::PutNumber("m_tau", 0.075); m_MotorLeft.SetInverted(true); m_MotorLeftFollower.SetInverted(true); @@ -95,41 +86,19 @@ void Robot::TeleopPeriodic() { m_Gyro_Angle.set(angle); m_FusAngle.Update(m_gyro.GetRate()/180.0*3.14159265,m_accelerometer.GetY(),m_accelerometer.GetX()); + m_FusAngle.SetTau(frc::SmartDashboard::GetNumber("m_tau",0.075)); frc::SmartDashboard::PutNumber("FuseAngle",m_FusAngle.GetAngle()*180.0/3.14159265); frc::SmartDashboard::PutNumber("m_K",m_FusAngle.m_k); frc::SmartDashboard::PutNumber("m_dt",m_FusAngle.m_dt); - frc::SmartDashboard::PutNumber("m_tau",m_FusAngle.m_tau); frc::SmartDashboard::PutNumber("m_a",m_FusAngle.m_a); frc::SmartDashboard::PutNumber("m_angleAccel",m_FusAngle.m_angleAccel); - - - if (m_Gyro_Angle.m_delta > bruit) - { - m_Sum_Delta_Gyro_Angle += m_Gyro_Angle.m_delta; - } - else if (m_Gyro_Angle.m_delta < -bruit) - { - m_Sum_Delta_Gyro_Angle += m_Gyro_Angle.m_delta; - } - - frc::SmartDashboard::PutNumber("Accel_X",x); frc::SmartDashboard::PutNumber("Accel_Y",y); frc::SmartDashboard::PutNumber("Accel_Z",z); frc::SmartDashboard::PutNumber("Angle_Gyro",angle); - frc::SmartDashboard::PutNumber("Accel_X_Average",m_AccelerometerX_Average.get()); - frc::SmartDashboard::PutNumber("Accel_Arcos_X",arcos_X); - frc::SmartDashboard::PutNumber("Accel_Arcos_X_Average",m_AccelerometerX_Arcos_Average.get()); - // frc::SmartDashboard::PutNumber("Current_Dynamic_Accel_X",m_AccelerometerX.m_current); - // frc::SmartDashboard::PutNumber("Current_Dynamic_Accel_Y",m_AccelerometerY.m_current); - // frc::SmartDashboard::PutNumber("Current_Dynamic_Gryo",m_Gyro_Angle.m_current); - // frc::SmartDashboard::PutNumber("Delta_Dynamic_Accel_X",m_AccelerometerX.m_delta); - // frc::SmartDashboard::PutNumber("Delta_Dynamic_Accel_Y",m_AccelerometerY.m_delta); - frc::SmartDashboard::PutNumber("Delta_Dynamic_Gryo",m_Gyro_Angle.m_delta); - frc::SmartDashboard::PutNumber("Sum_Delta_Gyro_Angle",m_Sum_Delta_Gyro_Angle); - frc::SmartDashboard::PutNumber("Signe",signe(m_Sum_Delta_Gyro_Angle)); + frc::SmartDashboard::PutNumber("Angle_Accel_X",arcos_X); @@ -139,7 +108,7 @@ void Robot::TeleopPeriodic() { double output = m_pidController.Calculate(std::abs(x), frc::SmartDashboard::GetNumber("Setpoint", 1)); - double speed = signe(m_Sum_Delta_Gyro_Angle)*std::clamp(output,-0.3,0.3); + double speed = signe(m_FusAngle.GetAngle())*std::clamp(output,-0.3,0.3); if (m_joystickRight.GetRawButton(1)) @@ -160,15 +129,8 @@ void Robot::TeleopPeriodic() { } frc::SmartDashboard::PutNumber("speed",speed); - // frc::SmartDashboard::PutNumber("Z", z); - // frc::SmartDashboard::PutNumber("Y", y); - // frc::SmartDashboard::PutNumber("X",x); - // frc::SmartDashboard::PutNumber("m_AccelerometerX_Average",m_AccelerometerX_Average.get()); - // frc::SmartDashboard::PutNumber("m_AccelerometerX_arcos_Average",m_AccelerometerX_Arcos_Average.get()); frc::SmartDashboard::PutNumber("Output", output); frc::SmartDashboard::PutNumber("error",m_pidController.GetPositionError()); - // frc::SmartDashboard::PutNumber("Angle", angle); - // frc::SmartDashboard::PutNumber("arcos accelerometer X average",arcos_X); } diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 301de55..50f8927 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -20,6 +20,7 @@ #define NLERP(a,b,t) ( a + (b - a)*t ) #define bruit 0.1 +#define signe(a) (((a) < 0) ? -1:1) #include #include @@ -57,7 +58,7 @@ class Robot : public frc::TimedRobot { void TestPeriodic() override; - double signe(double x); + // double signe(double x); double Calcul_De_Notre_Brave_JM(double forward, double turn, bool wheelSide); double GetAngle(); diff --git a/src/main/include/lib/Angle_AG.h b/src/main/include/lib/Angle_AG.h index ab948be..73a76f0 100644 --- a/src/main/include/lib/Angle_AG.h +++ b/src/main/include/lib/Angle_AG.h @@ -11,6 +11,7 @@ class Angle_AG double Update(double rate_gyro, double xaccel,double zaccel); // x correspond à la verticale vers le haut et y à l'horizontale vers l'avant et z l'ordonée double GetAngle(){return m_angle;}; void Reset(double angle =0.0); + void SetTau(double tau){m_tau=tau;}; double m_k; // coef calculé grace à m_tau et dt double m_tau; // temp en seconde double m_dt; // dela t en seconde From 3b2b40a77fa65c78a058f0ba678277329442fbb2 Mon Sep 17 00:00:00 2001 From: mimi980 <90112270+mimi980@users.noreply.github.com> Date: Fri, 10 Feb 2023 17:07:52 +0100 Subject: [PATCH 17/36] classe pid --- src/main/cpp/Robot.cpp | 26 +++++--------------------- src/main/cpp/lib/Angle_AG.cpp | 10 +++++----- src/main/cpp/lib/Pid.cpp | 25 +++++++++++++++++++++++++ src/main/include/Robot.h | 2 ++ src/main/include/lib/Angle_AG.h | 24 ++++++++++-------------- src/main/include/lib/Pid.h | 22 ++++++++++++++++++++++ 6 files changed, 69 insertions(+), 40 deletions(-) create mode 100644 src/main/cpp/lib/Pid.cpp create mode 100644 src/main/include/lib/Pid.h diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 05c0b9e..3e32bd2 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -24,7 +24,6 @@ double Robot::Calcul_De_Notre_Brave_JM(double forward, double turn, bool wheelSi } void Robot::RobotInit() { - frc::ADXRS450_Gyro(frc::SPI::Port::kOnboardCS0); m_gyro.Reset(); m_gyro.Calibrate(); } @@ -37,7 +36,6 @@ void Robot::TeleopInit() { frc::SmartDashboard::PutNumber("P", 0); frc::SmartDashboard::PutNumber("I", 0); frc::SmartDashboard::PutNumber("D", 0); - frc::SmartDashboard::PutNumber("Setpoint", 1); frc::SmartDashboard::PutNumber("m_tau", 0.075); m_MotorLeft.SetInverted(true); @@ -64,6 +62,9 @@ void Robot::TeleopInit() { m_MotorRightFollower.SetNeutralMode(ctre::phoenix::motorcontrol::NeutralMode::Brake); m_MotorRightFollower2.SetNeutralMode(ctre::phoenix::motorcontrol::NeutralMode::Brake); + m_pidController.SetTolerance(1.5); + + } void Robot::TeleopPeriodic() { @@ -73,13 +74,8 @@ void Robot::TeleopPeriodic() { double x = m_accelerometer.GetX(); // -1/1 double y =m_accelerometer.GetY(); - double z =m_accelerometer.GetZ(); - double angle = m_gyro.GetAngle(); - m_AccelerometerX_Average.add(x); - double arcos_X = std::acos(m_AccelerometerX_Average.get())/3.14159265*180.0; - m_AccelerometerX_Arcos_Average.add(arcos_X); m_AccelerometerX.set(x); m_AccelerometerY.set(y); @@ -90,25 +86,13 @@ void Robot::TeleopPeriodic() { frc::SmartDashboard::PutNumber("FuseAngle",m_FusAngle.GetAngle()*180.0/3.14159265); frc::SmartDashboard::PutNumber("m_K",m_FusAngle.m_k); - frc::SmartDashboard::PutNumber("m_dt",m_FusAngle.m_dt); - frc::SmartDashboard::PutNumber("m_a",m_FusAngle.m_a); - frc::SmartDashboard::PutNumber("m_angleAccel",m_FusAngle.m_angleAccel); - frc::SmartDashboard::PutNumber("Accel_X",x); frc::SmartDashboard::PutNumber("Accel_Y",y); - frc::SmartDashboard::PutNumber("Accel_Z",z); frc::SmartDashboard::PutNumber("Angle_Gyro",angle); - frc::SmartDashboard::PutNumber("Angle_Accel_X",arcos_X); - - - - - - - double output = m_pidController.Calculate(std::abs(x), frc::SmartDashboard::GetNumber("Setpoint", 1)); - double speed = signe(m_FusAngle.GetAngle())*std::clamp(output,-0.3,0.3); + double output = m_pidController.Calculate(m_FusAngle.GetAngle()*180.0/3.14159265, 0.0); + double speed = std::clamp(output,-0.3,0.3); if (m_joystickRight.GetRawButton(1)) diff --git a/src/main/cpp/lib/Angle_AG.cpp b/src/main/cpp/lib/Angle_AG.cpp index 4905875..c664c5b 100644 --- a/src/main/cpp/lib/Angle_AG.cpp +++ b/src/main/cpp/lib/Angle_AG.cpp @@ -8,11 +8,11 @@ Angle_AG::Angle_AG(double dt,double tau,double angle) : m_angle(angle), m_dt(dt) //xaccel et zaccel -> base ortonormée directe avec z pointant vers le haut et x axe horizontale double Angle_AG::Update(double rate_gyro, double xaccel, double zaccel) { - m_a =NCLAMP(-1.0,zaccel,1.0); - m_b=NCLAMP(-1.0,xaccel,1.0); - // m_angleAccel = acos(m_a); - m_angleAccel =atan2(m_b,m_a); - m_angle=m_k *(m_angle+rate_gyro*m_dt)+(1.0-m_k)*(m_angleAccel); + double a = NCLAMP(-1.0,zaccel,1.0); + double b = NCLAMP(-1.0,xaccel,1.0); + // m_angleAccel = acos(a); + double angleaccel = atan2(b,a); + m_angle = m_k *(m_angle+rate_gyro*m_dt)+(1.0-m_k)*(angleaccel); return m_angle; diff --git a/src/main/cpp/lib/Pid.cpp b/src/main/cpp/lib/Pid.cpp new file mode 100644 index 0000000..48eea80 --- /dev/null +++ b/src/main/cpp/lib/Pid.cpp @@ -0,0 +1,25 @@ +#include + +Pid::Pid(double setpoint, double kp, double ki, double kd) : m_setpoint(0.0), m_kp(0.0), m_ki(0.0), m_kd(0.0){} + +void Pid::SetGains(double kp, double ki, double kd) +{ + m_kp = kp; + m_ki = ki; + m_kd = kd; +} + +void Pid::SetSetpoint(double setpoint) +{ + m_setpoint = setpoint; +} + +double Pid::Update(double measurement) +{ + m_error = m_setpoint - measurement; // calcul de l'erreur entre la consigne et la mesure + m_integrative += m_error; // ajout de l'erreur + m_derivative = m_error - m_lastError; // calcul de la dérivée de l'erreur + m_lastError = m_error; // sauvegarde de l'erreur précédente + m_output = m_kp * m_error + m_ki * m_integrative + m_kd * m_derivative; // calcul de l'output + return m_output; +} \ No newline at end of file diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 50f8927..ecb39aa 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -82,6 +82,8 @@ class Robot : public frc::TimedRobot { NdoubleRollingAverage m_AccelerometerX_Average{25}; NdoubleRollingAverage m_AccelerometerX_Arcos_Average{25}; + NdoubleRollingAverage m_error{10}; + Dynamic m_AccelerometerX; Dynamic m_AccelerometerY; Dynamic m_Gyro_Angle; diff --git a/src/main/include/lib/Angle_AG.h b/src/main/include/lib/Angle_AG.h index 73a76f0..c807db9 100644 --- a/src/main/include/lib/Angle_AG.h +++ b/src/main/include/lib/Angle_AG.h @@ -7,23 +7,19 @@ class Angle_AG { public : - Angle_AG(double dt,double tau,double angle=0.0); - double Update(double rate_gyro, double xaccel,double zaccel); // x correspond à la verticale vers le haut et y à l'horizontale vers l'avant et z l'ordonée - double GetAngle(){return m_angle;}; - void Reset(double angle =0.0); - void SetTau(double tau){m_tau=tau;}; - double m_k; // coef calculé grace à m_tau et dt - double m_tau; // temp en seconde - double m_dt; // dela t en seconde - double m_a; - double m_b; - double m_angleAccel; - + Angle_AG(double dt,double tau,double angle=0.0); + double Update(double rate_gyro, double xaccel,double zaccel); // x correspond à la verticale vers le haut et y à l'horizontale vers l'avant et z l'ordonée + double GetAngle(){return m_angle;}; + void Reset(double angle =0.0); + void SetTau(double tau){m_tau=tau; m_k=m_tau/(m_tau+m_dt);}; + double m_k; // coef calculé grace à m_tau et dt private : - double m_angle; // angle en radian combinant accel et gyro - + double m_angle; // angle en radian combinant accel et gyro + double m_dt; // dela t en seconde + double m_tau; // temp en seconde + }; \ No newline at end of file diff --git a/src/main/include/lib/Pid.h b/src/main/include/lib/Pid.h new file mode 100644 index 0000000..850e189 --- /dev/null +++ b/src/main/include/lib/Pid.h @@ -0,0 +1,22 @@ +class Pid +{ + Pid(double setpoint, double kp, double ki, double kd); + + public: + + void SetGains(double kp, double ki, double kd); // définit les gains + void SetSetpoint(double setpoint); // définit la consigne + double Update(double measurement); // retourne l'output à appliquer measurement en degrés + + private: + + double m_output; // output du pid + double m_error; // erreur entre la consigne et la mesure + double m_lastError; // erreur précédente + double m_setpoint; // consigne + double m_integrative; // intégrale de l'erreur + double m_derivative; // dérivée de l'erreur + double m_kp; // gain proportionnel + double m_ki; // gain intégral + double m_kd; // gain dérivé +}; \ No newline at end of file From db0992d43a3db9011e3ed7072badf635bdd2f79c Mon Sep 17 00:00:00 2001 From: mimi980 <90112270+mimi980@users.noreply.github.com> Date: Fri, 10 Feb 2023 17:26:25 +0100 Subject: [PATCH 18/36] tolerance PID --- src/main/cpp/Robot.cpp | 13 ++++++------- src/main/cpp/lib/Pid.cpp | 19 ++++++++++++------- src/main/include/Robot.h | 11 ++++++++--- src/main/include/lib/Pid.h | 14 +++++++++++--- 4 files changed, 37 insertions(+), 20 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 3e32bd2..d96bf2e 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -62,15 +62,14 @@ void Robot::TeleopInit() { m_MotorRightFollower.SetNeutralMode(ctre::phoenix::motorcontrol::NeutralMode::Brake); m_MotorRightFollower2.SetNeutralMode(ctre::phoenix::motorcontrol::NeutralMode::Brake); - m_pidController.SetTolerance(1.5); + m_PidController.SetTolerance(1.5); } void Robot::TeleopPeriodic() { - m_pidController.SetP(frc::SmartDashboard::GetNumber("P", 0)); - m_pidController.SetI(frc::SmartDashboard::GetNumber("I", 0)); - m_pidController.SetD(frc::SmartDashboard::GetNumber("D", 0)); + m_PidController.SetGains(frc::SmartDashboard::GetNumber("P", 0),frc::SmartDashboard::GetNumber("I", 0),frc::SmartDashboard::GetNumber("D", 0)); + double x = m_accelerometer.GetX(); // -1/1 double y =m_accelerometer.GetY(); @@ -91,10 +90,11 @@ void Robot::TeleopPeriodic() { frc::SmartDashboard::PutNumber("Angle_Gyro",angle); - double output = m_pidController.Calculate(m_FusAngle.GetAngle()*180.0/3.14159265, 0.0); + double output = m_PidController.Calculate(m_FusAngle.GetAngle()*180.0/3.14159265); double speed = std::clamp(output,-0.3,0.3); + if (m_joystickRight.GetRawButton(1)) { m_MotorRight.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, speed); @@ -109,12 +109,11 @@ void Robot::TeleopPeriodic() { } if (m_joystickLeft.GetRawButton(1)) { - m_pidController.Reset(); + m_PidController.Reset(); } frc::SmartDashboard::PutNumber("speed",speed); frc::SmartDashboard::PutNumber("Output", output); - frc::SmartDashboard::PutNumber("error",m_pidController.GetPositionError()); } diff --git a/src/main/cpp/lib/Pid.cpp b/src/main/cpp/lib/Pid.cpp index 48eea80..fc28ce7 100644 --- a/src/main/cpp/lib/Pid.cpp +++ b/src/main/cpp/lib/Pid.cpp @@ -9,17 +9,22 @@ void Pid::SetGains(double kp, double ki, double kd) m_kd = kd; } -void Pid::SetSetpoint(double setpoint) -{ - m_setpoint = setpoint; -} - -double Pid::Update(double measurement) +double Pid::Calculate(double measurement) { m_error = m_setpoint - measurement; // calcul de l'erreur entre la consigne et la mesure m_integrative += m_error; // ajout de l'erreur m_derivative = m_error - m_lastError; // calcul de la dérivée de l'erreur m_lastError = m_error; // sauvegarde de l'erreur précédente m_output = m_kp * m_error + m_ki * m_integrative + m_kd * m_derivative; // calcul de l'output + if (abs(m_error) < m_tolerance){m_output = 0.0; }// si l'erreur est inférieure à la tolérance on arrête le moteur return m_output; -} \ No newline at end of file +} + +void Pid::Reset(double error,double lastError,double integrative,double derivative,double output) +{ + m_error = error; + m_lastError = lastError; + m_integrative = integrative; + m_derivative = derivative; + m_output = output; +} diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index ecb39aa..b0a63b0 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -37,6 +37,10 @@ #include #include #include +#include +#include +#include +#include //gyro @@ -68,7 +72,7 @@ class Robot : public frc::TimedRobot { frc::Joystick m_joystickRight{0}; frc::Joystick m_joystickLeft{1}; frc::BuiltInAccelerometer m_accelerometer{}; - frc::PIDController m_pidController{0, 0, 0}; + // frc::PIDController m_pidController{0, 0, 0}; frc::ADXRS450_Gyro m_gyro{frc::SPI::Port::kOnboardCS0}; ctre::phoenix::motorcontrol::can::TalonFX m_MotorRight{1}; @@ -90,10 +94,11 @@ class Robot : public frc::TimedRobot { double m_Sum_Delta_Gyro_Angle=0.0; - frc::Encoder m_EncoderRight{1,2}; - frc::Encoder m_EncoderLeft{3,4}; + frc::Encoder m_EncoderRight{0,1}; + frc::Encoder m_EncoderLeft{2,3}; Angle_AG m_FusAngle{0.02,0.075}; + Pid m_PidController{0.0,0.01,0.0005,0.0}; diff --git a/src/main/include/lib/Pid.h b/src/main/include/lib/Pid.h index 850e189..c4dae0b 100644 --- a/src/main/include/lib/Pid.h +++ b/src/main/include/lib/Pid.h @@ -1,12 +1,19 @@ +#include +#include +#include + class Pid { - Pid(double setpoint, double kp, double ki, double kd); public: + Pid(double setpoint, double kp, double ki, double kd); void SetGains(double kp, double ki, double kd); // définit les gains - void SetSetpoint(double setpoint); // définit la consigne - double Update(double measurement); // retourne l'output à appliquer measurement en degrés + void SetSetpoint(double setpoint){m_setpoint=setpoint;}; // définit la consigne + double Calculate(double measurement); // retourne l'output à appliquer measurement en degrés + void Reset(double error=0.0,double lastError=0.0,double integrative=0.0,double derivative=0.0,double output=0.0); // réinitialise le pid + void SetTolerance(double tolerance){m_tolerance=tolerance;}; + private: @@ -16,6 +23,7 @@ class Pid double m_setpoint; // consigne double m_integrative; // intégrale de l'erreur double m_derivative; // dérivée de l'erreur + double m_tolerance; // tolérance de la mesure double m_kp; // gain proportionnel double m_ki; // gain intégral double m_kd; // gain dérivé From d11df34c7dedd22e9db4bb306a4cfb675de35202 Mon Sep 17 00:00:00 2001 From: mimi980 <90112270+mimi980@users.noreply.github.com> Date: Fri, 10 Feb 2023 20:43:44 +0100 Subject: [PATCH 19/36] mettre 1/3 de distance --- src/main/cpp/Robot.cpp | 78 ++++++++++++++++++++++++++++++-------- src/main/include/Robot.h | 23 ++++++++++- src/main/include/lib/Pid.h | 3 +- 3 files changed, 86 insertions(+), 18 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index d96bf2e..11f7e32 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -36,7 +36,7 @@ void Robot::TeleopInit() { frc::SmartDashboard::PutNumber("P", 0); frc::SmartDashboard::PutNumber("I", 0); frc::SmartDashboard::PutNumber("D", 0); - frc::SmartDashboard::PutNumber("m_tau", 0.075); + frc::SmartDashboard::PutNumber("m_tau", 0.5); m_MotorLeft.SetInverted(true); m_MotorLeftFollower.SetInverted(true); @@ -63,12 +63,20 @@ void Robot::TeleopInit() { m_MotorRightFollower2.SetNeutralMode(ctre::phoenix::motorcontrol::NeutralMode::Brake); m_PidController.SetTolerance(1.5); + m_state=State::End; + m_EncoderLeft.SetDistancePerPulse(1.0/2048.0); } void Robot::TeleopPeriodic() { - m_PidController.SetGains(frc::SmartDashboard::GetNumber("P", 0),frc::SmartDashboard::GetNumber("I", 0),frc::SmartDashboard::GetNumber("D", 0)); + + + + + + double a=0.0; + double ratio_distance=0.0; double x = m_accelerometer.GetX(); // -1/1 @@ -81,36 +89,74 @@ void Robot::TeleopPeriodic() { m_Gyro_Angle.set(angle); m_FusAngle.Update(m_gyro.GetRate()/180.0*3.14159265,m_accelerometer.GetY(),m_accelerometer.GetX()); - m_FusAngle.SetTau(frc::SmartDashboard::GetNumber("m_tau",0.075)); + m_FusAngle.SetTau(frc::SmartDashboard::GetNumber("m_tau",0.5)); - frc::SmartDashboard::PutNumber("FuseAngle",m_FusAngle.GetAngle()*180.0/3.14159265); - frc::SmartDashboard::PutNumber("m_K",m_FusAngle.m_k); - frc::SmartDashboard::PutNumber("Accel_X",x); - frc::SmartDashboard::PutNumber("Accel_Y",y); - frc::SmartDashboard::PutNumber("Angle_Gyro",angle); + // frc::SmartDashboard::PutNumber("FuseAngle",m_FusAngle.GetAngle()*180.0/3.14159265); + // frc::SmartDashboard::PutNumber("m_K",m_FusAngle.m_k); + // frc::SmartDashboard::PutNumber("Accel_X",x); + // frc::SmartDashboard::PutNumber("Accel_Y",y); + // frc::SmartDashboard::PutNumber("Angle_Gyro",angle); double output = m_PidController.Calculate(m_FusAngle.GetAngle()*180.0/3.14159265); double speed = std::clamp(output,-0.3,0.3); - - if (m_joystickRight.GetRawButton(1)) { + m_distanceAparcourir = CHARGE_STATION_WIDTH;// à mettre + m_distanceParcourue = 0.0; + m_distanceRestante=m_distanceAparcourir-m_distanceParcourue; + m_encoder_origine=abs(m_EncoderLeft.GetDistance()*(3.0*0.0254)*(2.0*3.14159265)); + m_PidController.Reset(); + m_PidController.SetSetpoint(0); + m_signe_error = signe(m_PidController.m_error); + m_state= State::Adjusting; + } + frc::SmartDashboard::PutNumber("m_distanceParcourue",m_distanceParcourue); + frc::SmartDashboard::PutNumber("m_distanceRestante",m_distanceRestante); +switch (m_state) + { + case Adjusting: + std::cout<<("Adjusting")< #include @@ -67,6 +68,16 @@ class Robot : public frc::TimedRobot { double GetAngle(); + //machine à état + enum State + { + Init, + Adjusting, + End + }; + + State m_state=State::End; + private: frc::Joystick m_joystickRight{0}; @@ -99,7 +110,17 @@ class Robot : public frc::TimedRobot { Angle_AG m_FusAngle{0.02,0.075}; Pid m_PidController{0.0,0.01,0.0005,0.0}; - + + double m_kPmin = 0.0075; + double m_kPmax = 0.02; + + double m_distanceParcourue = 0.0; + double m_distanceRestante = 0.0; + double m_distanceAparcourir = 0.0; + + double m_signe_error; + double m_encoder_origine; + }; diff --git a/src/main/include/lib/Pid.h b/src/main/include/lib/Pid.h index c4dae0b..d81c423 100644 --- a/src/main/include/lib/Pid.h +++ b/src/main/include/lib/Pid.h @@ -13,12 +13,13 @@ class Pid double Calculate(double measurement); // retourne l'output à appliquer measurement en degrés void Reset(double error=0.0,double lastError=0.0,double integrative=0.0,double derivative=0.0,double output=0.0); // réinitialise le pid void SetTolerance(double tolerance){m_tolerance=tolerance;}; + double m_error; // erreur entre la consigne et la mesure + private: double m_output; // output du pid - double m_error; // erreur entre la consigne et la mesure double m_lastError; // erreur précédente double m_setpoint; // consigne double m_integrative; // intégrale de l'erreur From da06e6356056f0de276081c1896e2c48215118b9 Mon Sep 17 00:00:00 2001 From: jjTerazan <47304610+jjTerazan@users.noreply.github.com> Date: Sat, 11 Feb 2023 09:03:11 +0100 Subject: [PATCH 20/36] =?UTF-8?q?code=20corrig=C3=A9=20+=20ajout=20de=20la?= =?UTF-8?q?=20classe=20TiltTracker?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/cpp/ChargeStationTiltTracker.cpp | 37 +++++++++++++++++++++ src/main/include/ChargeStationTiltTracker.h | 21 ++++++++++++ 2 files changed, 58 insertions(+) create mode 100644 src/main/cpp/ChargeStationTiltTracker.cpp create mode 100644 src/main/include/ChargeStationTiltTracker.h diff --git a/src/main/cpp/ChargeStationTiltTracker.cpp b/src/main/cpp/ChargeStationTiltTracker.cpp new file mode 100644 index 0000000..740a7f4 --- /dev/null +++ b/src/main/cpp/ChargeStationTiltTracker.cpp @@ -0,0 +1,37 @@ +#include "lib/RblUtils.h" +#include "ChargeStationTiltTracker.h" + +/// @brief +TiltTracker::TiltTracker():m_tiltA(0.0),m_tiltB(0.0),m_pTilt(&m_tiltB), m_angle(0.0),m_angleThreshold(0.0){} + +TiltTracker::TiltTracker(double angle_threshold):m_tiltA(0.0),m_tiltB(0.0),m_angle(0.0),m_angleThreshold(angle_threshold){} + +/// @brief +TiltTracker::~TiltTracker(){} + +/// @brief initialise le Tilttracker. A appeler au pied de la charge station. +/// @param left_encoder_dist valeur renvoyée par le getDistance de l'encodeur gauche en mètres +/// @param right_encoder_dist valeur renvoyée par le getDistance de l'encodeur droit en mètres +/// @param angle angle actuel du robot ( sera considéré comme premier angle de tilt ) +/// @param estimated_next_tilt_distance estimation de la distance à laquelle surviendra le prochain Tilt en mètre +void TiltTracker::initialize(double left_encoder_dist, double right_encoder_dist, double angle, double estimated_next_tilt_distance) +{ + m_tiltA = (left_encoder_dist + right_encoder_dist)/2.0; + m_tiltB = m_tiltA + estimated_next_tilt_distance; + m_angle = angle; +} + +void TiltTracker::DetectTiltPoint(double left_encoder_dist, double right_encoder_dist, double angle) +{ + if( NSIGN(angle) != NSIGN(m_angle) ) + { + double dif = angle - m_angle; + dif = NABS(dif); + if(dif > m_angleThreshold) + { + m_angle = angle; + *m_pTilt = (left_encoder_dist + right_encoder_dist)/2.0; + m_pTilt = (m_pTilt == &m_tiltA) ? &m_tiltB:&m_tiltA; + } + } +} diff --git a/src/main/include/ChargeStationTiltTracker.h b/src/main/include/ChargeStationTiltTracker.h new file mode 100644 index 0000000..a40c2df --- /dev/null +++ b/src/main/include/ChargeStationTiltTracker.h @@ -0,0 +1,21 @@ +#pragma once +#include +#include "lib/RblUtils.h" + +class TiltTracker +{ +public: + TiltTracker(); + TiltTracker(double angle_threshold); + ~TiltTracker(); + + void initialize(double left_encoder_dist, double right_encoder_dist, double m_angle, double estimated_next_tilt_distance ); + void DetectTiltPoint(double left_encoder_dist, double right_encoder_dist,double angle); + double getDistanceBetweenTilts(){return NABS(m_tiltB - m_tiltA);} + double getAlgebricDistanceBetweenTilts(){return m_tiltB - m_tiltA;} + double m_tiltA; + double m_tiltB; + double* m_pTilt; + double m_angle; + double m_angleThreshold; +}; From d8111320094586325eed04c60d3a06df90aa1c4b Mon Sep 17 00:00:00 2001 From: jjTerazan <47304610+jjTerazan@users.noreply.github.com> Date: Sat, 11 Feb 2023 12:23:51 +0100 Subject: [PATCH 21/36] ++ --- src/main/include/ChargeStationTiltTracker.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/include/ChargeStationTiltTracker.h b/src/main/include/ChargeStationTiltTracker.h index a40c2df..d3ef555 100644 --- a/src/main/include/ChargeStationTiltTracker.h +++ b/src/main/include/ChargeStationTiltTracker.h @@ -13,9 +13,11 @@ class TiltTracker void DetectTiltPoint(double left_encoder_dist, double right_encoder_dist,double angle); double getDistanceBetweenTilts(){return NABS(m_tiltB - m_tiltA);} double getAlgebricDistanceBetweenTilts(){return m_tiltB - m_tiltA;} + double m_tiltA; double m_tiltB; double* m_pTilt; double m_angle; double m_angleThreshold; + }; From cb6adfc15ab92820d14ceda1d3e6a95d5800f3ca Mon Sep 17 00:00:00 2001 From: jjTerazan <47304610+jjTerazan@users.noreply.github.com> Date: Sat, 11 Feb 2023 12:29:11 +0100 Subject: [PATCH 22/36] +++ --- src/main/cpp/Robot.cpp | 77 ++++++++++++++------------ src/main/cpp/lib/Pid.cpp | 3 +- src/main/include/Robot.h | 43 ++++++++------ src/main/include/lib/Angle_AG.h | 24 ++++---- src/main/include/lib/NRollingAverage.h | 8 +-- src/main/include/lib/Pid.h | 2 + src/main/include/lib/RblUtils.h | 35 ++++++++++++ src/main/include/lib/rate_limiter.h | 1 + 8 files changed, 121 insertions(+), 72 deletions(-) create mode 100644 src/main/include/lib/RblUtils.h diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 11f7e32..a4c580d 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -2,6 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. +#include "lib/RblUtils.h" #include "Robot.h" double Robot::Calcul_De_Notre_Brave_JM(double forward, double turn, bool wheelSide) // calcule la vitesse des roues @@ -26,6 +27,9 @@ double Robot::Calcul_De_Notre_Brave_JM(double forward, double turn, bool wheelSi void Robot::RobotInit() { m_gyro.Reset(); m_gyro.Calibrate(); + + m_kPmin = 0.0075; + m_kPmax = 0.02; } void Robot::RobotPeriodic() {} @@ -75,22 +79,21 @@ void Robot::TeleopPeriodic() { - double a=0.0; + double a =0.0; double ratio_distance=0.0; - double x = m_accelerometer.GetX(); // -1/1 - double y =m_accelerometer.GetY(); - - double angle = m_gyro.GetAngle(); + double x = m_accelerometer.GetX(); // -1/1 + double y =m_accelerometer.GetY(); + double angle = m_gyro.GetAngle(); m_AccelerometerX.set(x); m_AccelerometerY.set(y); m_Gyro_Angle.set(angle); - - m_FusAngle.Update(m_gyro.GetRate()/180.0*3.14159265,m_accelerometer.GetY(),m_accelerometer.GetX()); + m_FusAngle.SetTau(frc::SmartDashboard::GetNumber("m_tau",0.5)); - + m_FusAngle.Update(NDEGtoRAD(m_gyro.GetRate()),m_accelerometer.GetY(),m_accelerometer.GetX()); // A la place de ... m_FusAngle.Update(m_gyro.GetRate()/180.0*3.14159265,m_accelerometer.GetY(),m_accelerometer.GetX()); + // frc::SmartDashboard::PutNumber("FuseAngle",m_FusAngle.GetAngle()*180.0/3.14159265); // frc::SmartDashboard::PutNumber("m_K",m_FusAngle.m_k); // frc::SmartDashboard::PutNumber("Accel_X",x); @@ -98,47 +101,54 @@ void Robot::TeleopPeriodic() { // frc::SmartDashboard::PutNumber("Angle_Gyro",angle); - double output = m_PidController.Calculate(m_FusAngle.GetAngle()*180.0/3.14159265); - double speed = std::clamp(output,-0.3,0.3); + if (m_joystickRight.GetRawButton(1)) { - m_distanceAparcourir = CHARGE_STATION_WIDTH;// à mettre - m_distanceParcourue = 0.0; - m_distanceRestante=m_distanceAparcourir-m_distanceParcourue; - m_encoder_origine=abs(m_EncoderLeft.GetDistance()*(3.0*0.0254)*(2.0*3.14159265)); + // reset de la posiition de ref des encodeurs. + m_encoderOrigin = NABS(m_EncoderLeft.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE); // a la place de ... m_encoder_origine=abs(m_EncoderLeft.GetDistance()*(3.0*0.0254)*(2.0*3.14159265)); + m_refDistance = CHARGE_STATION_WIDTH; // a la place de ... m_distanceAparcourir = CHARGE_STATION_WIDTH; + m_traveledDistance = 0.0; // a la place de ... m_distanceParcourue = 0.0; + // on supprime .... m_distanceRestante = m_distanceAparcourir-m_distanceParcourue; m_PidController.Reset(); - m_PidController.SetSetpoint(0); - m_signe_error = signe(m_PidController.m_error); + m_PidController.SetSetpoint(0.0); // à la place de ... m_PidController.SetSetpoint(0); + m_errorSign = NSIGN(m_PidController.m_error); // à la place de ... m_signe_error = NSIGN(m_PidController.m_error); m_state= State::Adjusting; } - frc::SmartDashboard::PutNumber("m_distanceParcourue",m_distanceParcourue); - frc::SmartDashboard::PutNumber("m_distanceRestante",m_distanceRestante); -switch (m_state) - { + + frc::SmartDashboard::PutNumber("m_traveledDistance",m_traveledDistance); // à la place de ... frc::SmartDashboard::PutNumber("m_distanceParcourue",m_distanceParcourue); + frc::SmartDashboard::PutNumber("m_refDistance",m_refDistance); // à la place de ... frc::SmartDashboard::PutNumber("m_distanceRestante",m_distanceRestante); + switch (m_state) + { case Adjusting: std::cout<<("Adjusting")< Pid::Pid(double setpoint, double kp, double ki, double kd) : m_setpoint(0.0), m_kp(0.0), m_ki(0.0), m_kd(0.0){} @@ -16,7 +17,7 @@ double Pid::Calculate(double measurement) m_derivative = m_error - m_lastError; // calcul de la dérivée de l'erreur m_lastError = m_error; // sauvegarde de l'erreur précédente m_output = m_kp * m_error + m_ki * m_integrative + m_kd * m_derivative; // calcul de l'output - if (abs(m_error) < m_tolerance){m_output = 0.0; }// si l'erreur est inférieure à la tolérance on arrête le moteur + if (NABS(m_error) < m_tolerance){m_output = 0.0; }// si l'erreur est inférieure à la tolérance on arrête le moteur return m_output; } diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 9054f13..c7457b1 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -11,18 +11,15 @@ (((2.0 * VMAX) / AXLETRACK) / \ 1.7) // vitesse angulaire Max theorique .. à modifier avec Garice -#define NABS(a) (((a) < 0) ? -(a) : (a)) // VALEUR ABSOLUE -#define NMAX(a, b) (((a) > (b)) ? (a) : (b)) // Max -#define NMIN(a, b) (((a) < (b)) ? (a) : (b)) // Min -#define NROUND(fval) ( ( (fval) >= 0.0f ) ? ((Ns32)((fval) + 0.5f)) : ((Ns32)((fval) - 0.5f)) ) -#define NSIGN(a) (((a)<0) ? -1:1) -#define NCLAMP(mn,a,mx) ( ((a)<(mn)) ? (mn) : ((a)>(mx)) ? (mx) : (a) ) -#define NLERP(a,b,t) ( a + (b - a)*t ) - -#define bruit 0.1 -#define signe(a) (((a) < 0) ? -1:1) + +#define NOISE 0.1 // à la place de .... #define bruit 0.1 #define CHARGE_STATION_WIDTH 1.0 // en metre +#define TRACTION_WHEEL_DIAMETER (6.0*0.0254) // Diametre des roues du robot +#define TRACTION_WHEEL_RADIUS (3.0*0.0254) // rayon des roues du robot +#define TRACTION_WHEEL_CIRCUMFERENCE (3.0*0.0254*NF64_2PI) // circonference des roues du robot + + #include #include #include @@ -111,16 +108,26 @@ class Robot : public frc::TimedRobot { Angle_AG m_FusAngle{0.02,0.075}; Pid m_PidController{0.0,0.01,0.0005,0.0}; - double m_kPmin = 0.0075; - double m_kPmax = 0.02; - - double m_distanceParcourue = 0.0; - double m_distanceRestante = 0.0; - double m_distanceAparcourir = 0.0; +// ARGH !!!!!!!!!!!!!!!!!!!!!!! +// Pas d'affectation dans la déclaration !!! + /* + double m_kPmin = 0.0075; !!! MAL + double m_kPmax = 0.02; !!! MAL - double m_signe_error; - double m_encoder_origine; + double m_distanceParcourue = 0.0; !!! MAL + double m_distanceRestante = 0.0; !!! MAL + double m_distanceAparcourir = 0.0; !!! MAL + */ + double m_kPmin; + double m_kPmax; + double m_selfConfidenceMin; //à la place de ... double m_kPmin; + double m_selfConfidenceMax; //à la place de ... double m_kPmax; + double m_traveledDistance; //à la place de ... double m_distanceParcourue; + double m_refDistance; //à la place de ... double m_distanceAparcourir; + // rien .................... à la place de ... double m_distanceRestante; + double m_errorSign; //à la place de ... double m_signe_error; + double m_encoderOrigin; //à la place de ... double m_encoder_origin; }; diff --git a/src/main/include/lib/Angle_AG.h b/src/main/include/lib/Angle_AG.h index c807db9..2cad9ad 100644 --- a/src/main/include/lib/Angle_AG.h +++ b/src/main/include/lib/Angle_AG.h @@ -6,20 +6,22 @@ class Angle_AG { - public : - Angle_AG(double dt,double tau,double angle=0.0); - double Update(double rate_gyro, double xaccel,double zaccel); // x correspond à la verticale vers le haut et y à l'horizontale vers l'avant et z l'ordonée - double GetAngle(){return m_angle;}; - void Reset(double angle =0.0); - void SetTau(double tau){m_tau=tau; m_k=m_tau/(m_tau+m_dt);}; - double m_k; // coef calculé grace à m_tau et dt + public : + Angle_AG(double dt,double tau,double angle=0.0); + double Update(double rate_gyro, double xaccel,double zaccel); + void Reset(double angle = 0.0); + void SetTau(double tau){m_tau=tau; m_k=m_tau/(m_tau+m_dt);}; + double GetAngle(){return m_angle;}; + double GetK(){return m_k;}; + double GetTau(){return m_tau;}; - private : - double m_angle; // angle en radian combinant accel et gyro - double m_dt; // dela t en seconde - double m_tau; // temp en seconde + private : + double m_angle; // angle en radian combinant accel et gyro + double m_dt; // intervalle de temps en seconde entre + double m_tau; // temp en seconde + double m_k; // coef calculé grace à m_tau et dt }; \ No newline at end of file diff --git a/src/main/include/lib/NRollingAverage.h b/src/main/include/lib/NRollingAverage.h index 1bd4caa..56c92c4 100644 --- a/src/main/include/lib/NRollingAverage.h +++ b/src/main/include/lib/NRollingAverage.h @@ -1,5 +1,4 @@ -#ifndef __NROLLINGAVERAGE_H -#define __NROLLINGAVERAGE_H +#pragma once #include // *************************************************************************************** @@ -62,8 +61,3 @@ class NlongRollingAverage }; - - - -#endif // __NROLLINGAVERAGE_H - diff --git a/src/main/include/lib/Pid.h b/src/main/include/lib/Pid.h index d81c423..99a1225 100644 --- a/src/main/include/lib/Pid.h +++ b/src/main/include/lib/Pid.h @@ -1,3 +1,5 @@ +#pragma once + #include #include #include diff --git a/src/main/include/lib/RblUtils.h b/src/main/include/lib/RblUtils.h new file mode 100644 index 0000000..6c32dde --- /dev/null +++ b/src/main/include/lib/RblUtils.h @@ -0,0 +1,35 @@ +#pragma once + +// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +// + NF64 +// + +// +--+ PI +#define NF64_PI 3.14159265358979323846 // PI +#define NF64_2PI 6.28318530717958647693 // 2*PI +#define NF64_PI_2 1.57079632679489661923 // PI/2 +#define NF64_PI_3 1.04719755119659774615 // PI/3 +#define NF64_PI_4 0.78539816339744830962 // PI/4 +#define NF64_1_PI 0.31830988618379067154 // 1/PI +#define NF64_1_2PI 0.15915494309189533577 // 1/(2*PI) +#define NF64_2_PI 0.63661977236758134308 // 2/PI +#define NF64_1_SQRTPI 0.56418958354775628695 // 1/sqrt(PI) +#define NF64_2_SQRTPI 1.12837916709551257390 // 2/sqrt(PI) +// + +// +--+ SQRT(2) +// + +#define NF64_SQRT2 1.41421356237309504880 // Sqrt(2) +#define NF64_1_SQRT2 0.70710678118654752440 // 1/Sqrt(2) +#define NF64_SQRT2_2 0.70710678118654752440 // Sqrt(2)/2 + +// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +// + Quick MACROS functions +#define NDEGtoRAD(deg) ((NF64_PI / 180.0) * (deg)) // Deg -> Radian +#define NRADtoDEG(rad) ((180.0 / NF64_PI) * (rad)) // Radian -> Deg + +#define NABS(a) (((a) < 0) ? -(a) : (a)) +#define NMAX(a, b) (((a) > (b)) ? (a) : (b)) +#define NMIN(a, b) (((a) < (b)) ? (a) : (b)) +//#define NROUND(fval) ( ( (fval) >= 0.0 ) ? ((Ns32)((fval) + 0.5)) : ((Ns32)((fval) - 0.5)) ) +#define NSIGN(a) (((a)<0) ? -1:1) +#define NCLAMP(mn,a,mx) ( ((a)<(mn)) ? (mn) : ((a)>(mx)) ? (mx) : (a) ) +#define NLERP(a,b,t) ( a + (b - a)*t ) diff --git a/src/main/include/lib/rate_limiter.h b/src/main/include/lib/rate_limiter.h index edc93ac..60cfe5b 100644 --- a/src/main/include/lib/rate_limiter.h +++ b/src/main/include/lib/rate_limiter.h @@ -1,4 +1,5 @@ +#pragma once class RateLimiter { From c10d623e2550ab0f935a406c9a61af5c07ff7b7d Mon Sep 17 00:00:00 2001 From: mimi980 <90112270+mimi980@users.noreply.github.com> Date: Sat, 11 Feb 2023 21:23:21 +0100 Subject: [PATCH 23/36] double PID --- src/main/cpp/ChargeStationTiltTracker.cpp | 10 +++- src/main/cpp/Robot.cpp | 65 +++++++++++++-------- src/main/cpp/lib/Angle_AG.cpp | 8 ++- src/main/include/ChargeStationTiltTracker.h | 6 +- src/main/include/Robot.h | 12 +++- src/main/include/lib/Angle_AG.h | 9 ++- 6 files changed, 77 insertions(+), 33 deletions(-) diff --git a/src/main/cpp/ChargeStationTiltTracker.cpp b/src/main/cpp/ChargeStationTiltTracker.cpp index 740a7f4..1e18f42 100644 --- a/src/main/cpp/ChargeStationTiltTracker.cpp +++ b/src/main/cpp/ChargeStationTiltTracker.cpp @@ -21,7 +21,7 @@ void TiltTracker::initialize(double left_encoder_dist, double right_encoder_dist m_angle = angle; } -void TiltTracker::DetectTiltPoint(double left_encoder_dist, double right_encoder_dist, double angle) +bool TiltTracker::DetectTiltPoint(double left_encoder_dist, double right_encoder_dist, double angle) { if( NSIGN(angle) != NSIGN(m_angle) ) { @@ -32,6 +32,14 @@ void TiltTracker::DetectTiltPoint(double left_encoder_dist, double right_encoder m_angle = angle; *m_pTilt = (left_encoder_dist + right_encoder_dist)/2.0; m_pTilt = (m_pTilt == &m_tiltA) ? &m_tiltB:&m_tiltA; + return true; } + } + return false; +} + +double TiltTracker::getNormalizeDistance() +{ + return 0.0; } diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index a4c580d..d5863b9 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -37,9 +37,9 @@ void Robot::AutonomousInit() {} void Robot::AutonomousPeriodic() {} void Robot::TeleopInit() { - frc::SmartDashboard::PutNumber("P", 0); - frc::SmartDashboard::PutNumber("I", 0); - frc::SmartDashboard::PutNumber("D", 0); + frc::SmartDashboard::PutNumber("P", 0.0); + frc::SmartDashboard::PutNumber("I", 0.0); + frc::SmartDashboard::PutNumber("D", 0.0); frc::SmartDashboard::PutNumber("m_tau", 0.5); m_MotorLeft.SetInverted(true); @@ -66,19 +66,14 @@ void Robot::TeleopInit() { m_MotorRightFollower.SetNeutralMode(ctre::phoenix::motorcontrol::NeutralMode::Brake); m_MotorRightFollower2.SetNeutralMode(ctre::phoenix::motorcontrol::NeutralMode::Brake); - m_PidController.SetTolerance(1.5); + m_AngleController.SetTolerance(4); m_state=State::End; m_EncoderLeft.SetDistancePerPulse(1.0/2048.0); + m_EncoderRight.SetDistancePerPulse(1.0/2048.0); } void Robot::TeleopPeriodic() { - - - - - - double a =0.0; double ratio_distance=0.0; @@ -90,6 +85,8 @@ void Robot::TeleopPeriodic() { m_AccelerometerX.set(x); m_AccelerometerY.set(y); m_Gyro_Angle.set(angle); + m_DeltaAngle_Average.add(m_FusAngle.GetDelta()*180.0/3.14159265); + m_FusAngle.SetTau(frc::SmartDashboard::GetNumber("m_tau",0.5)); m_FusAngle.Update(NDEGtoRAD(m_gyro.GetRate()),m_accelerometer.GetY(),m_accelerometer.GetX()); // A la place de ... m_FusAngle.Update(m_gyro.GetRate()/180.0*3.14159265,m_accelerometer.GetY(),m_accelerometer.GetX()); @@ -105,14 +102,18 @@ void Robot::TeleopPeriodic() { if (m_joystickRight.GetRawButton(1)) { + m_FusAngle.AutoSetBias(); // reset de la posiition de ref des encodeurs. - m_encoderOrigin = NABS(m_EncoderLeft.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE); // a la place de ... m_encoder_origine=abs(m_EncoderLeft.GetDistance()*(3.0*0.0254)*(2.0*3.14159265)); - m_refDistance = CHARGE_STATION_WIDTH; // a la place de ... m_distanceAparcourir = CHARGE_STATION_WIDTH; - m_traveledDistance = 0.0; // a la place de ... m_distanceParcourue = 0.0; + //m_encoderOrigin = NABS(m_EncoderLeft.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE); // a la place de ... m_encoder_origine=abs(m_EncoderLeft.GetDistance()*(3.0*0.0254)*(2.0*3.14159265)); + //m_refDistance = CHARGE_STATION_WIDTH; // a la place de ... m_distanceAparcourir = CHARGE_STATION_WIDTH; + // m_traveledDistance = 0.0; // a la place de ... m_distanceParcourue = 0.0; // on supprime .... m_distanceRestante = m_distanceAparcourir-m_distanceParcourue; - m_PidController.Reset(); - m_PidController.SetSetpoint(0.0); // à la place de ... m_PidController.SetSetpoint(0); - m_errorSign = NSIGN(m_PidController.m_error); // à la place de ... m_signe_error = NSIGN(m_PidController.m_error); + m_AngleController.Reset(); + m_AngleController.SetSetpoint(0.0); // à la place de ... m_AngleController.SetSetpoint(0); + m_VangleController.Reset(); + m_VangleController.SetSetpoint(0.0); + // m_errorSign = NSIGN(m_AngleController.m_error); // à la place de ... m_signe_error = NSIGN(m_AngleController.m_error); + m_TiltTracker.initialize(m_EncoderLeft.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE,m_EncoderRight.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE,m_FusAngle.GetAngle(),CHARGE_STATION_WIDTH); m_state= State::Adjusting; } @@ -122,7 +123,10 @@ void Robot::TeleopPeriodic() { { case Adjusting: std::cout<<("Adjusting")< rate du gyro en radian/sec @@ -12,8 +14,10 @@ double Angle_AG::Update(double rate_gyro, double xaccel, double zaccel) double b = NCLAMP(-1.0,xaccel,1.0); // m_angleAccel = acos(a); double angleaccel = atan2(b,a); - m_angle = m_k *(m_angle+rate_gyro*m_dt)+(1.0-m_k)*(angleaccel); - return m_angle; + a = m_k *(m_angle+rate_gyro*m_dt)+(1.0-m_k)*(angleaccel); + m_delta = a -m_angle; + m_angle = a; + return m_angle-m_bias; //doc : diff --git a/src/main/include/ChargeStationTiltTracker.h b/src/main/include/ChargeStationTiltTracker.h index d3ef555..b76065f 100644 --- a/src/main/include/ChargeStationTiltTracker.h +++ b/src/main/include/ChargeStationTiltTracker.h @@ -10,10 +10,10 @@ class TiltTracker ~TiltTracker(); void initialize(double left_encoder_dist, double right_encoder_dist, double m_angle, double estimated_next_tilt_distance ); - void DetectTiltPoint(double left_encoder_dist, double right_encoder_dist,double angle); + bool DetectTiltPoint(double left_encoder_dist, double right_encoder_dist,double angle); double getDistanceBetweenTilts(){return NABS(m_tiltB - m_tiltA);} - double getAlgebricDistanceBetweenTilts(){return m_tiltB - m_tiltA;} - + double getAlgebricDistanceBetweenTilts(){return m_tiltB - m_tiltA;} + double getNormalizeDistance(); double m_tiltA; double m_tiltB; double* m_pTilt; diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index c7457b1..01cee85 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -18,7 +18,7 @@ #define TRACTION_WHEEL_DIAMETER (6.0*0.0254) // Diametre des roues du robot #define TRACTION_WHEEL_RADIUS (3.0*0.0254) // rayon des roues du robot #define TRACTION_WHEEL_CIRCUMFERENCE (3.0*0.0254*NF64_2PI) // circonference des roues du robot - +#define K_ANTICIPATION 0.4 #include #include @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -80,7 +81,7 @@ class Robot : public frc::TimedRobot { frc::Joystick m_joystickRight{0}; frc::Joystick m_joystickLeft{1}; frc::BuiltInAccelerometer m_accelerometer{}; - // frc::PIDController m_pidController{0, 0, 0}; + // frc::PIDController m_AngleController{0, 0, 0}; frc::ADXRS450_Gyro m_gyro{frc::SPI::Port::kOnboardCS0}; ctre::phoenix::motorcontrol::can::TalonFX m_MotorRight{1}; @@ -93,6 +94,7 @@ class Robot : public frc::TimedRobot { NdoubleRollingAverage m_AccelerometerX_Average{25}; NdoubleRollingAverage m_AccelerometerX_Arcos_Average{25}; + NdoubleRollingAverage m_DeltaAngle_Average{10}; NdoubleRollingAverage m_error{10}; @@ -106,7 +108,11 @@ class Robot : public frc::TimedRobot { frc::Encoder m_EncoderLeft{2,3}; Angle_AG m_FusAngle{0.02,0.075}; - Pid m_PidController{0.0,0.01,0.0005,0.0}; + Pid m_VangleController{0.0,0.01,0.0005,0.0}; + Pid m_AngleController{0.0,0.01,0.0005,0.0}; + + TiltTracker m_TiltTracker{5}; + // ARGH !!!!!!!!!!!!!!!!!!!!!!! // Pas d'affectation dans la déclaration !!! diff --git a/src/main/include/lib/Angle_AG.h b/src/main/include/lib/Angle_AG.h index 2cad9ad..3aea61a 100644 --- a/src/main/include/lib/Angle_AG.h +++ b/src/main/include/lib/Angle_AG.h @@ -12,16 +12,23 @@ class Angle_AG void Reset(double angle = 0.0); void SetTau(double tau){m_tau=tau; m_k=m_tau/(m_tau+m_dt);}; - double GetAngle(){return m_angle;}; + double GetAngle(){return m_angle-m_bias;}; double GetK(){return m_k;}; double GetTau(){return m_tau;}; + double GetDelta(){return m_delta;}; + void SetBias(double bias){m_bias=bias;}; + void AutoSetBias(){m_bias=m_angle;}; private : + double m_angle; // angle en radian combinant accel et gyro + double m_delta; // différence entre l'angle et l'angle précédent double m_dt; // intervalle de temps en seconde entre double m_tau; // temp en seconde double m_k; // coef calculé grace à m_tau et dt + double m_bias; // + }; \ No newline at end of file From 768b0d1252dc3891d4a3ea5691d793aff1973971 Mon Sep 17 00:00:00 2001 From: jjTerazan <47304610+jjTerazan@users.noreply.github.com> Date: Sun, 12 Feb 2023 09:08:46 +0100 Subject: [PATCH 24/36] =?UTF-8?q?quelques=20ajustements=20et=20utilisation?= =?UTF-8?q?=20du=20rate=20natif=20du=20gyro=20=C3=A0=20la=20place=20du=20d?= =?UTF-8?q?elta=20de=20fuseangle?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/cpp/Robot.cpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index d5863b9..f912d7a 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -78,19 +78,19 @@ void Robot::TeleopPeriodic() { double ratio_distance=0.0; - double x = m_accelerometer.GetX(); // -1/1 - double y =m_accelerometer.GetY(); - double angle = m_gyro.GetAngle(); + double x_g = m_accelerometer.GetX(); // -1/1 + double y_g = m_accelerometer.GetY(); + double angle_deg = m_gyro.GetAngle(); + double gyro_rate_degps = m_gyro.GetRate(); - m_AccelerometerX.set(x); - m_AccelerometerY.set(y); - m_Gyro_Angle.set(angle); - m_DeltaAngle_Average.add(m_FusAngle.GetDelta()*180.0/3.14159265); + m_AccelerometerX.set(x_g); + m_AccelerometerY.set(y_g); + m_GyroAngle.set(angle_deg); - m_FusAngle.SetTau(frc::SmartDashboard::GetNumber("m_tau",0.5)); - m_FusAngle.Update(NDEGtoRAD(m_gyro.GetRate()),m_accelerometer.GetY(),m_accelerometer.GetX()); // A la place de ... m_FusAngle.Update(m_gyro.GetRate()/180.0*3.14159265,m_accelerometer.GetY(),m_accelerometer.GetX()); - + m_FusAngle.Update(NDEGtoRAD(gyro_rate_degps),y_g,x_g); // A la place de ... m_FusAngle.Update(m_gyro.GetRate()/180.0*3.14159265,m_accelerometer.GetY(),m_accelerometer.GetX()); + m_gyroRateAverage.add(gyro_rate_degps); //m_gyroRateAverage.add(m_FusAngle.GetDelta()*180.0/3.14159265); + // frc::SmartDashboard::PutNumber("FuseAngle",m_FusAngle.GetAngle()*180.0/3.14159265); // frc::SmartDashboard::PutNumber("m_K",m_FusAngle.m_k); // frc::SmartDashboard::PutNumber("Accel_X",x); @@ -157,7 +157,7 @@ void Robot::TeleopPeriodic() { m_AngleController.SetGains(p,i,d); // avant m_AngleController.SetGains(a,0.0,0.0); m_VangleController.SetGains(p*8.0,i*10.0,d*10.0); double Angleoutput = m_AngleController.Calculate(m_FusAngle.GetAngle()*180.0/3.14159265); - double Voutput = m_VangleController.Calculate(m_DeltaAngle_Average.get()); + double Voutput = m_VangleController.Calculate(m_gyroRateAverage.get()); double output = K_ANTICIPATION*Voutput+(1-K_ANTICIPATION)*Angleoutput; output = NCLAMP(-0.3,output,0.3); frc::SmartDashboard::PutNumber("a",a); From 020d4ef83efdb69dbf249d249c8d05e4d4283fb5 Mon Sep 17 00:00:00 2001 From: jjTerazan <47304610+jjTerazan@users.noreply.github.com> Date: Sun, 12 Feb 2023 09:09:39 +0100 Subject: [PATCH 25/36] + --- src/main/include/Robot.h | 29 +++++++++-------------------- 1 file changed, 9 insertions(+), 20 deletions(-) diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 01cee85..5021ae8 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -94,13 +94,13 @@ class Robot : public frc::TimedRobot { NdoubleRollingAverage m_AccelerometerX_Average{25}; NdoubleRollingAverage m_AccelerometerX_Arcos_Average{25}; - NdoubleRollingAverage m_DeltaAngle_Average{10}; + NdoubleRollingAverage m_gyroRateAverage{10}; NdoubleRollingAverage m_error{10}; Dynamic m_AccelerometerX; Dynamic m_AccelerometerY; - Dynamic m_Gyro_Angle; + Dynamic m_GyroAngle; double m_Sum_Delta_Gyro_Angle=0.0; @@ -113,27 +113,16 @@ class Robot : public frc::TimedRobot { TiltTracker m_TiltTracker{5}; - -// ARGH !!!!!!!!!!!!!!!!!!!!!!! -// Pas d'affectation dans la déclaration !!! - /* - double m_kPmin = 0.0075; !!! MAL - double m_kPmax = 0.02; !!! MAL - - double m_distanceParcourue = 0.0; !!! MAL - double m_distanceRestante = 0.0; !!! MAL - double m_distanceAparcourir = 0.0; !!! MAL - */ double m_kPmin; double m_kPmax; - double m_selfConfidenceMin; //à la place de ... double m_kPmin; - double m_selfConfidenceMax; //à la place de ... double m_kPmax; + double m_selfConfidenceMin; + double m_selfConfidenceMax; - double m_traveledDistance; //à la place de ... double m_distanceParcourue; - double m_refDistance; //à la place de ... double m_distanceAparcourir; - // rien .................... à la place de ... double m_distanceRestante; + double m_traveledDistance; + double m_refDistance; + - double m_errorSign; //à la place de ... double m_signe_error; - double m_encoderOrigin; //à la place de ... double m_encoder_origin; + double m_errorSign; + double m_encoderOrigin; }; From 41514d9971349dc07c0342d4d0fd0f83c74fcae9 Mon Sep 17 00:00:00 2001 From: mimi980 <90112270+mimi980@users.noreply.github.com> Date: Sun, 12 Feb 2023 19:31:15 +0100 Subject: [PATCH 26/36] log csv --- src/main/cpp/ChargeStationTiltTracker.cpp | 1 + src/main/cpp/Robot.cpp | 59 +++++-- src/main/cpp/lib/Angle_AG.cpp | 4 +- src/main/cpp/lib/NLCsv.cpp | 168 ++++++++++++++++++++ src/main/include/ChargeStationTiltTracker.h | 5 + src/main/include/Robot.h | 20 ++- src/main/include/lib/Angle_AG.h | 2 + src/main/include/lib/NLCsv.h | 53 ++++++ 8 files changed, 291 insertions(+), 21 deletions(-) create mode 100644 src/main/cpp/lib/NLCsv.cpp create mode 100644 src/main/include/lib/NLCsv.h diff --git a/src/main/cpp/ChargeStationTiltTracker.cpp b/src/main/cpp/ChargeStationTiltTracker.cpp index 1e18f42..b989b04 100644 --- a/src/main/cpp/ChargeStationTiltTracker.cpp +++ b/src/main/cpp/ChargeStationTiltTracker.cpp @@ -23,6 +23,7 @@ void TiltTracker::initialize(double left_encoder_dist, double right_encoder_dist bool TiltTracker::DetectTiltPoint(double left_encoder_dist, double right_encoder_dist, double angle) { + m_kAnticipation=0.5; if( NSIGN(angle) != NSIGN(m_angle) ) { double dif = angle - m_angle; diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index f912d7a..396021f 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -71,6 +71,19 @@ void Robot::TeleopInit() { m_EncoderLeft.SetDistancePerPulse(1.0/2048.0); m_EncoderRight.SetDistancePerPulse(1.0/2048.0); + m_logCSV.open("/home/lvuser/", true); + m_logCSV.setItem(0, "m_FusAngle",5, &m_LogFusAngle); + m_logCSV.setItem(1, "m_gyroRateAverage",5, &m_LogGyroRateAverage); + m_logCSV.setItem(2, "m_gyrorate",5, &m_LogGyroRate); + m_logCSV.setItem(3, "m_AngleController.m_error",5, &m_AngleController.m_error); + m_logCSV.setItem(4, "m_VangleController.m_error",5, &m_VangleController.m_error); + m_logCSV.setItem(5, "m_vOutput",5, &m_vOutput); + m_logCSV.setItem(6, "m_AngleOutput",5, &m_AngleOutput); + m_logCSV.setItem(7, "m_Output",5, &m_Output); + m_logCSV.setItem(8, "m_EncoderMetre",5, &m_LogEncoderM); + m_logCSV.setItem(9, "m_AccelX",5, &m_LogAccelX); + m_logCSV.setItem(10, "m_AngleAccel",5, &m_LogAngleAccel); + } void Robot::TeleopPeriodic() { @@ -91,7 +104,9 @@ void Robot::TeleopPeriodic() { m_FusAngle.Update(NDEGtoRAD(gyro_rate_degps),y_g,x_g); // A la place de ... m_FusAngle.Update(m_gyro.GetRate()/180.0*3.14159265,m_accelerometer.GetY(),m_accelerometer.GetX()); m_gyroRateAverage.add(gyro_rate_degps); //m_gyroRateAverage.add(m_FusAngle.GetDelta()*180.0/3.14159265); - // frc::SmartDashboard::PutNumber("FuseAngle",m_FusAngle.GetAngle()*180.0/3.14159265); + frc::SmartDashboard::PutNumber("FuseAngle",NRADtoDEG(m_FusAngle.GetAngle())); + frc::SmartDashboard::PutNumber("GyroRate",m_gyroRateAverage.get()); + frc::SmartDashboard::PutNumber("anglegyro",m_gyro.GetAngle()); // frc::SmartDashboard::PutNumber("m_K",m_FusAngle.m_k); // frc::SmartDashboard::PutNumber("Accel_X",x); // frc::SmartDashboard::PutNumber("Accel_Y",y); @@ -117,8 +132,6 @@ void Robot::TeleopPeriodic() { m_state= State::Adjusting; } - frc::SmartDashboard::PutNumber("m_traveledDistance",m_traveledDistance); // à la place de ... frc::SmartDashboard::PutNumber("m_distanceParcourue",m_distanceParcourue); - frc::SmartDashboard::PutNumber("m_refDistance",m_refDistance); // à la place de ... frc::SmartDashboard::PutNumber("m_distanceRestante",m_distanceRestante); switch (m_state) { case Adjusting: @@ -153,27 +166,29 @@ void Robot::TeleopPeriodic() { double p=frc::SmartDashboard::GetNumber("P",0.008); double i =frc::SmartDashboard::GetNumber("I",0.0001); double d =frc::SmartDashboard::GetNumber("D",0.07); + m_TiltTracker.DetectTiltPoint(m_EncoderLeft.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE,m_EncoderRight.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE,m_FusAngle.GetAngle()); m_AngleController.SetGains(p,i,d); // avant m_AngleController.SetGains(a,0.0,0.0); m_VangleController.SetGains(p*8.0,i*10.0,d*10.0); - double Angleoutput = m_AngleController.Calculate(m_FusAngle.GetAngle()*180.0/3.14159265); - double Voutput = m_VangleController.Calculate(m_gyroRateAverage.get()); - double output = K_ANTICIPATION*Voutput+(1-K_ANTICIPATION)*Angleoutput; - output = NCLAMP(-0.3,output,0.3); + m_AngleOutput = m_AngleController.Calculate(NRADtoDEG(m_FusAngle.GetAngle())); + m_vOutput = m_VangleController.Calculate(m_gyroRateAverage.get()); + m_Output = m_TiltTracker.m_kAnticipation*m_vOutput+(1-m_TiltTracker.m_kAnticipation)*m_AngleOutput; + frc::SmartDashboard::PutNumber("k_anticipation",m_TiltTracker.m_kAnticipation); + m_Output = NCLAMP(-0.3,0,0.3); frc::SmartDashboard::PutNumber("a",a); frc::SmartDashboard::PutNumber("error",m_AngleController.m_error); frc::SmartDashboard::PutNumber("delta_error",m_VangleController.m_error); frc::SmartDashboard::PutNumber("ratio_distance",ratio_distance); - frc::SmartDashboard::PutNumber("m_tiltA",m_TiltTracker.m_tiltA); - frc::SmartDashboard::PutNumber("m_tiltB",m_TiltTracker.m_tiltB); - frc::SmartDashboard::PutNumber("encoder",(m_EncoderLeft.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE+m_EncoderRight.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE)/2); - + frc::SmartDashboard::PutNumber("m_encoderRight",m_EncoderRight.GetDistance()); + frc::SmartDashboard::PutNumber("m_encoderLeft",m_EncoderLeft.GetDistance()); + frc::SmartDashboard::PutNumber("encoder",((m_EncoderLeft.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE)+(m_EncoderRight.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE))/2); + if (m_joystickLeft.GetRawButton(1)) { - m_MotorRight.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, output); - m_MotorLeft.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, output); + m_MotorRight.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, m_Output); + m_MotorLeft.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, m_Output); } else { @@ -182,9 +197,20 @@ void Robot::TeleopPeriodic() { } - frc::SmartDashboard::PutNumber("Output", output); - frc::SmartDashboard::PutNumber("Voutput", Voutput); - frc::SmartDashboard::PutNumber("Angleoutput", Angleoutput); + frc::SmartDashboard::PutNumber("m_Output", m_Output); + frc::SmartDashboard::PutNumber("m_vOutput", m_vOutput); + frc::SmartDashboard::PutNumber("m_AngleOutput", m_AngleOutput); + m_LogFusAngle = m_FusAngle.GetAngle(); + m_LogGyroRateAverage = m_gyroRateAverage.get(); + m_LogGyroRate = m_gyro.GetRate(); + m_LogEncoderM = ((m_EncoderLeft.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE)+(m_EncoderRight.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE))/2; + m_LogAngleAccel = m_FusAngle.m_angleAccel; + m_LogAccelX = m_accelerometer.GetX(); + + + + + m_logCSV.write(); } @@ -192,6 +218,7 @@ void Robot::TeleopPeriodic() { void Robot::DisabledInit() { m_gyro.Reset(); + m_logCSV.close(); } void Robot::DisabledPeriodic() {} diff --git a/src/main/cpp/lib/Angle_AG.cpp b/src/main/cpp/lib/Angle_AG.cpp index 881d5fb..c9a7bc4 100644 --- a/src/main/cpp/lib/Angle_AG.cpp +++ b/src/main/cpp/lib/Angle_AG.cpp @@ -13,8 +13,8 @@ double Angle_AG::Update(double rate_gyro, double xaccel, double zaccel) double a = NCLAMP(-1.0,zaccel,1.0); double b = NCLAMP(-1.0,xaccel,1.0); // m_angleAccel = acos(a); - double angleaccel = atan2(b,a); - a = m_k *(m_angle+rate_gyro*m_dt)+(1.0-m_k)*(angleaccel); + m_angleAccel = atan2(b,a); + a = m_k *(m_angle+rate_gyro*m_dt)+(1.0-m_k)*(m_angleAccel); m_delta = a -m_angle; m_angle = a; return m_angle-m_bias; diff --git a/src/main/cpp/lib/NLCsv.cpp b/src/main/cpp/lib/NLCsv.cpp new file mode 100644 index 0000000..d213d3a --- /dev/null +++ b/src/main/cpp/lib/NLCsv.cpp @@ -0,0 +1,168 @@ + +#include "lib/NLCsv.h" + +NLCSV::NLCSV(unsigned long itemlistsize) +{ + m_pFile = nullptr; + if (itemlistsize) + { + m_itemListSize = itemlistsize; + m_pItems = (NLCSVITEM*)malloc(sizeof(NLCSVITEM) * itemlistsize); + memset(m_pItems, 0, sizeof(NLCSVITEM) * itemlistsize); + } + else + { + m_itemListSize = 0; + m_pItems = nullptr; + } +} + + +void NLCSV::setItem(const unsigned long idx, const char* label, const unsigned char precision, double* pdblptr) +{ + assert(idx < m_itemListSize); + assert(strlen(label) < (NLCSVITEM_LABEL_MAXSIZE - 1)); + assert(pdblptr); + + strcpy(m_pItems[idx].m_label, label); + if (precision) + sprintf(m_pItems[idx].m_format, "%%.%dlf", precision); + else + sprintf(m_pItems[idx].m_format, "%.0lf"); + + m_pItems[idx].m_type = NLCSVITEM_TYPE_DOUBLE; + m_pItems[idx].m_ptr = (void*)pdblptr; +} + +void NLCSV::setItem(const unsigned long idx, const char* label, const unsigned char precision, float* pfltptr) +{ + assert(idx < m_itemListSize); + assert(strlen(label) < (NLCSVITEM_LABEL_MAXSIZE - 1)); + assert(pfltptr); + + strcpy(m_pItems[idx].m_label, label); + if (precision) + sprintf(m_pItems[idx].m_format, "%%.%df", precision); + else + sprintf(m_pItems[idx].m_format, "%.0f"); + + m_pItems[idx].m_type = NLCSVITEM_TYPE_FLOAT; + m_pItems[idx].m_ptr = (void*)pfltptr; +} + +void NLCSV::setItem(const unsigned long idx, const char* label, int* pintptr) +{ + assert(idx < m_itemListSize); + assert(strlen(label) < (NLCSVITEM_LABEL_MAXSIZE - 1)); + assert(pintptr); + + strcpy(m_pItems[idx].m_label, label); + sprintf(m_pItems[idx].m_format, "%d"); + + m_pItems[idx].m_type = NLCSVITEM_TYPE_INT; + m_pItems[idx].m_ptr = (void*)pintptr; +} + +bool NLCSV::open(const char* logname) +{ + // fichier dej� ouvert ! :( + assert(!m_pFile); + if (m_pFile) + return false; + + char filename[NLCSVITEM_FILENAME_MAXSIZE]; + time_t t; + + time(&t); + char*strt = ctime(&t); + if (strt[strlen(strt) - 1] == '\n') strt[strlen(strt) - 1] = '\0'; + + if (logname) + { + sprintf(filename, "%s", logname, strt); + strftime(&filename[strlen(logname)], NLCSVITEM_FILENAME_MAXSIZE - strlen(logname), "log[%d_%m_%Y__%H_%M_%S].csv", localtime(&t)); + } + else + strftime(filename, NLCSVITEM_FILENAME_MAXSIZE, "log[%d_%m_%Y__%H_%M_%S].csv", localtime(&t)); + + m_pFile = fopen(filename, "w , ccs=UTF-8"); + if (m_pFile) + return true; + else + return false; +} + +bool NLCSV::open(const char* logname, const char* headline ) +{ + if (open(logname)) + { + if (headline) + fprintf(m_pFile, headline); + return true; + } + else + return false; +} + +bool NLCSV::open(const char* logname, const bool bautoheadline) +{ + if (open(logname)) + { + if (bautoheadline && m_pItems) + { + NLCSVITEM* pitem = m_pItems; + for (unsigned long i = 0; i < m_itemListSize; i++, pitem++) + { + if(pitem->m_type != 0) + fprintf(m_pFile, "%s;", pitem->m_label); + else + fprintf(m_pFile, "?;"); + } + fprintf(m_pFile, "\n"); + } + return true; + } + else + return false; +} + +void NLCSV::write(const char* format, ...) +{ + assert(m_pFile); + + if (m_pFile) + { + va_list args; + va_start(args, format); + vfprintf(m_pFile, format, args); + va_end(args); + } +} + +void NLCSV::write() +{ + if (m_pFile && m_pItems) + { + NLCSVITEM* pitem = m_pItems; + for (unsigned long i = 0; i < m_itemListSize; i++, pitem++) + { + switch (pitem->m_type) + { + case NLCSVITEM_TYPE_DOUBLE: + fprintf(m_pFile, pitem->m_format, *((double*)pitem->m_ptr)); + break; + case NLCSVITEM_TYPE_FLOAT: + fprintf(m_pFile, pitem->m_format, *((float*)pitem->m_ptr)); + break; + case NLCSVITEM_TYPE_INT: + fprintf(m_pFile, pitem->m_format, *((int*)pitem->m_ptr)); + break; + default: + fprintf(m_pFile," ? "); + break; + } + fprintf(m_pFile, ";"); + } + fprintf(m_pFile, "\n"); + } +} diff --git a/src/main/include/ChargeStationTiltTracker.h b/src/main/include/ChargeStationTiltTracker.h index b76065f..7ca4afd 100644 --- a/src/main/include/ChargeStationTiltTracker.h +++ b/src/main/include/ChargeStationTiltTracker.h @@ -1,6 +1,8 @@ #pragma once #include #include "lib/RblUtils.h" +#define K_MIN 0.2 +#define K_MAX 0.8 class TiltTracker { @@ -19,5 +21,8 @@ class TiltTracker double* m_pTilt; double m_angle; double m_angleThreshold; + double m_kAnticipation; + double m_a; + }; diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 5021ae8..15d2a62 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -40,6 +40,7 @@ #include #include #include +#include //gyro @@ -104,8 +105,8 @@ class Robot : public frc::TimedRobot { double m_Sum_Delta_Gyro_Angle=0.0; - frc::Encoder m_EncoderRight{0,1}; - frc::Encoder m_EncoderLeft{2,3}; + frc::Encoder m_EncoderRight{0,1,true}; + frc::Encoder m_EncoderLeft{2,3,false}; Angle_AG m_FusAngle{0.02,0.075}; Pid m_VangleController{0.0,0.01,0.0005,0.0}; @@ -124,5 +125,18 @@ class Robot : public frc::TimedRobot { double m_errorSign; - double m_encoderOrigin; + double m_encoderOrigin; + + double m_vOutput; + double m_Output; + double m_AngleOutput; + double m_LogFusAngle; + double m_LogGyroRate; + double m_LogGyroRateAverage; + double m_LogAngleAccel; + double m_LogEncoderM; + double m_LogAccelX; + + + NLCSV m_logCSV{11}; }; diff --git a/src/main/include/lib/Angle_AG.h b/src/main/include/lib/Angle_AG.h index 3aea61a..0f92939 100644 --- a/src/main/include/lib/Angle_AG.h +++ b/src/main/include/lib/Angle_AG.h @@ -19,6 +19,7 @@ class Angle_AG void SetBias(double bias){m_bias=bias;}; void AutoSetBias(){m_bias=m_angle;}; + double m_angleAccel; // angle en radian calculé avec l'accelerometre @@ -31,4 +32,5 @@ class Angle_AG double m_k; // coef calculé grace à m_tau et dt double m_bias; // + }; \ No newline at end of file diff --git a/src/main/include/lib/NLCsv.h b/src/main/include/lib/NLCsv.h new file mode 100644 index 0000000..0634bdb --- /dev/null +++ b/src/main/include/lib/NLCsv.h @@ -0,0 +1,53 @@ +#pragma once +#include +#include + +#include +#include +#include +#include + +#define NLCSVITEM_LABEL_MAXSIZE 32 +#define NLCSVITEM_FORMAT_MAXSIZE 8 +#define NLCSVITEM_FILENAME_MAXSIZE 256 + +#define NLCSVITEM_TYPE_FLOAT 1 +#define NLCSVITEM_TYPE_DOUBLE 2 +#define NLCSVITEM_TYPE_INT 3 + +typedef struct NLCSVITEM NLCSVITEM; +struct NLCSVITEM +{ + unsigned long m_type; + char m_label[NLCSVITEM_LABEL_MAXSIZE]; + char m_format[NLCSVITEM_FORMAT_MAXSIZE]; + void* m_ptr; +}; + +class NLCSV +{ +public: + NLCSV() :m_pFile(nullptr), m_pItems(nullptr), m_itemListSize(0) {}; + NLCSV(unsigned long itemlistsize); + ~NLCSV() { close(); if (m_pItems)free(m_pItems); }; + + + void setItem(const unsigned long idx, const char* label, const unsigned char precision, double* pdblptr); + void setItem(const unsigned long idx, const char* label, const unsigned char precision, float* pfltptr); + void setItem(const unsigned long idx, const char* label, int* pintptr); + + bool open(const char* logname = nullptr); + bool open(const char* logname, const char* headline); + bool open(const char* logname, const bool bautoheadline); + bool isOpen() { return m_pFile ? true : false; } + + void write(const char* format, ...); + void write(); + void close(){if (m_pFile) { fclose(m_pFile); m_pFile = nullptr; }}; + + +private: + FILE* m_pFile; + NLCSVITEM* m_pItems; + unsigned long m_itemListSize; +}; \ No newline at end of file From da2652d6bc34b98db8fc5a38b598c26b64ea8c90 Mon Sep 17 00:00:00 2001 From: jjTerazan <47304610+jjTerazan@users.noreply.github.com> Date: Mon, 13 Feb 2023 01:24:22 +0100 Subject: [PATCH 27/36] TiltTracker : integration pointeurs "tournants" sur Tilts & fonction parabolique pour k. --- src/main/cpp/ChargeStationTiltTracker.cpp | 45 ++++++++++++--------- src/main/cpp/Robot.cpp | 9 +++-- src/main/include/ChargeStationTiltTracker.h | 20 ++++++--- 3 files changed, 44 insertions(+), 30 deletions(-) diff --git a/src/main/cpp/ChargeStationTiltTracker.cpp b/src/main/cpp/ChargeStationTiltTracker.cpp index b989b04..fa68bb8 100644 --- a/src/main/cpp/ChargeStationTiltTracker.cpp +++ b/src/main/cpp/ChargeStationTiltTracker.cpp @@ -2,42 +2,47 @@ #include "ChargeStationTiltTracker.h" /// @brief -TiltTracker::TiltTracker():m_tiltA(0.0),m_tiltB(0.0),m_pTilt(&m_tiltB), m_angle(0.0),m_angleThreshold(0.0){} +TiltTracker::TiltTracker():m_dt(0.0),m_tiltA(0.0),m_tiltB(0.0),m_pFrom(&m_tiltA),m_pTo(&m_tiltB), m_angleThreshold(0.0),m_deltaTimeThreshold(0.0),m_aParabolic(1.0/(0.5*0.5)),m_bParabolic(0.0){} -TiltTracker::TiltTracker(double angle_threshold):m_tiltA(0.0),m_tiltB(0.0),m_angle(0.0),m_angleThreshold(angle_threshold){} +TiltTracker::TiltTracker(const double angle_rate_threshold, const double dt_threshold, const double kmin):m_dt(0.0),m_tiltA(0.0),m_tiltB(0.0),m_pFrom(&m_tiltA),m_pTo(&m_tiltB),m_angleThreshold(angle_rate_threshold),m_deltaTimeThreshold(dt_threshold),m_aParabolic((1.0-kmin)/(0.5*0.5)),m_bParabolic(kmin){} /// @brief TiltTracker::~TiltTracker(){} /// @brief initialise le Tilttracker. A appeler au pied de la charge station. -/// @param left_encoder_dist valeur renvoyée par le getDistance de l'encodeur gauche en mètres -/// @param right_encoder_dist valeur renvoyée par le getDistance de l'encodeur droit en mètres -/// @param angle angle actuel du robot ( sera considéré comme premier angle de tilt ) +/// @param position valeur renvoyée par le getDistance de l'encodeur gauche en mètres /// @param estimated_next_tilt_distance estimation de la distance à laquelle surviendra le prochain Tilt en mètre -void TiltTracker::initialize(double left_encoder_dist, double right_encoder_dist, double angle, double estimated_next_tilt_distance) +void TiltTracker::initialize(const double position,const double estimated_next_tilt_distance) { - m_tiltA = (left_encoder_dist + right_encoder_dist)/2.0; + m_tiltA = position; m_tiltB = m_tiltA + estimated_next_tilt_distance; - m_angle = angle; + m_pFrom = &m_tiltA; + m_pTo = &m_tiltB; } -bool TiltTracker::DetectTiltPoint(double left_encoder_dist, double right_encoder_dist, double angle) +double TiltTracker::Update(const double dt, const double position, const double angle,const double angle_rate) { - m_kAnticipation=0.5; - if( NSIGN(angle) != NSIGN(m_angle) ) + + m_dt += dt; + if(m_dt >= m_deltaTimeThreshold) { - double dif = angle - m_angle; - dif = NABS(dif); - if(dif > m_angleThreshold) + m_dt = 0.0; + if( NSIGN(angle) != NSIGN(angle_rate) ) { - m_angle = angle; - *m_pTilt = (left_encoder_dist + right_encoder_dist)/2.0; - m_pTilt = (m_pTilt == &m_tiltA) ? &m_tiltB:&m_tiltA; - return true; + if(angle_rate > m_angleThreshold) + { + *m_pTo = position; + double *pd = m_pFrom; + m_pFrom = m_pTo; + m_pTo = pd; + } } - } - return false; + double nrmx = (position - *m_pFrom) / (*m_pTo - *m_pFrom); + nrmx = NCLAMP(0,nrmx,1.0) - 0.5; + + m_k = m_aParabolic*nrmx*nrmx + m_bParabolic; + return m_k; } double TiltTracker::getNormalizeDistance() diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 396021f..d10c0bf 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -128,7 +128,7 @@ void Robot::TeleopPeriodic() { m_VangleController.Reset(); m_VangleController.SetSetpoint(0.0); // m_errorSign = NSIGN(m_AngleController.m_error); // à la place de ... m_signe_error = NSIGN(m_AngleController.m_error); - m_TiltTracker.initialize(m_EncoderLeft.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE,m_EncoderRight.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE,m_FusAngle.GetAngle(),CHARGE_STATION_WIDTH); + m_TiltTracker.initialize((m_EncoderLeft.GetDistance()+m_EncoderRight.GetDistance())*TRACTION_WHEEL_CIRCUMFERENCE/2.0,CHARGE_STATION_WIDTH); m_state= State::Adjusting; } @@ -167,13 +167,14 @@ void Robot::TeleopPeriodic() { double i =frc::SmartDashboard::GetNumber("I",0.0001); double d =frc::SmartDashboard::GetNumber("D",0.07); - m_TiltTracker.DetectTiltPoint(m_EncoderLeft.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE,m_EncoderRight.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE,m_FusAngle.GetAngle()); + m_TiltTracker.Update(0.02,(m_EncoderLeft.GetDistance()+m_EncoderRight.GetDistance())*TRACTION_WHEEL_CIRCUMFERENCE/2.0,m_FusAngle.GetAngle(),m_gyroRateAverage.get()); m_AngleController.SetGains(p,i,d); // avant m_AngleController.SetGains(a,0.0,0.0); m_VangleController.SetGains(p*8.0,i*10.0,d*10.0); m_AngleOutput = m_AngleController.Calculate(NRADtoDEG(m_FusAngle.GetAngle())); m_vOutput = m_VangleController.Calculate(m_gyroRateAverage.get()); - m_Output = m_TiltTracker.m_kAnticipation*m_vOutput+(1-m_TiltTracker.m_kAnticipation)*m_AngleOutput; - frc::SmartDashboard::PutNumber("k_anticipation",m_TiltTracker.m_kAnticipation); + m_Output = m_vOutput+ m_TiltTracker.m_k*m_AngleOutput; + + frc::SmartDashboard::PutNumber("k_anticipation",m_TiltTracker.m_k); m_Output = NCLAMP(-0.3,0,0.3); frc::SmartDashboard::PutNumber("a",a); frc::SmartDashboard::PutNumber("error",m_AngleController.m_error); diff --git a/src/main/include/ChargeStationTiltTracker.h b/src/main/include/ChargeStationTiltTracker.h index 7ca4afd..191ecbe 100644 --- a/src/main/include/ChargeStationTiltTracker.h +++ b/src/main/include/ChargeStationTiltTracker.h @@ -8,20 +8,28 @@ class TiltTracker { public: TiltTracker(); - TiltTracker(double angle_threshold); + TiltTracker(const double angle_rate_threshold, const double dt_threshold, const double m_kmin); ~TiltTracker(); - void initialize(double left_encoder_dist, double right_encoder_dist, double m_angle, double estimated_next_tilt_distance ); - bool DetectTiltPoint(double left_encoder_dist, double right_encoder_dist,double angle); + void initialize(const double position,const double estimated_next_tilt_distance); + double Update(const double dt, const double position, const double angle,const double angle_rate); double getDistanceBetweenTilts(){return NABS(m_tiltB - m_tiltA);} double getAlgebricDistanceBetweenTilts(){return m_tiltB - m_tiltA;} double getNormalizeDistance(); + double m_tiltA; double m_tiltB; - double* m_pTilt; - double m_angle; + double* m_pFrom; + double* m_pTo; + + double m_dt; double m_angleThreshold; - double m_kAnticipation; + double m_deltaTimeThreshold; + + + double m_aParabolic; + double m_bParabolic; // = m_k minimum + double m_k; double m_a; From 69a23ca833b80a4de563c33fa2a9a96a28ebd168 Mon Sep 17 00:00:00 2001 From: mimi980 <90112270+mimi980@users.noreply.github.com> Date: Tue, 14 Feb 2023 09:43:37 +0100 Subject: [PATCH 28/36] smartdashboard --- src/main/cpp/Robot.cpp | 94 ++++++++++++++-------------------------- src/main/include/Robot.h | 2 +- 2 files changed, 33 insertions(+), 63 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index d10c0bf..b9074a7 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -37,7 +37,7 @@ void Robot::AutonomousInit() {} void Robot::AutonomousPeriodic() {} void Robot::TeleopInit() { - frc::SmartDashboard::PutNumber("P", 0.0); + frc::SmartDashboard::PutNumber("P", 0.02); frc::SmartDashboard::PutNumber("I", 0.0); frc::SmartDashboard::PutNumber("D", 0.0); frc::SmartDashboard::PutNumber("m_tau", 0.5); @@ -66,23 +66,22 @@ void Robot::TeleopInit() { m_MotorRightFollower.SetNeutralMode(ctre::phoenix::motorcontrol::NeutralMode::Brake); m_MotorRightFollower2.SetNeutralMode(ctre::phoenix::motorcontrol::NeutralMode::Brake); - m_AngleController.SetTolerance(4); m_state=State::End; m_EncoderLeft.SetDistancePerPulse(1.0/2048.0); m_EncoderRight.SetDistancePerPulse(1.0/2048.0); - m_logCSV.open("/home/lvuser/", true); - m_logCSV.setItem(0, "m_FusAngle",5, &m_LogFusAngle); - m_logCSV.setItem(1, "m_gyroRateAverage",5, &m_LogGyroRateAverage); - m_logCSV.setItem(2, "m_gyrorate",5, &m_LogGyroRate); - m_logCSV.setItem(3, "m_AngleController.m_error",5, &m_AngleController.m_error); - m_logCSV.setItem(4, "m_VangleController.m_error",5, &m_VangleController.m_error); - m_logCSV.setItem(5, "m_vOutput",5, &m_vOutput); - m_logCSV.setItem(6, "m_AngleOutput",5, &m_AngleOutput); - m_logCSV.setItem(7, "m_Output",5, &m_Output); - m_logCSV.setItem(8, "m_EncoderMetre",5, &m_LogEncoderM); - m_logCSV.setItem(9, "m_AccelX",5, &m_LogAccelX); - m_logCSV.setItem(10, "m_AngleAccel",5, &m_LogAngleAccel); + // m_logCSV.open("/home/lvuser/", true); + // m_logCSV.setItem(0, "m_FusAngle",5, &m_LogFusAngle); + // m_logCSV.setItem(1, "m_gyroRateAverage",5, &m_LogGyroRateAverage); + // m_logCSV.setItem(2, "m_gyrorate",5, &m_LogGyroRate); + // m_logCSV.setItem(3, "m_AngleController.m_error",5, &m_AngleController.m_error); + // m_logCSV.setItem(4, "m_VangleController.m_error",5, &m_VangleController.m_error); + // m_logCSV.setItem(5, "m_vOutput",5, &m_vOutput); + // m_logCSV.setItem(6, "m_AngleOutput",5, &m_AngleOutput); + // m_logCSV.setItem(7, "m_Output",5, &m_Output); + // m_logCSV.setItem(8, "m_EncoderMetre",5, &m_LogEncoderM); + // m_logCSV.setItem(9, "m_AccelX",5, &m_LogAccelX); + // m_logCSV.setItem(10, "m_AngleAccel",5, &m_LogAngleAccel); } @@ -124,58 +123,29 @@ void Robot::TeleopPeriodic() { // m_traveledDistance = 0.0; // a la place de ... m_distanceParcourue = 0.0; // on supprime .... m_distanceRestante = m_distanceAparcourir-m_distanceParcourue; m_AngleController.Reset(); - m_AngleController.SetSetpoint(0.0); // à la place de ... m_AngleController.SetSetpoint(0); + m_AngleController.SetSetpoint(0.0); + m_AngleController.SetTolerance(4); + // à la place de ... m_AngleController.SetSetpoint(0); m_VangleController.Reset(); - m_VangleController.SetSetpoint(0.0); + m_VangleController.SetSetpoint(0.0); + m_VangleController.SetTolerance(4); // m_errorSign = NSIGN(m_AngleController.m_error); // à la place de ... m_signe_error = NSIGN(m_AngleController.m_error); m_TiltTracker.initialize((m_EncoderLeft.GetDistance()+m_EncoderRight.GetDistance())*TRACTION_WHEEL_CIRCUMFERENCE/2.0,CHARGE_STATION_WIDTH); - m_state= State::Adjusting; } - - switch (m_state) - { - case Adjusting: - std::cout<<("Adjusting")< Date: Tue, 14 Feb 2023 22:15:54 +0100 Subject: [PATCH 29/36] modif en tout genre --- src/main/cpp/ChargeStationTiltTracker.cpp | 51 ++++++++++++----- src/main/cpp/Robot.cpp | 62 +++++++++++---------- src/main/include/ChargeStationTiltTracker.h | 7 ++- src/main/include/Robot.h | 6 +- src/main/include/lib/Angle_AG.h | 3 +- 5 files changed, 79 insertions(+), 50 deletions(-) diff --git a/src/main/cpp/ChargeStationTiltTracker.cpp b/src/main/cpp/ChargeStationTiltTracker.cpp index fa68bb8..ad0fb27 100644 --- a/src/main/cpp/ChargeStationTiltTracker.cpp +++ b/src/main/cpp/ChargeStationTiltTracker.cpp @@ -1,10 +1,11 @@ #include "lib/RblUtils.h" #include "ChargeStationTiltTracker.h" +#include /// @brief -TiltTracker::TiltTracker():m_dt(0.0),m_tiltA(0.0),m_tiltB(0.0),m_pFrom(&m_tiltA),m_pTo(&m_tiltB), m_angleThreshold(0.0),m_deltaTimeThreshold(0.0),m_aParabolic(1.0/(0.5*0.5)),m_bParabolic(0.0){} +TiltTracker::TiltTracker():m_dt(0.0),m_tiltA(0.0),m_tiltB(0.0),m_pFrom(&m_tiltA),m_pTo(&m_tiltB), m_angleThreshold(0.0),m_deltaTimeThreshold(0.0),m_aParabolic(1.0/(0.5*0.5)),m_bParabolic(0.0),m_angle(0.0){} -TiltTracker::TiltTracker(const double angle_rate_threshold, const double dt_threshold, const double kmin):m_dt(0.0),m_tiltA(0.0),m_tiltB(0.0),m_pFrom(&m_tiltA),m_pTo(&m_tiltB),m_angleThreshold(angle_rate_threshold),m_deltaTimeThreshold(dt_threshold),m_aParabolic((1.0-kmin)/(0.5*0.5)),m_bParabolic(kmin){} +TiltTracker::TiltTracker(const double angle_rate_threshold, const double dt_threshold, const double kmin):m_dt(0.0),m_tiltA(0.0),m_tiltB(0.0),m_pFrom(&m_tiltA),m_pTo(&m_tiltB),m_angleThreshold(angle_rate_threshold),m_deltaTimeThreshold(dt_threshold),m_aParabolic((1.0-kmin)/(0.5*0.5)),m_bParabolic(kmin),m_angle(0.0){} /// @brief TiltTracker::~TiltTracker(){} @@ -12,37 +13,61 @@ TiltTracker::~TiltTracker(){} /// @brief initialise le Tilttracker. A appeler au pied de la charge station. /// @param position valeur renvoyée par le getDistance de l'encodeur gauche en mètres /// @param estimated_next_tilt_distance estimation de la distance à laquelle surviendra le prochain Tilt en mètre -void TiltTracker::initialize(const double position,const double estimated_next_tilt_distance) +void TiltTracker::initialize(const double angle,const double position,const double estimated_next_tilt_distance) { + m_angle=angle; m_tiltA = position; m_tiltB = m_tiltA + estimated_next_tilt_distance; m_pFrom = &m_tiltA; m_pTo = &m_tiltB; } -double TiltTracker::Update(const double dt, const double position, const double angle,const double angle_rate) +bool TiltTracker::Update(const double dt, const double position, const double angle,const double angle_rate) { - + bool r=false; m_dt += dt; if(m_dt >= m_deltaTimeThreshold) { - m_dt = 0.0; - if( NSIGN(angle) != NSIGN(angle_rate) ) + std::cout<<"(m_dt >= m_deltaTimeThreshold)"<m_angleThreshold) { - if(angle_rate > m_angleThreshold) - { + std::cout<<"(abs(dif)>m_angleThreshold"< m_angleThreshold) + // { + r=true; *m_pTo = position; double *pd = m_pFrom; m_pFrom = m_pTo; m_pTo = pd; - } + m_dt = 0.0; + + // } } } + m_angle=angle; + +//-----------------------parabolic--------------------------- +// double nrmx = (position - *m_pFrom) / (*m_pTo - *m_pFrom); +// nrmx = NCLAMP(0,nrmx,1.0) - 0.5; + +// m_k = m_aParabolic*nrmx*nrmx + m_bParabolic; + +//----------------------------cos----------------------------- double nrmx = (position - *m_pFrom) / (*m_pTo - *m_pFrom); - nrmx = NCLAMP(0,nrmx,1.0) - 0.5; + nrmx = NCLAMP(0,nrmx,1.0) *NF64_PI; + nrmx=(cos(nrmx)+1.0)/2.0; + m_k = m_bParabolic + nrmx*(1-m_bParabolic); // m_bparabole = m_cosinus min + +//----------------------------cos2pi-------------------------- +// double nrmx = (position - *m_pFrom) / (*m_pTo - *m_pFrom); +// nrmx = NCLAMP(0,nrmx,1.0) *NF64_2PI; +// nrmx=(cos(nrmx)+1.0)/2.0; + +// m_k = m_bParabolic + nrmx*(1-m_bParabolic); // m_bparabole = m_cosinus min - m_k = m_aParabolic*nrmx*nrmx + m_bParabolic; - return m_k; + return r; } double TiltTracker::getNormalizeDistance() diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index b9074a7..b70e9de 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -37,7 +37,7 @@ void Robot::AutonomousInit() {} void Robot::AutonomousPeriodic() {} void Robot::TeleopInit() { - frc::SmartDashboard::PutNumber("P", 0.02); + frc::SmartDashboard::PutNumber("P", 0.016); frc::SmartDashboard::PutNumber("I", 0.0); frc::SmartDashboard::PutNumber("D", 0.0); frc::SmartDashboard::PutNumber("m_tau", 0.5); @@ -70,18 +70,18 @@ void Robot::TeleopInit() { m_EncoderLeft.SetDistancePerPulse(1.0/2048.0); m_EncoderRight.SetDistancePerPulse(1.0/2048.0); - // m_logCSV.open("/home/lvuser/", true); - // m_logCSV.setItem(0, "m_FusAngle",5, &m_LogFusAngle); - // m_logCSV.setItem(1, "m_gyroRateAverage",5, &m_LogGyroRateAverage); - // m_logCSV.setItem(2, "m_gyrorate",5, &m_LogGyroRate); - // m_logCSV.setItem(3, "m_AngleController.m_error",5, &m_AngleController.m_error); - // m_logCSV.setItem(4, "m_VangleController.m_error",5, &m_VangleController.m_error); - // m_logCSV.setItem(5, "m_vOutput",5, &m_vOutput); - // m_logCSV.setItem(6, "m_AngleOutput",5, &m_AngleOutput); - // m_logCSV.setItem(7, "m_Output",5, &m_Output); - // m_logCSV.setItem(8, "m_EncoderMetre",5, &m_LogEncoderM); - // m_logCSV.setItem(9, "m_AccelX",5, &m_LogAccelX); - // m_logCSV.setItem(10, "m_AngleAccel",5, &m_LogAngleAccel); + m_logCSV.open("/home/lvuser/", true); + m_logCSV.setItem(0, "m_FusAngle",5, &m_LogFusAngle); + m_logCSV.setItem(1, "m_gyroRateAverage",5, &m_LogGyroRateAverage); + m_logCSV.setItem(2, "m_gyrorate",5, &m_LogGyroRate); + m_logCSV.setItem(3, "m_AngleController.m_error",5, &m_AngleController.m_error); + m_logCSV.setItem(4, "m_VangleController.m_error",5, &m_VangleController.m_error); + m_logCSV.setItem(5, "m_vOutput",5, &m_vOutput); + m_logCSV.setItem(6, "m_AngleOutput",5, &m_AngleOutput); + m_logCSV.setItem(7, "m_Output",5, &m_Output); + m_logCSV.setItem(8, "m_EncoderMetre",5, &m_LogEncoderM); + m_logCSV.setItem(9, "m_AccelX",5, &m_LogAccelX); + m_logCSV.setItem(10, "m_AngleAccel",5, &m_LogAngleAccel); } @@ -128,23 +128,25 @@ void Robot::TeleopPeriodic() { // à la place de ... m_AngleController.SetSetpoint(0); m_VangleController.Reset(); m_VangleController.SetSetpoint(0.0); - m_VangleController.SetTolerance(4); + m_VangleController.SetTolerance(1); // m_errorSign = NSIGN(m_AngleController.m_error); // à la place de ... m_signe_error = NSIGN(m_AngleController.m_error); - m_TiltTracker.initialize((m_EncoderLeft.GetDistance()+m_EncoderRight.GetDistance())*TRACTION_WHEEL_CIRCUMFERENCE/2.0,CHARGE_STATION_WIDTH); + m_TiltTracker.initialize(m_FusAngle.GetAngle(),(m_EncoderLeft.GetDistance()+m_EncoderRight.GetDistance())*TRACTION_WHEEL_CIRCUMFERENCE/2.0,CHARGE_STATION_WIDTH); } - double p=frc::SmartDashboard::GetNumber("P",0.02); + double p=frc::SmartDashboard::GetNumber("P",0.012); double i =frc::SmartDashboard::GetNumber("I",0.0); double d =frc::SmartDashboard::GetNumber("D",0.0); - m_TiltTracker.Update(0.02,(m_EncoderLeft.GetDistance()+m_EncoderRight.GetDistance())*TRACTION_WHEEL_CIRCUMFERENCE/2.0,m_FusAngle.GetAngle(),m_gyroRateAverage.get()); - m_AngleController.SetGains(p,i,d); // avant m_AngleController.SetGains(a,0.0,0.0); - m_VangleController.SetGains(p*0.8,i*10.0,d*10.0); + bool tilt = m_TiltTracker.Update(0.02,(m_EncoderLeft.GetDistance()+m_EncoderRight.GetDistance())*TRACTION_WHEEL_CIRCUMFERENCE/2.0,m_FusAngle.GetAngle(),m_gyroRateAverage.get()); + m_AngleController.SetGains(0.03,0,0); // avant m_AngleController.SetGains(a,0.0,0.0); + m_VangleController.SetGains(0.005,i*10.0,d*10.0); m_AngleOutput = m_AngleController.Calculate(NRADtoDEG(m_FusAngle.GetAngle())); m_vOutput = m_VangleController.Calculate(m_gyroRateAverage.get()); - m_Output = m_vOutput+ m_TiltTracker.m_k*m_AngleOutput; + m_Output = m_TiltTracker.m_k*m_AngleOutput;// voutput frc::SmartDashboard::PutNumber("k_anticipation",m_TiltTracker.m_k); - m_Output = NCLAMP(-0.5,m_Output,0.5); + frc::SmartDashboard::PutNumber("k*angleoutput",m_TiltTracker.m_k*m_AngleOutput); + frc::SmartDashboard::PutNumber("delta_angle",m_FusAngle.m_delta); + m_Output = NCLAMP(-0.3,m_Output,0.3); frc::SmartDashboard::PutNumber("output",m_Output); frc::SmartDashboard::PutNumber("a",a); frc::SmartDashboard::PutNumber("error",m_AngleController.m_error); @@ -153,7 +155,7 @@ void Robot::TeleopPeriodic() { frc::SmartDashboard::PutNumber("m_encoderRight",m_EncoderRight.GetDistance()); frc::SmartDashboard::PutNumber("m_encoderLeft",m_EncoderLeft.GetDistance()); frc::SmartDashboard::PutNumber("encoder",((m_EncoderLeft.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE)+(m_EncoderRight.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE))/2); - + frc::SmartDashboard::PutBoolean("tilt",tilt); if (m_joystickLeft.GetRawButton(1)) { @@ -171,17 +173,17 @@ void Robot::TeleopPeriodic() { frc::SmartDashboard::PutNumber("m_Output", m_Output); frc::SmartDashboard::PutNumber("m_vOutput", m_vOutput); frc::SmartDashboard::PutNumber("m_AngleOutput", m_AngleOutput); - // m_LogFusAngle = m_FusAngle.GetAngle(); - // m_LogGyroRateAverage = m_gyroRateAverage.get(); - // m_LogGyroRate = m_gyro.GetRate(); - // m_LogEncoderM = ((m_EncoderLeft.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE)+(m_EncoderRight.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE))/2; - // m_LogAngleAccel = m_FusAngle.m_angleAccel; - // m_LogAccelX = m_accelerometer.GetX(); + m_LogFusAngle = m_FusAngle.GetAngle(); + m_LogGyroRateAverage = m_gyroRateAverage.get(); + m_LogGyroRate = m_gyro.GetRate(); + m_LogEncoderM = ((m_EncoderLeft.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE)+(m_EncoderRight.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE))/2; + m_LogAngleAccel = m_FusAngle.m_angleAccel; + m_LogAccelX = m_accelerometer.GetX(); - // m_logCSV.write(); + m_logCSV.write(); } @@ -189,7 +191,7 @@ void Robot::TeleopPeriodic() { void Robot::DisabledInit() { m_gyro.Reset(); - // m_logCSV.close(); + m_logCSV.close(); } void Robot::DisabledPeriodic() {} diff --git a/src/main/include/ChargeStationTiltTracker.h b/src/main/include/ChargeStationTiltTracker.h index 191ecbe..36d2e66 100644 --- a/src/main/include/ChargeStationTiltTracker.h +++ b/src/main/include/ChargeStationTiltTracker.h @@ -1,5 +1,6 @@ #pragma once #include +#include #include "lib/RblUtils.h" #define K_MIN 0.2 #define K_MAX 0.8 @@ -11,8 +12,8 @@ class TiltTracker TiltTracker(const double angle_rate_threshold, const double dt_threshold, const double m_kmin); ~TiltTracker(); - void initialize(const double position,const double estimated_next_tilt_distance); - double Update(const double dt, const double position, const double angle,const double angle_rate); + void initialize(const double angle,const double position,const double estimated_next_tilt_distance); + bool Update(const double dt, const double position, const double angle,const double angle_rate); double getDistanceBetweenTilts(){return NABS(m_tiltB - m_tiltA);} double getAlgebricDistanceBetweenTilts(){return m_tiltB - m_tiltA;} double getNormalizeDistance(); @@ -30,7 +31,7 @@ class TiltTracker double m_aParabolic; double m_bParabolic; // = m_k minimum double m_k; - double m_a; + double m_angle; }; diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index c5e1448..30d4666 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -13,7 +13,7 @@ #define NOISE 0.1 // à la place de .... #define bruit 0.1 -#define CHARGE_STATION_WIDTH 1.0 // en metre +#define CHARGE_STATION_WIDTH 1.75 // en metre #define TRACTION_WHEEL_DIAMETER (6.0*0.0254) // Diametre des roues du robot #define TRACTION_WHEEL_RADIUS (3.0*0.0254) // rayon des roues du robot @@ -95,7 +95,7 @@ class Robot : public frc::TimedRobot { NdoubleRollingAverage m_AccelerometerX_Average{25}; NdoubleRollingAverage m_AccelerometerX_Arcos_Average{25}; - NdoubleRollingAverage m_gyroRateAverage{10}; + NdoubleRollingAverage m_gyroRateAverage{25}; NdoubleRollingAverage m_error{10}; @@ -112,7 +112,7 @@ class Robot : public frc::TimedRobot { Pid m_VangleController{0.0,0.01,0.0005,0.0}; Pid m_AngleController{0.0,0.01,0.0005,0.0}; - TiltTracker m_TiltTracker{2.0,0.2,0.6}; + TiltTracker m_TiltTracker{0.05,1.0,0.2};//{80.0,0.5,0.4}; ça marche double m_kPmin; double m_kPmax; diff --git a/src/main/include/lib/Angle_AG.h b/src/main/include/lib/Angle_AG.h index 0f92939..e1bb6e3 100644 --- a/src/main/include/lib/Angle_AG.h +++ b/src/main/include/lib/Angle_AG.h @@ -20,13 +20,14 @@ class Angle_AG void AutoSetBias(){m_bias=m_angle;}; double m_angleAccel; // angle en radian calculé avec l'accelerometre + double m_delta; // différence entre l'angle et l'angle précédent + private : double m_angle; // angle en radian combinant accel et gyro - double m_delta; // différence entre l'angle et l'angle précédent double m_dt; // intervalle de temps en seconde entre double m_tau; // temp en seconde double m_k; // coef calculé grace à m_tau et dt From 8f1def29fcad4bafe89a4732cd3fa04abe667ef8 Mon Sep 17 00:00:00 2001 From: jjTerazan <47304610+jjTerazan@users.noreply.github.com> Date: Wed, 15 Feb 2023 08:51:25 +0100 Subject: [PATCH 30/36] tiltTracker en cours de refonte. --- src/main/cpp/ChargeStationTiltTracker.cpp | 50 +++++++++-- src/main/cpp/Robot.cpp | 10 +-- ...NRollingAverage.cpp => NMovingAverage.cpp} | 82 +++++++++++++------ src/main/include/ChargeStationTiltTracker.h | 12 ++- src/main/include/Robot.h | 12 +-- .../{NRollingAverage.h => NMovingAverage.h} | 35 +++++--- 6 files changed, 144 insertions(+), 57 deletions(-) rename src/main/cpp/lib/{NRollingAverage.cpp => NMovingAverage.cpp} (56%) rename src/main/include/lib/{NRollingAverage.h => NMovingAverage.h} (61%) diff --git a/src/main/cpp/ChargeStationTiltTracker.cpp b/src/main/cpp/ChargeStationTiltTracker.cpp index ad0fb27..a8906fb 100644 --- a/src/main/cpp/ChargeStationTiltTracker.cpp +++ b/src/main/cpp/ChargeStationTiltTracker.cpp @@ -3,9 +3,18 @@ #include /// @brief -TiltTracker::TiltTracker():m_dt(0.0),m_tiltA(0.0),m_tiltB(0.0),m_pFrom(&m_tiltA),m_pTo(&m_tiltB), m_angleThreshold(0.0),m_deltaTimeThreshold(0.0),m_aParabolic(1.0/(0.5*0.5)),m_bParabolic(0.0),m_angle(0.0){} +TiltTracker::TiltTracker():m_dt(0.0),m_tiltA(0.0),m_tiltB(0.0),m_pFrom(&m_tiltA),m_pTo(&m_tiltB), m_threshold(0.0),m_deltaTimeThreshold(0.0),m_aParabolic(1.0/(0.5*0.5)),m_bParabolic(0.0),m_angle(0.0){} -TiltTracker::TiltTracker(const double angle_rate_threshold, const double dt_threshold, const double kmin):m_dt(0.0),m_tiltA(0.0),m_tiltB(0.0),m_pFrom(&m_tiltA),m_pTo(&m_tiltB),m_angleThreshold(angle_rate_threshold),m_deltaTimeThreshold(dt_threshold),m_aParabolic((1.0-kmin)/(0.5*0.5)),m_bParabolic(kmin),m_angle(0.0){} +TiltTracker::TiltTracker(const int average_samples_nb, const double threshold, const double dt_threshold, const double kmin):m_filteredRateAverage(average_samples_nb), + m_dt(0.0), + m_tiltA(0.0), + m_tiltB(0.0), + m_pFrom(&m_tiltA), + m_pTo(&m_tiltB), + m_threshold(threshold), + m_deltaTimeThreshold(dt_threshold), + m_aParabolic((1.0-kmin)/(0.5*0.5)), + m_bParabolic(kmin),m_angle(0.0){} /// @brief TiltTracker::~TiltTracker(){} @@ -15,16 +24,22 @@ TiltTracker::~TiltTracker(){} /// @param estimated_next_tilt_distance estimation de la distance à laquelle surviendra le prochain Tilt en mètre void TiltTracker::initialize(const double angle,const double position,const double estimated_next_tilt_distance) { - m_angle=angle; + m_angle = angle; m_tiltA = position; m_tiltB = m_tiltA + estimated_next_tilt_distance; m_pFrom = &m_tiltA; m_pTo = &m_tiltB; + m_filteredRateAverage.reset(0,0.0); + m_previousFilteredRate = 0.0; } bool TiltTracker::Update(const double dt, const double position, const double angle,const double angle_rate) { bool r=false; + + /* + * ------------------------------------------------------------------------------------ + * V0.0 m_dt += dt; if(m_dt >= m_deltaTimeThreshold) { @@ -47,11 +62,37 @@ bool TiltTracker::Update(const double dt, const double position, const double an } } m_angle=angle; +* +* ------------------------------------------------------------------------------------ +*/ + +/* + * ------------------------------------------------------------------------------------ + * V0.1 +*/ +// Dans cette nouvelle version, angle_rate est supposé être la valeur brute de Gyro rate le tiltTracker incluant cette moyenne ! +double dv = angle_rate - m_filteredRateAverage.m_mean; +double filtered_rate = angle_rate; +// ci-dessous, ... pour eviter la racine carrée du "vrai test" à effectuer à partir de l'écart type: if(NABS(dv) > m_angleThreshold*sqrt(m_rateAverage.m_variance) ) +// on utilise la variance ( = ecart type au carré ), threshold devra donc être ajusté en conséquence ( élevé au carré pour conserver les mêmes résultats qu'avec la standard deviation) +if((dv*dv) > m_angleThreshold*m_filteredRateAverage.m_variance) +{ + // à partir d'ici nous sommes sûrs que : dv != 0.0 + + if(dv > 0.0) // angle_rate > m_rateAverage.m_mean + { + + } + else // angle_rate < m_rateAverage.m_mean + { + + } + filtered_rate +} //-----------------------parabolic--------------------------- // double nrmx = (position - *m_pFrom) / (*m_pTo - *m_pFrom); // nrmx = NCLAMP(0,nrmx,1.0) - 0.5; - // m_k = m_aParabolic*nrmx*nrmx + m_bParabolic; //----------------------------cos----------------------------- @@ -64,7 +105,6 @@ bool TiltTracker::Update(const double dt, const double position, const double an // double nrmx = (position - *m_pFrom) / (*m_pTo - *m_pFrom); // nrmx = NCLAMP(0,nrmx,1.0) *NF64_2PI; // nrmx=(cos(nrmx)+1.0)/2.0; - // m_k = m_bParabolic + nrmx*(1-m_bParabolic); // m_bparabole = m_cosinus min return r; diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index b70e9de..e2badf7 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -101,10 +101,10 @@ void Robot::TeleopPeriodic() { m_FusAngle.SetTau(frc::SmartDashboard::GetNumber("m_tau",0.5)); m_FusAngle.Update(NDEGtoRAD(gyro_rate_degps),y_g,x_g); // A la place de ... m_FusAngle.Update(m_gyro.GetRate()/180.0*3.14159265,m_accelerometer.GetY(),m_accelerometer.GetX()); - m_gyroRateAverage.add(gyro_rate_degps); //m_gyroRateAverage.add(m_FusAngle.GetDelta()*180.0/3.14159265); + m_gyroRateAverage.addSample(gyro_rate_degps); //m_gyroRateAverage.add(m_FusAngle.GetDelta()*180.0/3.14159265); frc::SmartDashboard::PutNumber("FuseAngle",NRADtoDEG(m_FusAngle.GetAngle())); - frc::SmartDashboard::PutNumber("GyroRate",m_gyroRateAverage.get()); + frc::SmartDashboard::PutNumber("GyroRate",m_gyroRateAverage.getMean()); frc::SmartDashboard::PutNumber("anglegyro",m_gyro.GetAngle()); // frc::SmartDashboard::PutNumber("m_K",m_FusAngle.m_k); // frc::SmartDashboard::PutNumber("Accel_X",x); @@ -136,11 +136,11 @@ void Robot::TeleopPeriodic() { double i =frc::SmartDashboard::GetNumber("I",0.0); double d =frc::SmartDashboard::GetNumber("D",0.0); - bool tilt = m_TiltTracker.Update(0.02,(m_EncoderLeft.GetDistance()+m_EncoderRight.GetDistance())*TRACTION_WHEEL_CIRCUMFERENCE/2.0,m_FusAngle.GetAngle(),m_gyroRateAverage.get()); + bool tilt = m_TiltTracker.Update(0.02,(m_EncoderLeft.GetDistance()+m_EncoderRight.GetDistance())*TRACTION_WHEEL_CIRCUMFERENCE/2.0,m_FusAngle.GetAngle(),m_gyroRateAverage.getMean()); m_AngleController.SetGains(0.03,0,0); // avant m_AngleController.SetGains(a,0.0,0.0); m_VangleController.SetGains(0.005,i*10.0,d*10.0); m_AngleOutput = m_AngleController.Calculate(NRADtoDEG(m_FusAngle.GetAngle())); - m_vOutput = m_VangleController.Calculate(m_gyroRateAverage.get()); + m_vOutput = m_VangleController.Calculate(m_gyroRateAverage.getMean()); m_Output = m_TiltTracker.m_k*m_AngleOutput;// voutput frc::SmartDashboard::PutNumber("k_anticipation",m_TiltTracker.m_k); @@ -174,7 +174,7 @@ void Robot::TeleopPeriodic() { frc::SmartDashboard::PutNumber("m_vOutput", m_vOutput); frc::SmartDashboard::PutNumber("m_AngleOutput", m_AngleOutput); m_LogFusAngle = m_FusAngle.GetAngle(); - m_LogGyroRateAverage = m_gyroRateAverage.get(); + m_LogGyroRateAverage = m_gyroRateAverage.getMean(); m_LogGyroRate = m_gyro.GetRate(); m_LogEncoderM = ((m_EncoderLeft.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE)+(m_EncoderRight.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE))/2; m_LogAngleAccel = m_FusAngle.m_angleAccel; diff --git a/src/main/cpp/lib/NRollingAverage.cpp b/src/main/cpp/lib/NMovingAverage.cpp similarity index 56% rename from src/main/cpp/lib/NRollingAverage.cpp rename to src/main/cpp/lib/NMovingAverage.cpp index 2884a4f..bbcd644 100644 --- a/src/main/cpp/lib/NRollingAverage.cpp +++ b/src/main/cpp/lib/NMovingAverage.cpp @@ -1,60 +1,90 @@ -#include "lib/NRollingAverage.h" +#include "lib/NMovingAverage.h" -NdoubleRollingAverage::NdoubleRollingAverage() +NdoubleMovingAverage::NdoubleMovingAverage() { m_last = 0; - m_pdouble = NULL; + m_pSamples = NULL; m_sum = 0.0; - m_average = 0.0; + m_sum2 = 0.0; + m_mean = 0.0; + m_variance = 0.0; m_index = 0; } -NdoubleRollingAverage::NdoubleRollingAverage(const int table_size, const double initial_average) +NdoubleMovingAverage::NdoubleMovingAverage(const int table_size, const double initial_average) { m_last = table_size - 1; - m_pdouble = (double*)malloc(sizeof(double)*table_size); + m_pSamples = (double*)malloc(sizeof(double)*table_size); m_sum = initial_average*table_size; - m_average = initial_average; + m_sum2 = m_sum*initial_average; + m_mean = initial_average; + m_variance = 0.0; m_index = 0; - double *pd = m_pdouble; + double *pd = m_pSamples; for(int i= 0;i 2 + { + if(m_last != (table_size - 1)) + { + m_last = table_size - 1; + m_pSamples = (double*)realloc(m_pSamples, sizeof(double)*table_size); + m_index = 0; + } + } + + m_sum = initial_average*(m_last+1); + m_sum2 = m_sum*initial_average; + m_mean = initial_average; + m_variance = 0.0; + m_pSamples -= m_index; // replace le pointeur en position initiale + m_index = 0; + + double *pd = m_pSamples; for(int i= 0;i<=m_last;i++,pd++) *pd = initial_average; } diff --git a/src/main/include/ChargeStationTiltTracker.h b/src/main/include/ChargeStationTiltTracker.h index 36d2e66..0b12493 100644 --- a/src/main/include/ChargeStationTiltTracker.h +++ b/src/main/include/ChargeStationTiltTracker.h @@ -2,6 +2,8 @@ #include #include #include "lib/RblUtils.h" +#include "lib/NMovingAverage.h" + #define K_MIN 0.2 #define K_MAX 0.8 @@ -9,7 +11,7 @@ class TiltTracker { public: TiltTracker(); - TiltTracker(const double angle_rate_threshold, const double dt_threshold, const double m_kmin); + TiltTracker(const int rate_average_range, const double angle_rate_threshold, const double dt_threshold, const double m_kmin); ~TiltTracker(); void initialize(const double angle,const double position,const double estimated_next_tilt_distance); @@ -24,9 +26,15 @@ class TiltTracker double* m_pTo; double m_dt; - double m_angleThreshold; + double m_threshold; + double m_peakInfluence; + double m_deltaTimeThreshold; + double m_previousFilteredRate; + NdoubleMovingAverage m_filteredRateAverage; + + double m_aParabolic; double m_bParabolic; // = m_k minimum diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 30d4666..532d9ce 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -31,7 +31,7 @@ #include #include #include -#include +#include #include #include #include @@ -93,11 +93,11 @@ class Robot : public frc::TimedRobot { ctre::phoenix::motorcontrol::can::TalonFX m_MotorLeftFollower2{6}; - NdoubleRollingAverage m_AccelerometerX_Average{25}; - NdoubleRollingAverage m_AccelerometerX_Arcos_Average{25}; - NdoubleRollingAverage m_gyroRateAverage{25}; + NdoubleMovingAverage m_AccelerometerX_Average{25}; + NdoubleMovingAverage m_AccelerometerX_Arcos_Average{25}; + NdoubleMovingAverage m_gyroRateAverage{25}; - NdoubleRollingAverage m_error{10}; + NdoubleMovingAverage m_error{10}; Dynamic m_AccelerometerX; Dynamic m_AccelerometerY; @@ -112,7 +112,7 @@ class Robot : public frc::TimedRobot { Pid m_VangleController{0.0,0.01,0.0005,0.0}; Pid m_AngleController{0.0,0.01,0.0005,0.0}; - TiltTracker m_TiltTracker{0.05,1.0,0.2};//{80.0,0.5,0.4}; ça marche + TiltTracker m_TiltTracker{5,0.05,1.0,0.2};//{80.0,0.5,0.4}; ça marche double m_kPmin; double m_kPmax; diff --git a/src/main/include/lib/NRollingAverage.h b/src/main/include/lib/NMovingAverage.h similarity index 61% rename from src/main/include/lib/NRollingAverage.h rename to src/main/include/lib/NMovingAverage.h index 56c92c4..d1bcf0e 100644 --- a/src/main/include/lib/NRollingAverage.h +++ b/src/main/include/lib/NMovingAverage.h @@ -1,6 +1,8 @@ #pragma once #include +#include + // *************************************************************************************** // *************************************************************************************** // ** ** @@ -8,22 +10,29 @@ // ** ** // *************************************************************************************** // *************************************************************************************** -class NdoubleRollingAverage +#define NMA_DEFAULT_SAMPLE_NB 8 +class NdoubleMovingAverage { public: - NdoubleRollingAverage(); - NdoubleRollingAverage(const int table_size, const double initial_average = 0.0 ); - ~NdoubleRollingAverage(); + NdoubleMovingAverage(); + NdoubleMovingAverage(const int table_size, const double initial_average = 0.0 ); + ~NdoubleMovingAverage(); + + void reset(const int table_size = 0, const double initial_average = 0.0 ); + const double addSample(const double value); + inline const double getMean(){return m_mean;} + inline const double getVariance(){return m_variance;} + inline const double getStdDeviation(){return sqrt(m_variance);} + + double m_sum2; // somme du carré des samples + double m_sum; // Somme algérique des samples + double* m_pSamples; // Table des samples + int m_last; // index du dernier sample ( = tablesize - 1) + int m_index; // index du sample "à écrire" + + double m_mean; // moyenne + double m_variance; // variance - void reset(const double initial_average = 0.0); - const double add(const double value); - inline const double get(){return m_average;} - - int m_last; - int m_index; - double m_average; - double m_sum; - double* m_pdouble; }; class NfloatRollingAverage From da9f6294b1220afa8022173d326dc2b4e4332131 Mon Sep 17 00:00:00 2001 From: Nathan Mutin <32647745+nathanmutin@users.noreply.github.com> Date: Wed, 15 Feb 2023 12:13:05 +0100 Subject: [PATCH 31/36] tilttracker updated - a debuger --- src/main/cpp/ChargeStationTiltTracker.cpp | 95 +++++++++++++++++---- src/main/include/ChargeStationTiltTracker.h | 6 +- 2 files changed, 82 insertions(+), 19 deletions(-) diff --git a/src/main/cpp/ChargeStationTiltTracker.cpp b/src/main/cpp/ChargeStationTiltTracker.cpp index a8906fb..aed4c81 100644 --- a/src/main/cpp/ChargeStationTiltTracker.cpp +++ b/src/main/cpp/ChargeStationTiltTracker.cpp @@ -3,18 +3,39 @@ #include /// @brief -TiltTracker::TiltTracker():m_dt(0.0),m_tiltA(0.0),m_tiltB(0.0),m_pFrom(&m_tiltA),m_pTo(&m_tiltB), m_threshold(0.0),m_deltaTimeThreshold(0.0),m_aParabolic(1.0/(0.5*0.5)),m_bParabolic(0.0),m_angle(0.0){} - -TiltTracker::TiltTracker(const int average_samples_nb, const double threshold, const double dt_threshold, const double kmin):m_filteredRateAverage(average_samples_nb), - m_dt(0.0), - m_tiltA(0.0), - m_tiltB(0.0), - m_pFrom(&m_tiltA), - m_pTo(&m_tiltB), - m_threshold(threshold), - m_deltaTimeThreshold(dt_threshold), - m_aParabolic((1.0-kmin)/(0.5*0.5)), - m_bParabolic(kmin),m_angle(0.0){} +TiltTracker::TiltTracker(): m_filteredRateAverage(), + /*m_dt(0.0),*/ + m_tiltA(0.0), + m_tiltB(0.0), + m_pFrom(&m_tiltA), + m_pTo(&m_tiltB), + m_threshold(0.0), + m_deltaTimeThreshold(0.0), + m_aParabolic(1.0/(0.5*0.5)), + m_bParabolic(0.0), + m_peakWidth(0.0), + m_tilted(0), + m_angle(0.0), + m_peakInfluence(0.0), + m_previousFilteredRate(0.0), + m_k(0.0){} + +TiltTracker::TiltTracker(const int average_samples_nb, const double threshold, const double dt_threshold, const double kmin): m_filteredRateAverage(average_samples_nb), + /*m_dt(0.0),*/ + m_tiltA(0.0), + m_tiltB(0.0), + m_pFrom(&m_tiltA), + m_pTo(&m_tiltB), + m_threshold(threshold), + m_deltaTimeThreshold(dt_threshold), + m_aParabolic((1.0-kmin)/(0.5*0.5)), + m_bParabolic(kmin), + m_peakWidth(0.0), + m_tilted(0), + m_angle(0.0), + m_peakInfluence(0.0), + m_previousFilteredRate(0.0), + m_k(0.0){} /// @brief TiltTracker::~TiltTracker(){} @@ -24,6 +45,7 @@ TiltTracker::~TiltTracker(){} /// @param estimated_next_tilt_distance estimation de la distance à laquelle surviendra le prochain Tilt en mètre void TiltTracker::initialize(const double angle,const double position,const double estimated_next_tilt_distance) { + m_tilted= 0; m_angle = angle; m_tiltA = position; m_tiltB = m_tiltA + estimated_next_tilt_distance; @@ -72,24 +94,63 @@ bool TiltTracker::Update(const double dt, const double position, const double an */ // Dans cette nouvelle version, angle_rate est supposé être la valeur brute de Gyro rate le tiltTracker incluant cette moyenne ! double dv = angle_rate - m_filteredRateAverage.m_mean; -double filtered_rate = angle_rate; +double filtered_rate; +bool btilt = false; // ci-dessous, ... pour eviter la racine carrée du "vrai test" à effectuer à partir de l'écart type: if(NABS(dv) > m_angleThreshold*sqrt(m_rateAverage.m_variance) ) // on utilise la variance ( = ecart type au carré ), threshold devra donc être ajusté en conséquence ( élevé au carré pour conserver les mêmes résultats qu'avec la standard deviation) -if((dv*dv) > m_angleThreshold*m_filteredRateAverage.m_variance) +if((dv*dv) > m_threshold*m_filteredRateAverage.m_variance) { // à partir d'ici nous sommes sûrs que : dv != 0.0 if(dv > 0.0) // angle_rate > m_rateAverage.m_mean { - + // pic positif + if(m_peakWidth<0.0) + m_peakWidth = 0.0; + + m_peakWidth += dt; + if(m_peakWidth > m_deltaTimeThreshold) + { + if(m_tilted<=0) + { + m_tilted = 1; + btilt = true; + } + } } else // angle_rate < m_rateAverage.m_mean { - + // pic negatif + if(m_peakWidth>0.0) + m_peakWidth = 0.0; + + m_peakWidth = -dt; + if(m_peakWidth < -m_deltaTimeThreshold) + { + if(m_tilted>=0) + { + m_tilted = -1; + btilt = true; + } + } } - filtered_rate + + filtered_rate = m_peakInfluence*angle_rate + (1.0-m_peakInfluence)*m_previousFilteredRate; +} +else +{ + filtered_rate = angle_rate; } +m_filteredRateAverage.addSample(filtered_rate); +// tilt ? +if(tilt != 0) +{ + *m_pTo = position; + double *pd = m_pFrom; + m_pFrom = m_pTo; + m_pTo = pd; +} //-----------------------parabolic--------------------------- // double nrmx = (position - *m_pFrom) / (*m_pTo - *m_pFrom); // nrmx = NCLAMP(0,nrmx,1.0) - 0.5; diff --git a/src/main/include/ChargeStationTiltTracker.h b/src/main/include/ChargeStationTiltTracker.h index 0b12493..d935ce5 100644 --- a/src/main/include/ChargeStationTiltTracker.h +++ b/src/main/include/ChargeStationTiltTracker.h @@ -25,15 +25,17 @@ class TiltTracker double* m_pFrom; double* m_pTo; - double m_dt; + //double m_dt; double m_threshold; double m_peakInfluence; + + double m_peakWidth; double m_deltaTimeThreshold; double m_previousFilteredRate; NdoubleMovingAverage m_filteredRateAverage; - + int m_tilted; double m_aParabolic; From e960d96cf84037a750c5beee38a692f7a5ae6537 Mon Sep 17 00:00:00 2001 From: mimi980 <90112270+mimi980@users.noreply.github.com> Date: Wed, 15 Feb 2023 16:10:59 +0100 Subject: [PATCH 32/36] modif --- src/main/cpp/ChargeStationTiltTracker.cpp | 313 +++++++++++--------- src/main/cpp/Robot.cpp | 194 ++++++------ src/main/cpp/lib/NMovingAverage.cpp | 179 ++++++----- src/main/include/ChargeStationTiltTracker.h | 56 ++-- src/main/include/Robot.h | 61 ++-- src/main/include/lib/NMovingAverage.h | 2 +- 6 files changed, 410 insertions(+), 395 deletions(-) diff --git a/src/main/cpp/ChargeStationTiltTracker.cpp b/src/main/cpp/ChargeStationTiltTracker.cpp index aed4c81..cd36f1e 100644 --- a/src/main/cpp/ChargeStationTiltTracker.cpp +++ b/src/main/cpp/ChargeStationTiltTracker.cpp @@ -2,173 +2,198 @@ #include "ChargeStationTiltTracker.h" #include -/// @brief -TiltTracker::TiltTracker(): m_filteredRateAverage(), - /*m_dt(0.0),*/ - m_tiltA(0.0), - m_tiltB(0.0), - m_pFrom(&m_tiltA), - m_pTo(&m_tiltB), - m_threshold(0.0), - m_deltaTimeThreshold(0.0), - m_aParabolic(1.0/(0.5*0.5)), - m_bParabolic(0.0), - m_peakWidth(0.0), - m_tilted(0), - m_angle(0.0), - m_peakInfluence(0.0), - m_previousFilteredRate(0.0), - m_k(0.0){} - -TiltTracker::TiltTracker(const int average_samples_nb, const double threshold, const double dt_threshold, const double kmin): m_filteredRateAverage(average_samples_nb), - /*m_dt(0.0),*/ - m_tiltA(0.0), - m_tiltB(0.0), - m_pFrom(&m_tiltA), - m_pTo(&m_tiltB), - m_threshold(threshold), - m_deltaTimeThreshold(dt_threshold), - m_aParabolic((1.0-kmin)/(0.5*0.5)), - m_bParabolic(kmin), - m_peakWidth(0.0), - m_tilted(0), - m_angle(0.0), - m_peakInfluence(0.0), - m_previousFilteredRate(0.0), - m_k(0.0){} - -/// @brief -TiltTracker::~TiltTracker(){} - -/// @brief initialise le Tilttracker. A appeler au pied de la charge station. -/// @param position valeur renvoyée par le getDistance de l'encodeur gauche en mètres +/// @brief +TiltTracker::TiltTracker() : m_filteredRateAverage(), + /*m_dt(0.0),*/ + m_tiltA(0.0), + m_tiltB(0.0), + m_pFrom(&m_tiltA), + m_pTo(&m_tiltB), + m_threshold(0.0), + m_deltaTimeThreshold(0.0), + m_aParabolic(1.0 / (0.5 * 0.5)), + m_bParabolic(0.0), + m_peakWidth(0.0), + m_tilted(0), + m_angle(0.0), + m_peakInfluence(0.0), + m_previousFilteredRate(0.0), + m_k(0.0) +{ +} + +TiltTracker::TiltTracker(const int average_samples_nb, const double threshold, const double dt_threshold, const double peak_influence, const double kmin) : m_filteredRateAverage(average_samples_nb), + /*m_dt(0.0),*/ + m_tiltA(0.0), + m_tiltB(0.0), + m_pFrom(&m_tiltA), + m_pTo(&m_tiltB), + m_threshold(threshold), + m_deltaTimeThreshold(dt_threshold), + m_aParabolic((1.0 - kmin) / (0.5 * 0.5)), + m_bParabolic(kmin), + m_peakWidth(0.0), + m_tilted(0), + m_angle(0.0), + m_peakInfluence(peak_influence), + m_previousFilteredRate(0.0), + m_k(0.0) +{ +} + +/// @brief +TiltTracker::~TiltTracker() {} + +/// @brief initialise le Tilttracker. A appeler au pied de la charge station. +/// @param position valeur renvoyée par le getDistance de l'encodeur gauche en mètres /// @param estimated_next_tilt_distance estimation de la distance à laquelle surviendra le prochain Tilt en mètre -void TiltTracker::initialize(const double angle,const double position,const double estimated_next_tilt_distance) +void TiltTracker::initialize(const double angle, const double position, const double estimated_next_tilt_distance) { - m_tilted= 0; + m_tilted = 0; m_angle = angle; m_tiltA = position; m_tiltB = m_tiltA + estimated_next_tilt_distance; m_pFrom = &m_tiltA; - m_pTo = &m_tiltB; - m_filteredRateAverage.reset(0,0.0); + m_pTo = &m_tiltB; + m_filteredRateAverage.reset(0, 0.0); m_previousFilteredRate = 0.0; } -bool TiltTracker::Update(const double dt, const double position, const double angle,const double angle_rate) +double TiltTracker::Update(const double dt, const double position, const double angle, const double angle_rate) { - bool r=false; + double r = 0; + + /* + * ------------------------------------------------------------------------------------ + * V0.0 + m_dt += dt; + if(m_dt >= m_deltaTimeThreshold) + { + std::cout<<"(m_dt >= m_deltaTimeThreshold)"<m_angleThreshold) + { + std::cout<<"(abs(dif)>m_angleThreshold"< m_angleThreshold) + // { + r=true; + *m_pTo = position; + double *pd = m_pFrom; + m_pFrom = m_pTo; + m_pTo = pd; + m_dt = 0.0; + + // } + } + } + m_angle=angle; + * + * ------------------------------------------------------------------------------------ + */ - /* - * ------------------------------------------------------------------------------------ - * V0.0 - m_dt += dt; - if(m_dt >= m_deltaTimeThreshold) + /* + * ------------------------------------------------------------------------------------ + * V0.1 + */ + // Dans cette nouvelle version, angle_rate est supposé être la valeur brute de Gyro rate le tiltTracker incluant cette moyenne ! + m_dv = angle_rate - m_filteredRateAverage.m_mean; + double filtered_rate; + bool btilt = false; + // ci-dessous, ... pour eviter la racine carrée du "vrai test" à effectuer à partir de l'écart type: if(NABS(m_dv) > m_angleThreshold*sqrt(m_rateAverage.m_variance) ) + // on utilise la variance ( = ecart type au carré ), threshold devra donc être ajusté en conséquence ( élevé au carré pour conserver les mêmes résultats qu'avec la standard deviation) + // if ((m_dv * m_dv) > (m_threshold * m_filteredRateAverage.m_variance)) + if (abs(m_dv) > m_filteredRateAverage.getStdDeviation()) { - std::cout<<"(m_dt >= m_deltaTimeThreshold)"<m_angleThreshold) + // à partir d'ici nous sommes sûrs que : m_dv != 0.0 + if (m_dv > 0.0) // angle_rate > m_rateAverage.m_mean { - std::cout<<"(abs(dif)>m_angleThreshold"< m_angleThreshold) + m_tilted = 1; + std::cout << "m_dv" << m_dv << std::endl; + // pic positif + // if (m_peakWidth < 0.0) + // { + // m_peakWidth = 0.0; + // std::cout << "m_peakWidth" << m_peakWidth << std::endl; + // } + // m_peakWidth += dt; + // if (m_peakWidth > m_deltaTimeThreshold) + // { + // std::cout << "m_peakWidth > m_deltaTimeThreshold" << m_peakWidth << m_deltaTimeThreshold << std::endl; + // if (m_tilted != 1) + // { + // r = 1; + // std::cout << "mtilted" << m_tilted << std::endl; + // m_tilted = 1; + // btilt = true; + // } + // } + } + else // angle_rate < m_rateAverage.m_mean + { + m_tilted = -1; + + std::cout << "m_dv" << m_dv << std::endl; + std::cout << "m_peakWidth" << m_peakWidth << std::endl; + + // pic negatif + // if (m_peakWidth > 0.0) // { - r=true; - *m_pTo = position; - double *pd = m_pFrom; - m_pFrom = m_pTo; - m_pTo = pd; - m_dt = 0.0; + // m_peakWidth = 0.0; + // std::cout << "m_peakWidth" << m_peakWidth << std::endl; + // } + + // m_peakWidth -= dt; + // if (m_peakWidth < -m_deltaTimeThreshold) + // { + // std::cout << "m_peakWidth < m_deltaTimeThreshold" << -m_deltaTimeThreshold << std::endl; + // std::cout << "m_tilted" << m_tilted << std::endl; + // if (m_tilted != -1) + // { + // r = 1; + // m_tilted = -1; + // btilt = true; + // std::cout << "mtilted" << m_tilted << std::endl; + // } // } } + filtered_rate = m_peakInfluence * angle_rate + (1.0 - m_peakInfluence) * m_previousFilteredRate; } - m_angle=angle; -* -* ------------------------------------------------------------------------------------ -*/ - -/* - * ------------------------------------------------------------------------------------ - * V0.1 -*/ -// Dans cette nouvelle version, angle_rate est supposé être la valeur brute de Gyro rate le tiltTracker incluant cette moyenne ! -double dv = angle_rate - m_filteredRateAverage.m_mean; -double filtered_rate; -bool btilt = false; -// ci-dessous, ... pour eviter la racine carrée du "vrai test" à effectuer à partir de l'écart type: if(NABS(dv) > m_angleThreshold*sqrt(m_rateAverage.m_variance) ) -// on utilise la variance ( = ecart type au carré ), threshold devra donc être ajusté en conséquence ( élevé au carré pour conserver les mêmes résultats qu'avec la standard deviation) -if((dv*dv) > m_threshold*m_filteredRateAverage.m_variance) -{ - // à partir d'ici nous sommes sûrs que : dv != 0.0 - - if(dv > 0.0) // angle_rate > m_rateAverage.m_mean + else { - // pic positif - if(m_peakWidth<0.0) - m_peakWidth = 0.0; - - m_peakWidth += dt; - if(m_peakWidth > m_deltaTimeThreshold) - { - if(m_tilted<=0) - { - m_tilted = 1; - btilt = true; - } - } + filtered_rate = angle_rate; + m_tilted = 0; } - else // angle_rate < m_rateAverage.m_mean + m_filteredRateAverage.addSample(filtered_rate); + m_previousFilteredRate = filtered_rate; + + // tilt ? + if (btilt) // btilt !=0 { - // pic negatif - if(m_peakWidth>0.0) - m_peakWidth = 0.0; - - m_peakWidth = -dt; - if(m_peakWidth < -m_deltaTimeThreshold) - { - if(m_tilted>=0) - { - m_tilted = -1; - btilt = true; - } - } + *m_pTo = position; + double *pd = m_pFrom; + m_pFrom = m_pTo; + m_pTo = pd; } - - filtered_rate = m_peakInfluence*angle_rate + (1.0-m_peakInfluence)*m_previousFilteredRate; -} -else -{ - filtered_rate = angle_rate; -} -m_filteredRateAverage.addSample(filtered_rate); + //-----------------------parabolic--------------------------- + // double nrmx = (position - *m_pFrom) / (*m_pTo - *m_pFrom); + // nrmx = NCLAMP(0,nrmx,1.0) - 0.5; + // m_k = m_aParabolic*nrmx*nrmx + m_bParabolic; -// tilt ? -if(tilt != 0) -{ - *m_pTo = position; - double *pd = m_pFrom; - m_pFrom = m_pTo; - m_pTo = pd; -} -//-----------------------parabolic--------------------------- -// double nrmx = (position - *m_pFrom) / (*m_pTo - *m_pFrom); -// nrmx = NCLAMP(0,nrmx,1.0) - 0.5; -// m_k = m_aParabolic*nrmx*nrmx + m_bParabolic; - -//----------------------------cos----------------------------- - double nrmx = (position - *m_pFrom) / (*m_pTo - *m_pFrom); - nrmx = NCLAMP(0,nrmx,1.0) *NF64_PI; - nrmx=(cos(nrmx)+1.0)/2.0; - m_k = m_bParabolic + nrmx*(1-m_bParabolic); // m_bparabole = m_cosinus min - -//----------------------------cos2pi-------------------------- -// double nrmx = (position - *m_pFrom) / (*m_pTo - *m_pFrom); -// nrmx = NCLAMP(0,nrmx,1.0) *NF64_2PI; -// nrmx=(cos(nrmx)+1.0)/2.0; -// m_k = m_bParabolic + nrmx*(1-m_bParabolic); // m_bparabole = m_cosinus min - - return r; + //----------------------------cos----------------------------- + double nrmx = (position - *m_pFrom) / (*m_pTo - *m_pFrom); + nrmx = NCLAMP(0, nrmx, 1.0) * NF64_PI; + nrmx = (cos(nrmx) + 1.0) / 2.0; + m_k = m_bParabolic + nrmx * (1 - m_bParabolic); // m_bparabole = m_cosinus min + + //----------------------------cos2pi-------------------------- + // double nrmx = (position - *m_pFrom) / (*m_pTo - *m_pFrom); + // nrmx = NCLAMP(0,nrmx,1.0) *NF64_2PI; + // nrmx=(cos(nrmx)+1.0)/2.0; + // m_k = m_bParabolic + nrmx*(1-m_bParabolic); // m_bparabole = m_cosinus min + + return r; } double TiltTracker::getNormalizeDistance() diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index e2badf7..c9b129e 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -7,36 +7,38 @@ double Robot::Calcul_De_Notre_Brave_JM(double forward, double turn, bool wheelSide) // calcule la vitesse des roues { - double m_forward = forward; - double m_turn = turn; + double m_forward = forward; + double m_turn = turn; - double left_wheel = m_forward + m_turn * SIGMA; - double right_wheel = m_forward - m_turn * SIGMA; + double left_wheel = m_forward + m_turn * SIGMA; + double right_wheel = m_forward - m_turn * SIGMA; - double k; - k = 1.0 / (NMAX(1, NMAX(NABS(left_wheel), NABS(right_wheel)))); - left_wheel *= k; - right_wheel *= k; + double k; + k = 1.0 / (NMAX(1, NMAX(NABS(left_wheel), NABS(right_wheel)))); + left_wheel *= k; + right_wheel *= k; - if (wheelSide == 0) - return right_wheel; - else - return left_wheel; + if (wheelSide == 0) + return right_wheel; + else + return left_wheel; } -void Robot::RobotInit() { - m_gyro.Reset(); +void Robot::RobotInit() +{ + m_gyro.Reset(); m_gyro.Calibrate(); m_kPmin = 0.0075; - m_kPmax = 0.02; + m_kPmax = 0.02; } void Robot::RobotPeriodic() {} void Robot::AutonomousInit() {} void Robot::AutonomousPeriodic() {} -void Robot::TeleopInit() { +void Robot::TeleopInit() +{ frc::SmartDashboard::PutNumber("P", 0.016); frc::SmartDashboard::PutNumber("I", 0.0); frc::SmartDashboard::PutNumber("D", 0.0); @@ -56,8 +58,6 @@ void Robot::TeleopInit() { m_MotorRightFollower.Follow(m_MotorRight); m_MotorRightFollower2.Follow(m_MotorRight); - - m_MotorLeft.SetNeutralMode(ctre::phoenix::motorcontrol::NeutralMode::Brake); m_MotorLeftFollower.SetNeutralMode(ctre::phoenix::motorcontrol::NeutralMode::Brake); m_MotorLeftFollower2.SetNeutralMode(ctre::phoenix::motorcontrol::NeutralMode::Brake); @@ -66,100 +66,104 @@ void Robot::TeleopInit() { m_MotorRightFollower.SetNeutralMode(ctre::phoenix::motorcontrol::NeutralMode::Brake); m_MotorRightFollower2.SetNeutralMode(ctre::phoenix::motorcontrol::NeutralMode::Brake); - m_state=State::End; - m_EncoderLeft.SetDistancePerPulse(1.0/2048.0); - m_EncoderRight.SetDistancePerPulse(1.0/2048.0); + m_state = State::End; + m_EncoderLeft.SetDistancePerPulse(1.0 / 2048.0); + m_EncoderRight.SetDistancePerPulse(1.0 / 2048.0); m_logCSV.open("/home/lvuser/", true); - m_logCSV.setItem(0, "m_FusAngle",5, &m_LogFusAngle); - m_logCSV.setItem(1, "m_gyroRateAverage",5, &m_LogGyroRateAverage); - m_logCSV.setItem(2, "m_gyrorate",5, &m_LogGyroRate); - m_logCSV.setItem(3, "m_AngleController.m_error",5, &m_AngleController.m_error); - m_logCSV.setItem(4, "m_VangleController.m_error",5, &m_VangleController.m_error); - m_logCSV.setItem(5, "m_vOutput",5, &m_vOutput); - m_logCSV.setItem(6, "m_AngleOutput",5, &m_AngleOutput); - m_logCSV.setItem(7, "m_Output",5, &m_Output); - m_logCSV.setItem(8, "m_EncoderMetre",5, &m_LogEncoderM); - m_logCSV.setItem(9, "m_AccelX",5, &m_LogAccelX); - m_logCSV.setItem(10, "m_AngleAccel",5, &m_LogAngleAccel); - - + m_logCSV.setItem(0, "m_FusAngle", 5, &m_LogFusAngle); + m_logCSV.setItem(1, "m_gyroRateAverage", 5, &m_LogGyroRateAverage); + m_logCSV.setItem(2, "m_gyrorate", 5, &m_LogGyroRate); + m_logCSV.setItem(3, "m_AngleController.m_error", 5, &m_AngleController.m_error); + m_logCSV.setItem(4, "m_VangleController.m_error", 5, &m_VangleController.m_error); + m_logCSV.setItem(5, "m_vOutput", 5, &m_vOutput); + m_logCSV.setItem(6, "m_AngleOutput", 5, &m_AngleOutput); + m_logCSV.setItem(7, "m_Output", 5, &m_Output); + m_logCSV.setItem(8, "m_EncoderMetre", 5, &m_LogEncoderM); + m_logCSV.setItem(9, "m_AccelX", 5, &m_LogAccelX); + m_logCSV.setItem(10, "m_AngleAccel", 5, &m_LogAngleAccel); } -void Robot::TeleopPeriodic() { - double a =0.0; - double ratio_distance=0.0; - +void Robot::TeleopPeriodic() +{ + double a = 0.0; + double ratio_distance = 0.0; - double x_g = m_accelerometer.GetX(); // -1/1 - double y_g = m_accelerometer.GetY(); - double angle_deg = m_gyro.GetAngle(); - double gyro_rate_degps = m_gyro.GetRate(); + double x_g = m_accelerometer.GetX(); // -1/1 + double y_g = m_accelerometer.GetY(); + double angle_deg = m_gyro.GetAngle(); + double gyro_rate_degps = m_gyro.GetRate(); m_AccelerometerX.set(x_g); m_AccelerometerY.set(y_g); m_GyroAngle.set(angle_deg); - m_FusAngle.SetTau(frc::SmartDashboard::GetNumber("m_tau",0.5)); - m_FusAngle.Update(NDEGtoRAD(gyro_rate_degps),y_g,x_g); // A la place de ... m_FusAngle.Update(m_gyro.GetRate()/180.0*3.14159265,m_accelerometer.GetY(),m_accelerometer.GetX()); - m_gyroRateAverage.addSample(gyro_rate_degps); //m_gyroRateAverage.add(m_FusAngle.GetDelta()*180.0/3.14159265); + m_FusAngle.SetTau(frc::SmartDashboard::GetNumber("m_tau", 0.5)); + m_FusAngle.Update(NDEGtoRAD(gyro_rate_degps), y_g, x_g); // A la place de ... m_FusAngle.Update(m_gyro.GetRate()/180.0*3.14159265,m_accelerometer.GetY(),m_accelerometer.GetX()); + m_gyroRateAverage.addSample(gyro_rate_degps); // m_gyroRateAverage.add(m_FusAngle.GetDelta()*180.0/3.14159265); - frc::SmartDashboard::PutNumber("FuseAngle",NRADtoDEG(m_FusAngle.GetAngle())); - frc::SmartDashboard::PutNumber("GyroRate",m_gyroRateAverage.getMean()); - frc::SmartDashboard::PutNumber("anglegyro",m_gyro.GetAngle()); + frc::SmartDashboard::PutNumber("FuseAngle", NRADtoDEG(m_FusAngle.GetAngle())); + frc::SmartDashboard::PutNumber("GyroRate", m_gyroRateAverage.getMean()); + frc::SmartDashboard::PutNumber("anglegyro", m_gyro.GetAngle()); // frc::SmartDashboard::PutNumber("m_K",m_FusAngle.m_k); // frc::SmartDashboard::PutNumber("Accel_X",x); // frc::SmartDashboard::PutNumber("Accel_Y",y); // frc::SmartDashboard::PutNumber("Angle_Gyro",angle); - - - if (m_joystickRight.GetRawButton(1)) { m_FusAngle.AutoSetBias(); - // reset de la posiition de ref des encodeurs. - //m_encoderOrigin = NABS(m_EncoderLeft.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE); // a la place de ... m_encoder_origine=abs(m_EncoderLeft.GetDistance()*(3.0*0.0254)*(2.0*3.14159265)); - //m_refDistance = CHARGE_STATION_WIDTH; // a la place de ... m_distanceAparcourir = CHARGE_STATION_WIDTH; - // m_traveledDistance = 0.0; // a la place de ... m_distanceParcourue = 0.0; - // on supprime .... m_distanceRestante = m_distanceAparcourir-m_distanceParcourue; - m_AngleController.Reset(); - m_AngleController.SetSetpoint(0.0); - m_AngleController.SetTolerance(4); - // à la place de ... m_AngleController.SetSetpoint(0); - m_VangleController.Reset(); - m_VangleController.SetSetpoint(0.0); - m_VangleController.SetTolerance(1); - // m_errorSign = NSIGN(m_AngleController.m_error); // à la place de ... m_signe_error = NSIGN(m_AngleController.m_error); - m_TiltTracker.initialize(m_FusAngle.GetAngle(),(m_EncoderLeft.GetDistance()+m_EncoderRight.GetDistance())*TRACTION_WHEEL_CIRCUMFERENCE/2.0,CHARGE_STATION_WIDTH); + // reset de la posiition de ref des encodeurs. + // m_encoderOrigin = NABS(m_EncoderLeft.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE); // a la place de ... m_encoder_origine=abs(m_EncoderLeft.GetDistance()*(3.0*0.0254)*(2.0*3.14159265)); + // m_refDistance = CHARGE_STATION_WIDTH; // a la place de ... m_distanceAparcourir = CHARGE_STATION_WIDTH; + // m_traveledDistance = 0.0; // a la place de ... m_distanceParcourue = 0.0; + // on supprime .... m_distanceRestante = m_distanceAparcourir-m_distanceParcourue; + m_AngleController.Reset(); + m_AngleController.SetSetpoint(0.0); + m_AngleController.SetTolerance(4); + // à la place de ... m_AngleController.SetSetpoint(0); + m_VangleController.Reset(); + m_VangleController.SetSetpoint(0.0); + m_VangleController.SetTolerance(1); + // m_errorSign = NSIGN(m_AngleController.m_error); // à la place de ... m_signe_error = NSIGN(m_AngleController.m_error); + m_TiltTracker.initialize(m_FusAngle.GetAngle(), (m_EncoderLeft.GetDistance() + m_EncoderRight.GetDistance()) * TRACTION_WHEEL_CIRCUMFERENCE / 2.0, CHARGE_STATION_WIDTH); } - double p=frc::SmartDashboard::GetNumber("P",0.012); - double i =frc::SmartDashboard::GetNumber("I",0.0); - double d =frc::SmartDashboard::GetNumber("D",0.0); + double p = frc::SmartDashboard::GetNumber("P", 0.012); + double i = frc::SmartDashboard::GetNumber("I", 0.0); + double d = frc::SmartDashboard::GetNumber("D", 0.0); - bool tilt = m_TiltTracker.Update(0.02,(m_EncoderLeft.GetDistance()+m_EncoderRight.GetDistance())*TRACTION_WHEEL_CIRCUMFERENCE/2.0,m_FusAngle.GetAngle(),m_gyroRateAverage.getMean()); - m_AngleController.SetGains(0.03,0,0); // avant m_AngleController.SetGains(a,0.0,0.0); - m_VangleController.SetGains(0.005,i*10.0,d*10.0); + double tilt = m_TiltTracker.Update(0.02, (m_EncoderLeft.GetDistance() + m_EncoderRight.GetDistance()) * TRACTION_WHEEL_CIRCUMFERENCE / 2.0, m_FusAngle.GetAngle(), gyro_rate_degps); + m_AngleController.SetGains(0.03, 0, 0); // avant m_AngleController.SetGains(a,0.0,0.0); + m_VangleController.SetGains(0.005, i * 10.0, d * 10.0); m_AngleOutput = m_AngleController.Calculate(NRADtoDEG(m_FusAngle.GetAngle())); m_vOutput = m_VangleController.Calculate(m_gyroRateAverage.getMean()); - m_Output = m_TiltTracker.m_k*m_AngleOutput;// voutput - - frc::SmartDashboard::PutNumber("k_anticipation",m_TiltTracker.m_k); - frc::SmartDashboard::PutNumber("k*angleoutput",m_TiltTracker.m_k*m_AngleOutput); - frc::SmartDashboard::PutNumber("delta_angle",m_FusAngle.m_delta); - m_Output = NCLAMP(-0.3,m_Output,0.3); - frc::SmartDashboard::PutNumber("output",m_Output); - frc::SmartDashboard::PutNumber("a",a); - frc::SmartDashboard::PutNumber("error",m_AngleController.m_error); - frc::SmartDashboard::PutNumber("delta_error",m_VangleController.m_error); - frc::SmartDashboard::PutNumber("ratio_distance",ratio_distance); - frc::SmartDashboard::PutNumber("m_encoderRight",m_EncoderRight.GetDistance()); - frc::SmartDashboard::PutNumber("m_encoderLeft",m_EncoderLeft.GetDistance()); - frc::SmartDashboard::PutNumber("encoder",((m_EncoderLeft.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE)+(m_EncoderRight.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE))/2); - frc::SmartDashboard::PutBoolean("tilt",tilt); + m_Output = m_TiltTracker.m_k * m_AngleOutput; // voutput + + frc::SmartDashboard::PutNumber("k_anticipation", m_TiltTracker.m_k); + frc::SmartDashboard::PutNumber("k*angleoutput", m_TiltTracker.m_k * m_AngleOutput); + frc::SmartDashboard::PutNumber("delta_angle", m_FusAngle.m_delta); + m_Output = NCLAMP(-0.3, m_Output, 0.3); + frc::SmartDashboard::PutNumber("output", m_Output); + frc::SmartDashboard::PutNumber("a", a); + frc::SmartDashboard::PutNumber("error", m_AngleController.m_error); + frc::SmartDashboard::PutNumber("delta_error", m_VangleController.m_error); + frc::SmartDashboard::PutNumber("ratio_distance", ratio_distance); + frc::SmartDashboard::PutNumber("m_encoderRight", m_EncoderRight.GetDistance()); + frc::SmartDashboard::PutNumber("m_encoderLeft", m_EncoderLeft.GetDistance()); + frc::SmartDashboard::PutNumber("encoder", ((m_EncoderLeft.GetDistance() * TRACTION_WHEEL_CIRCUMFERENCE) + (m_EncoderRight.GetDistance() * TRACTION_WHEEL_CIRCUMFERENCE)) / 2); + frc::SmartDashboard::PutNumber("tilt", tilt); + frc::SmartDashboard::PutNumber("variance", m_TiltTracker.m_filteredRateAverage.m_variance); + frc::SmartDashboard::PutNumber("moyenne", m_TiltTracker.m_filteredRateAverage.m_mean); + frc::SmartDashboard::PutNumber("m_peakWidth", m_TiltTracker.m_peakWidth); + frc::SmartDashboard::PutNumber("m_tilted", m_TiltTracker.m_tilted * 25); + frc::SmartDashboard::PutNumber("m_dv", m_TiltTracker.m_dv); + double up = m_TiltTracker.m_filteredRateAverage.m_mean + m_TiltTracker.m_filteredRateAverage.getStdDeviation(); + double down = m_TiltTracker.m_filteredRateAverage.m_mean - m_TiltTracker.m_filteredRateAverage.getStdDeviation(); + frc::SmartDashboard::PutNumber("up", up); + frc::SmartDashboard::PutNumber("down", down); if (m_joystickLeft.GetRawButton(1)) { - + m_MotorRight.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, m_Output); m_MotorLeft.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, m_Output); } @@ -167,7 +171,6 @@ void Robot::TeleopPeriodic() { { m_MotorRight.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, Calcul_De_Notre_Brave_JM(-m_joystickLeft.GetY(), m_joystickRight.GetZ(), 0)); m_MotorLeft.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, Calcul_De_Notre_Brave_JM(-m_joystickLeft.GetY(), m_joystickRight.GetZ(), 1)); - } frc::SmartDashboard::PutNumber("m_Output", m_Output); @@ -176,20 +179,15 @@ void Robot::TeleopPeriodic() { m_LogFusAngle = m_FusAngle.GetAngle(); m_LogGyroRateAverage = m_gyroRateAverage.getMean(); m_LogGyroRate = m_gyro.GetRate(); - m_LogEncoderM = ((m_EncoderLeft.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE)+(m_EncoderRight.GetDistance()*TRACTION_WHEEL_CIRCUMFERENCE))/2; + m_LogEncoderM = ((m_EncoderLeft.GetDistance() * TRACTION_WHEEL_CIRCUMFERENCE) + (m_EncoderRight.GetDistance() * TRACTION_WHEEL_CIRCUMFERENCE)) / 2; m_LogAngleAccel = m_FusAngle.m_angleAccel; m_LogAccelX = m_accelerometer.GetX(); - - - m_logCSV.write(); - - } - -void Robot::DisabledInit() { +void Robot::DisabledInit() +{ m_gyro.Reset(); m_logCSV.close(); } @@ -198,9 +196,9 @@ void Robot::DisabledPeriodic() {} void Robot::TestInit() {} void Robot::TestPeriodic() {} - #ifndef RUNNING_FRC_TESTS -int main() { +int main() +{ return frc::StartRobot(); } #endif diff --git a/src/main/cpp/lib/NMovingAverage.cpp b/src/main/cpp/lib/NMovingAverage.cpp index bbcd644..10b1091 100644 --- a/src/main/cpp/lib/NMovingAverage.cpp +++ b/src/main/cpp/lib/NMovingAverage.cpp @@ -2,131 +2,130 @@ NdoubleMovingAverage::NdoubleMovingAverage() { - m_last = 0; - m_pSamples = NULL; - m_sum = 0.0; - m_sum2 = 0.0; - m_mean = 0.0; - m_variance = 0.0; - m_index = 0; + m_last = 0; + m_pSamples = NULL; + m_sum = 0.0; + m_sum2 = 0.0; + m_mean = 0.0; + m_variance = 0.0; + m_index = 0; } NdoubleMovingAverage::NdoubleMovingAverage(const int table_size, const double initial_average) { - m_last = table_size - 1; - m_pSamples = (double*)malloc(sizeof(double)*table_size); - m_sum = initial_average*table_size; - m_sum2 = m_sum*initial_average; - m_mean = initial_average; - m_variance = 0.0; - m_index = 0; + m_last = table_size - 1; + m_pSamples = (double *)malloc(sizeof(double) * table_size); + m_sum = initial_average * table_size; + m_sum2 = m_sum * initial_average; + m_mean = initial_average; + m_variance = 0.0; + m_index = 0; double *pd = m_pSamples; - for(int i= 0;i 2 { - if(m_last != (table_size - 1)) + if (m_last != (table_size - 1)) { - m_last = table_size - 1; - m_pSamples = (double*)realloc(m_pSamples, sizeof(double)*table_size); - m_index = 0; + m_last = table_size - 1; + m_pSamples = (double *)realloc(m_pSamples, sizeof(double) * table_size); + m_index = 0; } } - m_sum = initial_average*(m_last+1); - m_sum2 = m_sum*initial_average; - m_mean = initial_average; - m_variance = 0.0; - m_pSamples -= m_index; // replace le pointeur en position initiale - m_index = 0; + m_sum = initial_average * (m_last + 1); + m_sum2 = m_sum * initial_average; + m_mean = initial_average; + m_variance = 0.0; + m_pSamples -= m_index; // replace le pointeur en position initiale + m_index = 0; double *pd = m_pSamples; - for(int i= 0;i<=m_last;i++,pd++) + for (int i = 0; i <= m_last; i++, pd++) *pd = initial_average; } -// - - +// NfloatRollingAverage::NfloatRollingAverage(const unsigned short table_size, const float initial_average) { - m_last = table_size - 1; - m_pfloat = (float*)malloc(sizeof(float)*table_size); - m_sum = initial_average*table_size; - m_average = initial_average; - m_index = 0; + m_last = table_size - 1; + m_pfloat = (float *)malloc(sizeof(float) * table_size); + m_sum = initial_average * table_size; + m_average = initial_average; + m_index = 0; float *pf = m_pfloat; - for(unsigned short i= 0;i @@ -42,10 +41,11 @@ #include #include -//gyro +// gyro -class Robot : public frc::TimedRobot { - public: +class Robot : public frc::TimedRobot +{ +public: void RobotInit() override; void RobotPeriodic() override; @@ -61,13 +61,12 @@ class Robot : public frc::TimedRobot { void TestInit() override; void TestPeriodic() override; - // double signe(double x); double Calcul_De_Notre_Brave_JM(double forward, double turn, bool wheelSide); double GetAngle(); - //machine à état + // machine à état enum State { Init, @@ -75,10 +74,9 @@ class Robot : public frc::TimedRobot { End }; - State m_state=State::End; - - private: + State m_state = State::End; +private: frc::Joystick m_joystickRight{0}; frc::Joystick m_joystickLeft{1}; frc::BuiltInAccelerometer m_accelerometer{}; @@ -92,7 +90,6 @@ class Robot : public frc::TimedRobot { ctre::phoenix::motorcontrol::can::TalonFX m_MotorLeftFollower{5}; ctre::phoenix::motorcontrol::can::TalonFX m_MotorLeftFollower2{6}; - NdoubleMovingAverage m_AccelerometerX_Average{25}; NdoubleMovingAverage m_AccelerometerX_Arcos_Average{25}; NdoubleMovingAverage m_gyroRateAverage{25}; @@ -103,29 +100,28 @@ class Robot : public frc::TimedRobot { Dynamic m_AccelerometerY; Dynamic m_GyroAngle; - double m_Sum_Delta_Gyro_Angle=0.0; + double m_Sum_Delta_Gyro_Angle = 0.0; - frc::Encoder m_EncoderRight{0,1,true}; - frc::Encoder m_EncoderLeft{2,3,false}; + frc::Encoder m_EncoderRight{0, 1, true}; + frc::Encoder m_EncoderLeft{2, 3, false}; - Angle_AG m_FusAngle{0.02,0.075}; - Pid m_VangleController{0.0,0.01,0.0005,0.0}; - Pid m_AngleController{0.0,0.01,0.0005,0.0}; + Angle_AG m_FusAngle{0.02, 0.075}; + Pid m_VangleController{0.0, 0.01, 0.0005, 0.0}; + Pid m_AngleController{0.0, 0.01, 0.0005, 0.0}; - TiltTracker m_TiltTracker{5,0.05,1.0,0.2};//{80.0,0.5,0.4}; ça marche + TiltTracker m_TiltTracker{5, 100.0, 0.06, 0.0001, 0.2}; //{80.0,0.5,0.4}; ça marche double m_kPmin; double m_kPmax; - double m_selfConfidenceMin; - double m_selfConfidenceMax; + double m_selfConfidenceMin; + double m_selfConfidenceMax; - double m_traveledDistance; - double m_refDistance; - + double m_traveledDistance; + double m_refDistance; - double m_errorSign; - double m_encoderOrigin; + double m_errorSign; + double m_encoderOrigin; double m_vOutput; double m_Output; @@ -137,6 +133,5 @@ class Robot : public frc::TimedRobot { double m_LogEncoderM; double m_LogAccelX; - - NLCSV m_logCSV{11}; + NLCSV m_logCSV{11}; }; diff --git a/src/main/include/lib/NMovingAverage.h b/src/main/include/lib/NMovingAverage.h index d1bcf0e..348bad2 100644 --- a/src/main/include/lib/NMovingAverage.h +++ b/src/main/include/lib/NMovingAverage.h @@ -18,7 +18,7 @@ class NdoubleMovingAverage NdoubleMovingAverage(const int table_size, const double initial_average = 0.0 ); ~NdoubleMovingAverage(); - void reset(const int table_size = 0, const double initial_average = 0.0 ); + void reset(const int table_size, const double initial_average ); const double addSample(const double value); inline const double getMean(){return m_mean;} inline const double getVariance(){return m_variance;} From 7d9f85b6e998dc98a8a80f1ccc247a9dda485dbe Mon Sep 17 00:00:00 2001 From: Nathan Mutin <32647745+nathanmutin@users.noreply.github.com> Date: Wed, 15 Feb 2023 16:46:50 +0100 Subject: [PATCH 33/36] =?UTF-8?q?bug=20corrig=C3=A9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/cpp/ChargeStationTiltTracker.cpp | 2 +- src/main/cpp/lib/NMovingAverage.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/cpp/ChargeStationTiltTracker.cpp b/src/main/cpp/ChargeStationTiltTracker.cpp index cd36f1e..2600ce3 100644 --- a/src/main/cpp/ChargeStationTiltTracker.cpp +++ b/src/main/cpp/ChargeStationTiltTracker.cpp @@ -103,7 +103,7 @@ double TiltTracker::Update(const double dt, const double position, const double // ci-dessous, ... pour eviter la racine carrée du "vrai test" à effectuer à partir de l'écart type: if(NABS(m_dv) > m_angleThreshold*sqrt(m_rateAverage.m_variance) ) // on utilise la variance ( = ecart type au carré ), threshold devra donc être ajusté en conséquence ( élevé au carré pour conserver les mêmes résultats qu'avec la standard deviation) // if ((m_dv * m_dv) > (m_threshold * m_filteredRateAverage.m_variance)) - if (abs(m_dv) > m_filteredRateAverage.getStdDeviation()) + if (abs(m_dv) > m_threshold * m_filteredRateAverage.getStdDeviation()) { // à partir d'ici nous sommes sûrs que : m_dv != 0.0 if (m_dv > 0.0) // angle_rate > m_rateAverage.m_mean diff --git a/src/main/cpp/lib/NMovingAverage.cpp b/src/main/cpp/lib/NMovingAverage.cpp index 10b1091..2177028 100644 --- a/src/main/cpp/lib/NMovingAverage.cpp +++ b/src/main/cpp/lib/NMovingAverage.cpp @@ -37,8 +37,8 @@ const double NdoubleMovingAverage::addSample(const double value) m_sum += (value - *m_pSamples); m_sum2 += (value * value - (*m_pSamples) * (*m_pSamples)); m_mean = m_sum / (m_last + 1); - // m_variance = (m_sum2 - (m_sum * m_sum) / (m_last + 1)) / (m_last + 1); - m_variance = (m_sum2 / (m_last + 1) - (m_mean * m_mean)); + m_variance = (m_sum2 - (m_sum * m_sum) / (m_last + 1)) / (m_last); + //m_variance = (m_sum2 / (m_last + 1) - (m_mean * m_mean)); *m_pSamples = value; if (m_index == m_last) From c15a229eac735cebddf927dd7759e047349a1ede Mon Sep 17 00:00:00 2001 From: mimi980 <90112270+mimi980@users.noreply.github.com> Date: Wed, 15 Feb 2023 23:07:53 +0100 Subject: [PATCH 34/36] ADD --- src/main/cpp/Robot.cpp | 48 ++++++++++++++++++++++------------------ src/main/include/Robot.h | 7 +++++- 2 files changed, 33 insertions(+), 22 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index c9b129e..77587b8 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -74,11 +74,11 @@ void Robot::TeleopInit() m_logCSV.setItem(0, "m_FusAngle", 5, &m_LogFusAngle); m_logCSV.setItem(1, "m_gyroRateAverage", 5, &m_LogGyroRateAverage); m_logCSV.setItem(2, "m_gyrorate", 5, &m_LogGyroRate); - m_logCSV.setItem(3, "m_AngleController.m_error", 5, &m_AngleController.m_error); - m_logCSV.setItem(4, "m_VangleController.m_error", 5, &m_VangleController.m_error); - m_logCSV.setItem(5, "m_vOutput", 5, &m_vOutput); - m_logCSV.setItem(6, "m_AngleOutput", 5, &m_AngleOutput); - m_logCSV.setItem(7, "m_Output", 5, &m_Output); + m_logCSV.setItem(3, "m_logTiltAverage", 5, &m_logTiltAverage); + m_logCSV.setItem(4, "m_logTiltVariance", 5, &m_logTiltVariance); + m_logCSV.setItem(5, "m_logTilt", 5, &m_logTilt); + m_logCSV.setItem(6, "m_logTiltDown", 5, &m_logTiltDown); + m_logCSV.setItem(7, "m_logTiltUp", 5, &m_logTiltUp); m_logCSV.setItem(8, "m_EncoderMetre", 5, &m_LogEncoderM); m_logCSV.setItem(9, "m_AccelX", 5, &m_LogAccelX); m_logCSV.setItem(10, "m_AngleAccel", 5, &m_LogAngleAccel); @@ -103,7 +103,7 @@ void Robot::TeleopPeriodic() frc::SmartDashboard::PutNumber("FuseAngle", NRADtoDEG(m_FusAngle.GetAngle())); frc::SmartDashboard::PutNumber("GyroRate", m_gyroRateAverage.getMean()); - frc::SmartDashboard::PutNumber("anglegyro", m_gyro.GetAngle()); + frc::SmartDashboard::PutNumber("anglegyro", angle_deg); // frc::SmartDashboard::PutNumber("m_K",m_FusAngle.m_k); // frc::SmartDashboard::PutNumber("Accel_X",x); // frc::SmartDashboard::PutNumber("Accel_Y",y); @@ -138,28 +138,29 @@ void Robot::TeleopPeriodic() m_vOutput = m_VangleController.Calculate(m_gyroRateAverage.getMean()); m_Output = m_TiltTracker.m_k * m_AngleOutput; // voutput - frc::SmartDashboard::PutNumber("k_anticipation", m_TiltTracker.m_k); - frc::SmartDashboard::PutNumber("k*angleoutput", m_TiltTracker.m_k * m_AngleOutput); - frc::SmartDashboard::PutNumber("delta_angle", m_FusAngle.m_delta); + // frc::SmartDashboard::PutNumber("k_anticipation", m_TiltTracker.m_k); + // frc::SmartDashboard::PutNumber("k*angleoutput", m_TiltTracker.m_k * m_AngleOutput); + // frc::SmartDashboard::PutNumber("delta_angle", m_FusAngle.m_delta); m_Output = NCLAMP(-0.3, m_Output, 0.3); - frc::SmartDashboard::PutNumber("output", m_Output); - frc::SmartDashboard::PutNumber("a", a); - frc::SmartDashboard::PutNumber("error", m_AngleController.m_error); - frc::SmartDashboard::PutNumber("delta_error", m_VangleController.m_error); - frc::SmartDashboard::PutNumber("ratio_distance", ratio_distance); - frc::SmartDashboard::PutNumber("m_encoderRight", m_EncoderRight.GetDistance()); - frc::SmartDashboard::PutNumber("m_encoderLeft", m_EncoderLeft.GetDistance()); - frc::SmartDashboard::PutNumber("encoder", ((m_EncoderLeft.GetDistance() * TRACTION_WHEEL_CIRCUMFERENCE) + (m_EncoderRight.GetDistance() * TRACTION_WHEEL_CIRCUMFERENCE)) / 2); + // frc::SmartDashboard::PutNumber("output", m_Output); + // frc::SmartDashboard::PutNumber("a", a); + // frc::SmartDashboard::PutNumber("error", m_AngleController.m_error); + // frc::SmartDashboard::PutNumber("delta_error", m_VangleController.m_error); + // frc::SmartDashboard::PutNumber("ratio_distance", ratio_distance); + // frc::SmartDashboard::PutNumber("m_encoderRight", m_EncoderRight.GetDistance()); + // frc::SmartDashboard::PutNumber("m_encoderLeft", m_EncoderLeft.GetDistance()); + // frc::SmartDashboard::PutNumber("encoder", ((m_EncoderLeft.GetDistance() * TRACTION_WHEEL_CIRCUMFERENCE) + (m_EncoderRight.GetDistance() * TRACTION_WHEEL_CIRCUMFERENCE)) / 2); frc::SmartDashboard::PutNumber("tilt", tilt); frc::SmartDashboard::PutNumber("variance", m_TiltTracker.m_filteredRateAverage.m_variance); frc::SmartDashboard::PutNumber("moyenne", m_TiltTracker.m_filteredRateAverage.m_mean); frc::SmartDashboard::PutNumber("m_peakWidth", m_TiltTracker.m_peakWidth); frc::SmartDashboard::PutNumber("m_tilted", m_TiltTracker.m_tilted * 25); - frc::SmartDashboard::PutNumber("m_dv", m_TiltTracker.m_dv); + frc::SmartDashboard::PutNumber("m_dv", abs(m_TiltTracker.m_dv)); double up = m_TiltTracker.m_filteredRateAverage.m_mean + m_TiltTracker.m_filteredRateAverage.getStdDeviation(); double down = m_TiltTracker.m_filteredRateAverage.m_mean - m_TiltTracker.m_filteredRateAverage.getStdDeviation(); frc::SmartDashboard::PutNumber("up", up); frc::SmartDashboard::PutNumber("down", down); + frc::SmartDashboard::PutNumber("deviation*coef", m_TiltTracker.m_threshold * m_TiltTracker.m_filteredRateAverage.getStdDeviation()); if (m_joystickLeft.GetRawButton(1)) { @@ -173,15 +174,20 @@ void Robot::TeleopPeriodic() m_MotorLeft.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, Calcul_De_Notre_Brave_JM(-m_joystickLeft.GetY(), m_joystickRight.GetZ(), 1)); } - frc::SmartDashboard::PutNumber("m_Output", m_Output); - frc::SmartDashboard::PutNumber("m_vOutput", m_vOutput); - frc::SmartDashboard::PutNumber("m_AngleOutput", m_AngleOutput); + // frc::SmartDashboard::PutNumber("m_Output", m_Output); + // frc::SmartDashboard::PutNumber("m_vOutput", m_vOutput); + // frc::SmartDashboard::PutNumber("m_AngleOutput", m_AngleOutput); m_LogFusAngle = m_FusAngle.GetAngle(); m_LogGyroRateAverage = m_gyroRateAverage.getMean(); m_LogGyroRate = m_gyro.GetRate(); m_LogEncoderM = ((m_EncoderLeft.GetDistance() * TRACTION_WHEEL_CIRCUMFERENCE) + (m_EncoderRight.GetDistance() * TRACTION_WHEEL_CIRCUMFERENCE)) / 2; m_LogAngleAccel = m_FusAngle.m_angleAccel; m_LogAccelX = m_accelerometer.GetX(); + m_logTiltAverage = m_TiltTracker.m_filteredRateAverage.m_mean; + m_logTiltVariance = m_TiltTracker.m_filteredRateAverage.m_variance; + m_logTilt = m_TiltTracker.m_tilted; + m_logTiltUp = up; + m_logTiltDown = down; m_logCSV.write(); } diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index d709d20..56366a4 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -109,7 +109,7 @@ class Robot : public frc::TimedRobot Pid m_VangleController{0.0, 0.01, 0.0005, 0.0}; Pid m_AngleController{0.0, 0.01, 0.0005, 0.0}; - TiltTracker m_TiltTracker{5, 100.0, 0.06, 0.0001, 0.2}; //{80.0,0.5,0.4}; ça marche + TiltTracker m_TiltTracker{25, 5.5, 0.06, 0.01, 0.2}; //{80.0,0.5,0.4}; ça marche double m_kPmin; double m_kPmax; @@ -132,6 +132,11 @@ class Robot : public frc::TimedRobot double m_LogAngleAccel; double m_LogEncoderM; double m_LogAccelX; + double m_logTiltAverage; + double m_logTiltVariance; + double m_logTilt; + double m_logTiltUp; + double m_logTiltDown; NLCSV m_logCSV{11}; }; From 33828a6a9f9a8bcf2302cead6aa1e987c433e4b2 Mon Sep 17 00:00:00 2001 From: jjTerazan <47304610+jjTerazan@users.noreply.github.com> Date: Thu, 16 Feb 2023 08:46:36 +0100 Subject: [PATCH 35/36] +3xrien+ --- src/main/cpp/lib/NMovingAverage.cpp | 6 +++--- src/main/include/lib/NMovingAverage.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/cpp/lib/NMovingAverage.cpp b/src/main/cpp/lib/NMovingAverage.cpp index 2177028..9dd6c49 100644 --- a/src/main/cpp/lib/NMovingAverage.cpp +++ b/src/main/cpp/lib/NMovingAverage.cpp @@ -35,9 +35,9 @@ NdoubleMovingAverage::~NdoubleMovingAverage() const double NdoubleMovingAverage::addSample(const double value) { m_sum += (value - *m_pSamples); - m_sum2 += (value * value - (*m_pSamples) * (*m_pSamples)); + m_sum2 += ((value * value) - (*m_pSamples) * (*m_pSamples)); m_mean = m_sum / (m_last + 1); - m_variance = (m_sum2 - (m_sum * m_sum) / (m_last + 1)) / (m_last); + m_variance = (m_sum2 - ((m_sum * m_sum) / (m_last + 1))) / (m_last); //m_variance = (m_sum2 / (m_last + 1) - (m_mean * m_mean)); *m_pSamples = value; @@ -55,7 +55,7 @@ const double NdoubleMovingAverage::addSample(const double value) return m_mean; } -void NdoubleMovingAverage::reset(const int table_size = 0, const double initial_average = 0.0) +void NdoubleMovingAverage::reset(const int table_size, const double initial_average) { if (table_size < 2) { diff --git a/src/main/include/lib/NMovingAverage.h b/src/main/include/lib/NMovingAverage.h index 348bad2..e94b5f7 100644 --- a/src/main/include/lib/NMovingAverage.h +++ b/src/main/include/lib/NMovingAverage.h @@ -18,7 +18,7 @@ class NdoubleMovingAverage NdoubleMovingAverage(const int table_size, const double initial_average = 0.0 ); ~NdoubleMovingAverage(); - void reset(const int table_size, const double initial_average ); + void reset(const int table_size = 0, const double initial_average = 0.0); const double addSample(const double value); inline const double getMean(){return m_mean;} inline const double getVariance(){return m_variance;} From 94f76f0f1e209ea6782fad3c45194c4443fa8430 Mon Sep 17 00:00:00 2001 From: jjTerazan <47304610+jjTerazan@users.noreply.github.com> Date: Thu, 16 Feb 2023 08:53:46 +0100 Subject: [PATCH 36/36] +oups1bugen moins dans TiltTracker+ --- src/main/cpp/ChargeStationTiltTracker.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/cpp/ChargeStationTiltTracker.cpp b/src/main/cpp/ChargeStationTiltTracker.cpp index 2600ce3..f0f1168 100644 --- a/src/main/cpp/ChargeStationTiltTracker.cpp +++ b/src/main/cpp/ChargeStationTiltTracker.cpp @@ -103,10 +103,10 @@ double TiltTracker::Update(const double dt, const double position, const double // ci-dessous, ... pour eviter la racine carrée du "vrai test" à effectuer à partir de l'écart type: if(NABS(m_dv) > m_angleThreshold*sqrt(m_rateAverage.m_variance) ) // on utilise la variance ( = ecart type au carré ), threshold devra donc être ajusté en conséquence ( élevé au carré pour conserver les mêmes résultats qu'avec la standard deviation) // if ((m_dv * m_dv) > (m_threshold * m_filteredRateAverage.m_variance)) - if (abs(m_dv) > m_threshold * m_filteredRateAverage.getStdDeviation()) + if (NABS(m_dv) > (m_threshold * m_filteredRateAverage.getStdDeviation())) { // à partir d'ici nous sommes sûrs que : m_dv != 0.0 - if (m_dv > 0.0) // angle_rate > m_rateAverage.m_mean + if (angle_rate > m_filteredRateAverage.m_mean) { m_tilted = 1; std::cout << "m_dv" << m_dv << std::endl;