Working controlled motor

Arduino/Things/DRV8833.h

@@ -18,7 +18,7 @@ class DRV8833 : public Thing {
     int AIn2;
     int BIn1;
     int BIn2;
-    int standby;
+    int standby = 255;
   };
 
   /// @brief Setup a DRV8833 motor controller

Arduino/Things/DigitalInput.cpp

@@ -7,7 +7,8 @@ namespace Arduino {
 
 #pragma region Digital input
 
-DigitalInput::DigitalInput(unsigned char pin, Thing* parent) : Thing(Type::Undetermined, parent) {
+DigitalInput::DigitalInput(unsigned char pin, Thing* parent)
+    : Thing(Type::Undetermined, parent) {
   this->pin = pin;
   pinMode(this->pin, INPUT);
   std::cout << "digital input start\n";
@@ -46,13 +47,15 @@ DigitalInput::RelativeEncoder::RelativeEncoder(Configuration config,
 
 void DigitalInput::RelativeEncoder::Start() {
   // We support up to 2 pulse counters
-  if (interruptCount == 0)
+  if (interruptCount == 0) {
+    std::cout << "pin interrupt 1 activated" << std::endl;
     attachInterrupt(digitalPinToInterrupt(digitalInput.pin), PulseInterrupt0,
                     RISING);
-  else if (interruptCount == 1)
+  } else if (interruptCount == 1) {
+    std::cout << "pin interrupt 2 activated" << std::endl;
     attachInterrupt(digitalPinToInterrupt(digitalInput.pin), PulseInterrupt1,
                     RISING);
-  else {
+  } else {
     // maximum interrupt count reached
     std::cout << "DigitalInput::RelativeEncoder: max. # counters of 2 reached"
               << std::endl;
@@ -85,14 +88,17 @@ void DigitalInput::RelativeEncoder::Update(bool recursive) {
   }
 
   float timeStep = (float)(currentTimeMs - this->lastUpdateTime) / 1000.0f;
+  if (timeStep <= 0)
+    return;
+
   int pulseCount = GetPulseCount();
   netPulseCount += pulseCount;
 
   this->pulseFrequency = pulseCount / timeStep;
   this->rotationSpeed = pulseFrequency / pulsesPerRevolution;
 
-  // std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency
-  //           << " timestep " << timeStep << std::endl;
+  std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency
+            << " timestep " << timeStep << std::endl;
 
   this->lastUpdateTime = currentTimeMs;
 }

Arduino/Things/UltrasonicSensor.cpp

@@ -9,8 +9,8 @@ namespace Arduino {
 UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
     : Thing(Type::Undetermined, parent) {
   this->name = "Ultrasonic sensor";
-  this->pinTrigger = config.triggerPin;
-  this->pinEcho = config.echoPin;
+  this->pinTrigger = config.trigger;
+  this->pinEcho = config.echo;
 
   pinMode(pinTrigger, OUTPUT);  // configure the trigger pin to output mode
   pinMode(pinEcho, INPUT);      // configure the echo pin to input mode

Arduino/Things/UltrasonicSensor.h

@@ -9,8 +9,8 @@ namespace Arduino {
 class UltrasonicSensor : Thing {
  public:
   struct Configuration {
-    int triggerPin;
-    int echoPin;
+    int trigger;
+    int echo;
   };
 
   UltrasonicSensor(Configuration config, Thing* parent = Thing::LocalRoot());

Things/ControlledMotor.cpp

@@ -1,4 +1,5 @@
 #include "ControlledMotor.h"
+#include "LinearAlgebra/FloatSingle.h"
 
 namespace RoboidControl {
 
@@ -7,9 +8,10 @@ ControlledMotor::ControlledMotor(Motor* motor,
                                  Thing* parent)
     : Motor(parent), motor(motor), encoder(encoder) {
   this->type = Type::ControlledMotor;
-  // encoder.SetParent(*this);
+  //encoder->SetParent(null);
   //  Thing parent = motor.GetParent();
   //  this->SetParent(parent);
+  this->integral = 0;
 }
 
 void ControlledMotor::SetTargetVelocity(float velocity) {
@@ -19,22 +21,34 @@ void ControlledMotor::SetTargetVelocity(float velocity) {
 }
 
 void ControlledMotor::Update(bool recurse) {
-  encoder->Update(false);
-
-  this->actualVelocity = (int)rotationDirection * encoder->rotationSpeed;
-
   unsigned long currentTimeMs = GetTimeMs();
   float timeStep = (currentTimeMs - this->lastUpdateTime) / 1000.0f;
   this->lastUpdateTime = currentTimeMs;
+  if (timeStep <= 0)
+    return;
+
+  // encoder->Update(false);
+
+  this->actualVelocity = (int)rotationDirection * encoder->rotationSpeed;
 
   float error = this->targetVelocity - this->actualVelocity;
-  float acceleration =
-      error * timeStep * pidP;  // Just P is used at this moment
-  std::cout << "motor acc. " << acceleration << std::endl;
-  motor->SetTargetVelocity(targetVelocity +
-                          acceleration);  // or something like that
+  float p_term = error * pidP;
+  this->integral += error * timeStep;
+  float i_term = pidI * this->integral;
+  float d_term = pidD * (error - this->lastError) / timeStep;
+  this->lastError = error;
 
-  motor->Update(false);
+  float output = p_term + i_term + d_term;
+  std::cout << "target " << this->targetVelocity << " actual "
+            << this->actualVelocity << " output = " << output << std::endl;
+  // float acceleration =
+  //     error * timeStep * pidP;  // Just P is used at this moment
+  // std::cout << "motor acc. " << acceleration << std::endl;
+
+  // float newTargetVelocity = motor->targetVelocity + acceleration;
+  output = LinearAlgebra::Float::Clamp(output, -1, 1);
+  motor->SetTargetVelocity(output);  // or something like that
+  //motor->Update(false);
 }
 
 // float ControlledMotor::GetActualVelocity() {

Things/ControlledMotor.h

@@ -12,9 +12,9 @@ class ControlledMotor : public Motor {
  public:
   ControlledMotor(Motor* motor, RelativeEncoder* encoder, Thing* parent = Thing::LocalRoot());
 
-  float pidP = 1;
+  float pidP = 0.5;
   float pidD = 0;
-  float pidI = 0;
+  float pidI = 0.2;
 
   /// @brief The actual velocity in revolutions per second
   float actualVelocity;
@@ -32,6 +32,8 @@ class ControlledMotor : public Motor {
   RelativeEncoder* encoder;
 
  protected:
+  float integral = 0;
+  float lastError = 0;
   float lastUpdateTime;
 };
 

Things/RelativeEncoder.h

@@ -14,14 +14,14 @@ class RelativeEncoder : public Thing {
   /// full rotation
   /// @param distancePerRevolution The distance a wheel travels per full
   /// rotation
-  RelativeEncoder(Participant* owner);
+  //RelativeEncoder(Participant* owner);
   // RelativeEncoder(Thing* parent);
   RelativeEncoder(Thing* parent = Thing::LocalRoot());
 
   /// @brief Get the rotation speed 
   /// @return The speed in revolutions per second
   virtual float GetRotationSpeed();
-  float rotationSpeed;
+  float rotationSpeed = 0;
 
   /// @brief Function used to generate binary data for this touch sensor
   /// @param buffer The byte array for thw binary data