dead reckoning added #17

include/EncoderControl.h

@@ -3,18 +3,10 @@
 
 #include <Wire.h>
 
-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
 

include/dead_Reckoning.h

@@ -0,0 +1,26 @@
+#ifndef DEAD_RECKONING_H
+#define DEAD_RECKONING_H
+
+#include <Arduino.h>
+
+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

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;
   }
 }

src/dead_Reckoning.cpp

@@ -0,0 +1,56 @@
+//  http://www-personal.umich.edu/~johannb/position.htm book link
+
+#include <Arduino.h>
+#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); 
+    }
+} 
+
+

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);
 }