Initial attempt for scripting

Accelerometer.h

@@ -7,11 +7,26 @@ class Accelerometer : public Sensor {
  public:
   Accelerometer(){};
 
-  /// @brief This gives the gravity direciton relative to the down direction.
-  /// @param forward the forward direction, negative is backward, positive is forward
-  /// @param sideward the sideward direction, negative is left, positive is to the right
+  /// @brief Returns the direction of the acceleration in the horizontal plane
+  /// @return The direction of the acceleration, negative is left, positive is
+  /// right
+  /// @note The horizontal plane is defined as being orthogonal to the gravity
+  /// vector
   /// @note the units (degrees, radians, -1..1, ...) depend on the sensor
-  void GetAccelerationDirection(float* forward, float* sideward);
+  float GetHorizontalAccelerationDirection();
+  /// @brief Returns the magnitude of the acceleration in the horizontal plane
+  /// @return The magnitude. This value is never negative.
+  /// @note the unity (m/s^2, 0..1) depends on the sensor.
+  float GetHorizontalAccelerationMagnitude();
+
+  /// @brief This gives the gravity direciton relative to the down direction.
+  /// @param horizontal the horizontal direction, negative is left, positive is
+  /// right
+  /// @param vertical the vertical direction, negative is down, positive is up
+  /// @note The horizontal plane is defined as being orthogonal to the gravity
+  /// vector
+  /// @note the units (degrees, radians, -1..1, ...) depend on the sensor
+  void GetAccelerationDirection(float* horizontal, float* vertical);
   /// @brief The magnitude of the gravity field.
   /// @return The magnitude. This value is never negative.
   /// @note the unity (m/s^2, 0..1) depends on the sensor.

ControlledMotor.cpp

@@ -1,20 +1,23 @@
 #include "ControlledMotor.h"
 
-ControlledMotor::ControlledMotor() {}
+ControlledMotor::ControlledMotor() {
+  this->isControlledMotor = true;
+}
 
 ControlledMotor::ControlledMotor(Motor* motor, Encoder* encoder) {
   this->motor = motor;
   this->encoder = encoder;
 }
 
-void ControlledMotor::SetTargetVelocity(float velocity) {
+void ControlledMotor::SetTargetSpeed(float velocity) {
   this->targetVelocity = velocity;
 }
 
 void ControlledMotor::Update(float timeStep) {
-  float velocity = GetActualVelocity();
+  float velocity = GetActualSpeed();
   float error = targetVelocity - velocity;
 
-  float acceleration = error * timeStep * pidP;    // Just P is used at this moment
+  float acceleration =
+      error * timeStep * pidP;  // Just P is used at this moment
   motor->SetSpeed(targetVelocity + acceleration);  // or something like that
 }

ControlledMotor.h

@@ -3,6 +3,9 @@
 #include "Encoder.h"
 #include "Motor.h"
 
+/// @brief A motor with speed control
+/// It uses a feedback loop from an encoder to regulate the speed
+/// The speed is measured in revolutions per second.
 class ControlledMotor : public Thing {
  public:
   ControlledMotor();
@@ -16,17 +19,36 @@ class ControlledMotor : public Thing {
 
   void Update(float timeStep);
 
-  void SetTargetVelocity(float rotationsPerSecond);
-  float GetActualVelocity() {
-    return (int)rotationDirection * encoder->GetRotationsPerSecond();
-  }  // in rotations per second
+  /// @brief Set the target speed for the motor controller
+  /// @param speed the target in revolutions per second.
+  void SetTargetSpeed(float speed);
+
+  /// @brief Get the actual speed from the encoder
+  /// @return The speed in revolutions per second
+  float GetActualSpeed() {
+    return (int)rotationDirection * encoder->GetRevolutionsPerSecond();
+  }
+
+  bool Drive(float distance) {
+    if (!driving) {
+      targetDistance = distance;
+      encoder->StartCountingRevolutions();
+      driving = true;
+    } else
+      targetDistance -= encoder->RestartCountingRevolutions();
+
+    return (targetDistance <= 0);
+  }
 
  protected:
   float targetVelocity;
   Motor* motor;
   Encoder* encoder;
-  enum Direction { Forward = 1,
-                   Reverse = -1 };
+  enum Direction { Forward = 1, Reverse = -1 };
 
   Direction rotationDirection;
+
+  bool driving = false;
+  float targetDistance = 0;
+  float lastEncoderPosition = 0;
 };

Encoder.cpp

@@ -20,7 +20,7 @@ void Encoder::InterruptHandler() {
   transitionCount++;
 }
 
-float Encoder::GetRotationsPerSecond() {
+float Encoder::GetRevolutionsPerSecond() {
   return rps;
 }
 
@@ -41,4 +41,10 @@ void Encoder::Update(float timeStep) {
 
   // float rps = distanceThisUpdate * timeStep;
   distance += distanceThisUpdate;
+}
+
+void Encoder::StartCountingRevolutions() {}
+
+float Encoder::RestartCountingRevolutions() {
+  return 0;
 }

Encoder.h

@@ -5,17 +5,20 @@ class Encoder {
   Encoder();
   Encoder(unsigned char pin, unsigned char transitionsPerRotation);
 
-  float GetRotationsPerSecond();
+  float GetPulsesPerSecond();
+  float GetRevolutionsPerSecond();
 
   void ResetDistance();
   float GetRotationDistance();
 
   void Update(float timeStep);
 
+  virtual void StartCountingRevolutions();
+  virtual float RestartCountingRevolutions();
+
  protected:
   static void InterruptHandler();
   static volatile unsigned char transitionCount;
-
   unsigned char transitionsPerRotation;
   float rps;
 

Motor.h

@@ -6,8 +6,7 @@ class Motor : public Thing {
  public:
   Motor();
   /// @brief Turning direction of the motor
-  enum Direction { Forward = 1,
-                   Reverse = -1 };
+  enum Direction { Forward = 1, Reverse = -1 };
 
   /// @brief Set the turning direction of the motor
   // void SetDirection(Direction direction);

Propulsion.cpp

@@ -77,7 +77,8 @@ void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
   };
 }
 
-void Propulsion::SetDiffDriveVelocities(float leftVelocity, float rightVelocity) {
+void Propulsion::SetDiffDriveVelocities(float leftVelocity,
+                                        float rightVelocity) {
   for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
     // Placement placement = placement[motorIx];
     // if (placement.position.x < 0)
@@ -88,7 +89,8 @@ void Propulsion::SetDiffDriveVelocities(float leftVelocity, float rightVelocity)
 }
 
 void Propulsion::SetTwistSpeed(float forward, float yaw) {
-  // This is configuration dependent, a drone will do something completely different...
+  // This is configuration dependent, a drone will do something completely
+  // different...
   float leftSpeed = Float::Clamp(forward - yaw, -1, 1);
   float rightSpeed = Float::Clamp(forward + yaw, -1, 1);
   SetDiffDriveSpeed(leftSpeed, rightSpeed);
@@ -109,17 +111,38 @@ void Propulsion::SetTwistSpeed(Vector3 linear, float yaw) {
     quadcopter->SetTwistSpeed(linear, yaw);
 }
 
-void Propulsion::SetTwistVelocity(float forwardVelocity, float turningVelocity) {
+void Propulsion::SetTwistVelocity(float forwardVelocity,
+                                  float turningVelocity) {
   float leftVelocity = Float::Clamp(forwardVelocity - turningVelocity, -1, 1);
   float rightVelocity = Float::Clamp(forwardVelocity + turningVelocity, -1, 1);
   SetDiffDriveVelocities(leftVelocity, rightVelocity);
 }
 
-void Propulsion::SetLinearSpeed(Vector3 velocity, float yawSpeed, float rollSpeed) {
+void Propulsion::SetLinearSpeed(Vector3 velocity,
+                                float yawSpeed,
+                                float rollSpeed) {
   if (quadcopter != nullptr)
     quadcopter->LinearMotion(velocity, yawSpeed, rollSpeed);
 }
 
 Quadcopter* Propulsion::GetQuadcopter() {
   return quadcopter;
-}
+}
+
+// bool Propulsion::Drive(Roboid* roboid, float forwardDistance) {
+//   bool finished = true;
+//   for (unsigned int motorIx = 0; motorIx < roboid->propulsion->motorCount;
+//        motorIx++) {
+//     Motor* motor = (Motor*)roboid->placement[motorIx].thing;
+//     if (motor == nullptr)
+//       continue;
+//     if (motor->isControlledMotor == false)
+//       continue;
+
+//     ControlledMotor* controlledMotor = (ControlledMotor*)motor;
+//     bool motorFinished = controlledMotor->Drive(forwardDistance);
+//     if (motorFinished == false)
+//       finished = false;
+//   }
+//   return finished;
+// }

Propulsion.h

@@ -1,17 +1,10 @@
 #pragma once
-
-#include "Quadcopter.h"
 #include "ControlledMotor.h"
 #include "Placement.h"
+#include "Quadcopter.h"
 #include "Vector2.h"
 
-#include <list>
-
-//struct MotorPlacement {
-//  Motor* motor;
-//  ControlledMotor* controlledMotor;
-//  Vector2 position;
-//};
+// #include <list>
 
 class Propulsion {
  public:
@@ -20,13 +13,14 @@ class Propulsion {
 
   void Update();
 
-  //void AddMotors(MotorPlacement* motors, unsigned int motorCount);
   void AddMotors(Placement* motors, unsigned int motorCount);
   void AddQuadcopter(Quadcopter* quadcopter);
+  Quadcopter* GetQuadcopter();
 
   unsigned int GetMotorCount();
   Motor* GetMotor(unsigned int motorIx);
 
+  // Velocity control
   void SetDiffDriveSpeed(float leftSpeed, float rightSpeed);
   void SetDiffDriveVelocities(float leftVelocity, float rightVelocity);
 
@@ -36,14 +30,22 @@ class Propulsion {
   void SetTwistVelocity(float forward, float yaw);
 
   // Think: drones
-  Quadcopter* GetQuadcopter();
-  void SetLinearSpeed(Vector3 direction, float yawSpeed = 0.0F, float rollSpeed = 0.0F);
+  void SetLinearSpeed(Vector3 direction,
+                      float yawSpeed = 0.0F,
+                      float rollSpeed = 0.0F);
+
+  // Position control (or actually, distance control)
+  // void Drive(float forwardDistance);
+  // void Turn(float turnDistance);
+
+  // void Update();
 
- protected:
-  //unsigned long lastMillis;
-  //MotorPlacement* motors = nullptr;
   Placement* placement = nullptr;
   unsigned int motorCount = 0;
 
+ protected:
   Quadcopter* quadcopter = nullptr;
+
+  bool driving = false;
+  float targetDistance = 0;
 };

Roboid.cpp

@@ -11,4 +11,21 @@ Roboid::Roboid(Placement configuration[], unsigned int thingCount) {
 
   perception.AddSensors(configuration, thingCount);
   propulsion.AddMotors(configuration, thingCount);
-}
+}
+
+bool Roboid::Drive(float forwardDistance) {
+  bool finished = true;
+  for (unsigned int motorIx = 0; motorIx < propulsion.motorCount; motorIx++) {
+    Motor* motor = (Motor*)propulsion.placement[motorIx].thing;
+    if (motor == nullptr)
+      continue;
+    if (motor->isControlledMotor == false)
+      continue;
+
+    ControlledMotor* controlledMotor = (ControlledMotor*)motor;
+    bool motorFinished = controlledMotor->Drive(forwardDistance);
+    if (motorFinished == false)
+      finished = false;
+  }
+  return finished;
+}

Roboid.h

@@ -7,7 +7,7 @@
 
 class Acceleration {
  public:
-  float GetMagnitude() { return 0;};
+  float GetMagnitude() { return 0; };
 };
 
 class Roboid {
@@ -21,4 +21,6 @@ class Roboid {
 
   Placement* configuration;
   unsigned int thingCount;
+
+  bool Drive(float forwardDistance);
 };

Thing.h

@@ -6,4 +6,5 @@ class Thing {
   Thing() { isSensor = false, isMotor = false; }
   bool isSensor;
   bool isMotor;
+  bool isControlledMotor;
 };