Fixed reach velocity for wheels

README.md

@@ -1,5 +1,5 @@
 # Introduction
-This repository is part of [OmniCar project](https://divelix.github.io/OmniCar/) and represents program for Arduino MEGA that solves omni platform kinematics to get wheel odometry solution and reach desired velocity.
+This repository is part of [OmniCar project](https://divelix.github.io/OmniCar/) and represents program for Arduino MEGA that solves omni platform kinematics to get wheel odometry solution and reach desired velocity. It utilitizes PID-controllers based on feedback from incremental encoders to control JGB37-520 DC motors. Also it can communicate via UART in half-duplex mode.
 
 # Showcase
 Arduino MEGA runs code of this repository and placed on the 1st floor of the mobile platform. It has `Freeduino Motor Shield V3` and special power board (reduces wiring) mounted on top of it.

include/wheelClassTest.h

@@ -38,13 +38,13 @@ void sendData()
   Serial.print(' ');
   Serial.print(w.getPidOutput());
   Serial.print(' ');
-  Serial.print(w.getCurrentVelocity());
+  Serial.print(w.getCurrentLinearVelocity());
   Serial.print(';');
 }
 
 void loop()
 {
   controlDesired();
-  w.reachVelocity(desiredVelocity);
+  w.reachLinearVelocity(desiredVelocity);
   sendData();
 }

lib/Car/Car.cpp

@@ -73,16 +73,15 @@ void Car::reachWheelsVelocity(Matrix<4> wheelsVel)
 
         findCarPose();
         double dt = (double)diff / 1000; // millis to seconds
-        w1->reachVelocity(wheelsVel(0), dt);
-        w2->reachVelocity(wheelsVel(1), dt);
-        w3->reachVelocity(wheelsVel(2), dt);
-        w4->reachVelocity(wheelsVel(3), dt);
+        w1->reachLinearVelocity(wheelsVel(0), dt);
+        w2->reachLinearVelocity(wheelsVel(1), dt);
+        w3->reachLinearVelocity(wheelsVel(2), dt);
+        w4->reachLinearVelocity(wheelsVel(3), dt);
     }
 }
 
 void Car::update()
 {
-    findCarPose();
     reachCarVelocity(*desiredCarVelocity);
 }
 

lib/Car/Car.h

@@ -3,7 +3,6 @@
 
 #define TRACK 175.0 / 1000     // mm to m
 #define WHEELBASE 165.0 / 1000 // mm to m
-#define DIAMETER 60.0 / 1000   // mm to m
 
 class Car
 {

lib/Motor/Motor.cpp

@@ -8,7 +8,7 @@ Motor::~Motor() { delete motor; }
 
 void Motor::setValue(int value) {
   int allowedValue = constrain(value, -255, 255);
-  if (abs(allowedValue) < 50) allowedValue = 0;
+  if (abs(allowedValue) < 70) allowedValue = 0;
   if (allowedValue == 0) {
     motor->run(RELEASE);
   } else if (allowedValue > 0) {

lib/Wheel/Wheel.cpp

@@ -2,18 +2,20 @@
 #include "Wheel.h"
 
 Wheel::Wheel(
-    unsigned int motorNum, 
-    unsigned int encPinA, 
-    unsigned int encPinB, 
-    bool isClockwise, 
+    unsigned int motorNum,
+    unsigned int encPinA,
+    unsigned int encPinB,
+    bool isClockwise,
     unsigned int intervalMillis)
     : interval(intervalMillis)
 {
     // 1900 - stability limit for motor 1
     // kP = 1140; kI = 3040; kD = 107; // position PID
 
     // kP = 50; kI = 800; kD = 5; // angular velocity PID (crazy on heap to zero)
-    kP = 50; kI = 500; kD = 0; // angular velocity PID
+    kP = 50;
+    kI = 500;
+    kD = 0; // angular velocity PID
 
     this->motor = new Motor(motorNum);
     this->encoder = new Encoder(encPinA, encPinB, isClockwise);
@@ -39,31 +41,24 @@ void Wheel::setValue(int value)
     motor->setValue(value);
 }
 
-void Wheel::reachVelocity(double desiredVelocity, double dt)
+void Wheel::reachLinearVelocity(double desiredLinearVelocity, double dt)
 {
-    // currentMillis = millis();
-    // unsigned long diff = currentMillis - previousMillis;
-    // if (diff > interval)
-    // {
-    //     previousMillis = currentMillis;
-
-        double revolutions = (double)encoder->getTicks() / TICKS_PER_REV;
-        resetEncoder();
-        // double linearDistance = PI * DIAMETER * revolutions;
-        currentAngularVelocity = revolutions / dt;
-        pidFeedback = currentAngularVelocity;
-        pidSetpoint = desiredVelocity;
-        pid->Compute();
-        motor->setValue(pidOutput);
-    // }
+    revolutions = (double)encoder->getTicks() / TICKS_PER_REV;
+    resetEncoder();
+    linearDistance = PI * DIAMETER * revolutions;
+    currentLinearVelocity = linearDistance / dt;
+    pidFeedback = currentLinearVelocity;
+    pidSetpoint = desiredLinearVelocity;
+    pid->Compute();
+    motor->setValue(pidOutput);
 }
 
 void Wheel::triggerA() { encoder->triggerA(); }
 void Wheel::triggerB() { encoder->triggerB(); }
 void Wheel::resetEncoder() { encoder->reset(); }
 
 double Wheel::getPidOutput() { return pidOutput; }
-double Wheel::getCurrentVelocity() { return currentAngularVelocity; }
+double Wheel::getCurrentLinearVelocity() { return currentLinearVelocity; }
 long Wheel::getTicks() { return encoder->getTicks(); }
 int Wheel::getEncPinA() { return encoder->getPinA(); }
 int Wheel::getEncPinB() { return encoder->getPinB(); }

lib/Wheel/Wheel.h

@@ -3,13 +3,16 @@
 #include <PID_v1.h>
 
 #define TICKS_PER_REV 720
+#define DIAMETER 60.0 / 1000   // mm to m
 
 class Wheel
 {
 private:
   double pidSetpoint, pidFeedback, pidOutput;
   double kP, kI, kD;
-  double currentAngularVelocity;
+  double currentLinearVelocity;
+  double revolutions;
+  double linearDistance;
 
   Motor *motor;
   Encoder *encoder;
@@ -28,13 +31,13 @@ class Wheel
       unsigned int intervalMillis);
   ~Wheel();
   void setValue(int value);
-  void reachVelocity(double desiredVelocity, double dt);
+  void reachLinearVelocity(double desiredVelocity, double dt);
   void triggerA();
   void triggerB();
   void resetEncoder();
 
   double getPidOutput();
-  double getCurrentVelocity();
+  double getCurrentLinearVelocity();
   long getTicks();
   int getEncPinA();
   int getEncPinB();