From 70ea13191faf1be6d6f32811ed4807d7ce4bd3ee Mon Sep 17 00:00:00 2001 From: Ibrahim Date: Mon, 28 Aug 2023 21:54:48 +0500 Subject: [PATCH] dead reckoning added #17 --- include/EncoderControl.h | 12 +--- include/dead_Reckoning.h | 26 ++++++++ src/EncoderControl.cpp | 129 ++++----------------------------------- src/dead_Reckoning.cpp | 56 +++++++++++++++++ src/main.cpp | 26 +++----- 5 files changed, 103 insertions(+), 146 deletions(-) create mode 100644 include/dead_Reckoning.h create mode 100644 src/dead_Reckoning.cpp diff --git a/include/EncoderControl.h b/include/EncoderControl.h index 0cb1d48..6d885ce 100644 --- a/include/EncoderControl.h +++ b/include/EncoderControl.h @@ -3,18 +3,10 @@ #include -extern const float WHEEL_DIAMETER; // Declare WHEEL_DIAMETER as an external variable - void setupEncoder0(); void setupEncoder1(); -void readEncoderAngle0(); -void readEncoderAngle1(); -void getEncoderSpeed0(); -void getEncoderSpeed1(); -int getEncoderRevolutions0(); -int getEncoderRevolutions1(); -int resolution_0(); -int resolution_1(); +int cummulativePos0(); +int cummulativePos1(); #endif // ENCODER_CONTROL_H diff --git a/include/dead_Reckoning.h b/include/dead_Reckoning.h new file mode 100644 index 0000000..98d4f90 --- /dev/null +++ b/include/dead_Reckoning.h @@ -0,0 +1,26 @@ +#ifndef DEAD_RECKONING_H +#define DEAD_RECKONING_H + +#include + +extern float disp_r_wheel; +extern float disp_l_wheel; +extern int count_R_prev; +extern int count_L_prev; +extern float x; +extern float y; +extern float theta; +extern float meter_per_ticks; +extern float orientation_angle; +extern float disp_body; + +extern int count_R; +extern int count_L; +extern const int channel_R; +extern const int channel_L; +extern int Right_motor_speed; +extern int Left_motor_speed; + +void calculate_traveling(); + +#endif diff --git a/src/EncoderControl.cpp b/src/EncoderControl.cpp index ece1dab..495d40c 100644 --- a/src/EncoderControl.cpp +++ b/src/EncoderControl.cpp @@ -36,141 +36,36 @@ void setupEncoder1() Serial.println("Connect device 1: true"); } } - -void readEncoderAngle0() +int cummulativePos0() { if (!as5600_0.begin()) { } else { - int angle1 = as5600_0.rawAngle() * AS5600_RAW_TO_DEGREES; - Serial.print("Encoder 0 Angle: "); - Serial.print(angle1); - // if (as5600_0.getRevolutions() > 1){ - // // motorRightStop(); - // stopMotors(); - // } - } -} - -void readEncoderAngle1() -{ - if (!as5600_1.begin()) - { - } - else - { - int angle2 = as5600_1.rawAngle() * AS5600_RAW_TO_DEGREES; - Serial.print("Encoder 1 Angle: "); - Serial.print(angle2); - } -} - -void getEncoderSpeed0() -{ - if (!as5600_0.begin()) - { - } - else - { - float speed1 = (PI * WHEEL_DIAMETER * as5600_0.getAngularSpeed(AS5600_MODE_RPM)) / 60.0; - Serial.print(" degrees | Speed 0 (cm/s): "); - Serial.println(speed1, 2); - Serial.print("Encoder 0 Speed (RPM): "); - Serial.println(as5600_0.getAngularSpeed(AS5600_MODE_RPM)); - } -} -void getEncoderSpeed1() -{ - if (!as5600_1.begin()) - { - } - else - { - float speed2 = (PI * WHEEL_DIAMETER * as5600_1.getAngularSpeed(AS5600_MODE_RPM)) / 60.0; - Serial.print(" degrees | Speed 1 (cm/s): "); - Serial.println(speed2, 2); - Serial.print("Encoder 1 Speed (RPM): "); - Serial.println(as5600_1.getAngularSpeed(AS5600_MODE_RPM)); - } -} - -int getEncoderRevolutions0() -{ - if (!as5600_0.begin()) - { - } - else - { - static uint32_t lastTime = 0; - // set initial position - as5600_0.getCumulativePosition(); - - // update every 100 ms + as5600_0.resetPosition(); + // as5600_0.getCumulativePosition(); // should be enough up to ~200 RPM - if (millis() - lastTime >= 100) - { - lastTime = millis(); - Serial.print(as5600_0.getCumulativePosition()); - Serial.print("Revolution 0: "); - Serial.println(as5600_0.getRevolutions()); - } - int revolution0 = as5600_0.getRevolutions(); - return revolution0; + int cummulative0 = as5600_0.readAngle(); //right + Serial.println(cummulative0); + return cummulative0; } } -int getEncoderRevolutions1() +int cummulativePos1() { if (!as5600_1.begin()) { } else { - static uint32_t lastTime = 0; - // set initial position - as5600_1.getCumulativePosition(); - - // update every 100 ms + as5600_1.resetPosition(); + // as5600_1.getCumulativePosition(); // should be enough up to ~200 RPM - if (millis() - lastTime >= 100) - { - lastTime = millis(); - Serial.print(as5600_1.getCumulativePosition()); - Serial.print("Revolution 1: "); - Serial.println(as5600_1.getRevolutions()); - } - int revolution1 = as5600_1.getRevolutions(); - return revolution1; - } -} - -int resolution_0() -{ - if (!as5600_0.begin()) - { - } - else - { - Serial.print("Resolution 1: "); - Serial.println(as5600_0.readAngle()); - int resolution0 = as5600_0.readAngle(); - return resolution0; - } -} -int resolution_1() -{ - if (!as5600_1.begin()) - { - } - else - { - Serial.print("Resolution 2: "); - Serial.println(as5600_1.readAngle()); - int resolution1 = as5600_1.readAngle(); - return resolution1; + int cummulative1 = as5600_1.readAngle(); + Serial.println(cummulative1); + return cummulative1; } } \ No newline at end of file diff --git a/src/dead_Reckoning.cpp b/src/dead_Reckoning.cpp new file mode 100644 index 0000000..1779aa5 --- /dev/null +++ b/src/dead_Reckoning.cpp @@ -0,0 +1,56 @@ +// http://www-personal.umich.edu/~johannb/position.htm book link + +#include +#include "dead_Reckoning.h" +#include "EncoderControl.h" +//Fixed values +#define WHEEL_DIAMETER 0.0335 +#define PULSES_PER_REVOLUTION 4096.0 +#define AXLE_LENGTH 0.067 +#define PI 3.1416 + +// New Variables +float disp_r_wheel = 0; +float disp_l_wheel = 0; +int count_R_prev = 0; +int count_L_prev = 0; +float x = 0; +float y = 0; +float theta = 0; +float meter_per_ticks = PI * WHEEL_DIAMETER / PULSES_PER_REVOLUTION; +float orientation_angle , disp_body; + +// Old Variables +int count_R = 0; //For Encoders +int count_L = 0; + +void calculate_traveling(){ + + count_L_prev = count_L; + count_R_prev = count_R; + count_L = cummulativePos0(); + count_R = cummulativePos1(); + disp_l_wheel = (float)count_L_prev * meter_per_ticks; // geting distance in meters each wheel has traveled + disp_r_wheel = (float)count_R_prev * meter_per_ticks; + + if (count_L_prev == count_R_prev) + { // The Straight line condition -> book reference Where am i ? + x += disp_l_wheel * cos(theta); + y += disp_l_wheel * sin(theta); + } + else // for circular arc equations change + { orientation_angle = (disp_r_wheel - disp_l_wheel)/AXLE_LENGTH; + disp_body = (disp_r_wheel + disp_l_wheel) / 2.0; + x += (disp_body/orientation_angle) * (sin(orientation_angle + theta) - sin(theta)); + y -= (disp_body/orientation_angle) * (cos(orientation_angle + theta) - cos(theta)); + theta += orientation_angle; + + + while(theta > PI) + theta -= (2.0*PI); + while(theta < -PI) + theta += (2.0*PI); + } +} + + diff --git a/src/main.cpp b/src/main.cpp index 6ad79cc..5111763 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -4,7 +4,7 @@ #include "MotorControl.h" #include "EncoderControl.h" #include "AS5600.h" - +#include "dead_Reckoning.h" void setup() { @@ -18,23 +18,11 @@ void setup() void loop() { - if (getEncoderRevolutions0() > -2 ) - { - moveRightF(150); - } - else - { - motorRightStop(); - } - - if (getEncoderRevolutions1() < 2) - { - moveLeftF(180); - } - else - { - motorLeftStop(); - } - + calculate_traveling(); + Serial.print("X: " ); + Serial.print(x); + Serial.print(" | Y: "); + Serial.println(y); + delay(1000); }