diff --git a/Activation.cpp b/Activation.cpp index 37002fa..5efc4d0 100644 --- a/Activation.cpp +++ b/Activation.cpp @@ -1,7 +1,7 @@ -#include +#include "Activation.h" float Activation::HeavisideStep(float inputValue, float bias) { - return (inputValue + bias > 0) ? 1 : 0; + return (inputValue + bias > 0) ? 1.0F : 0.0F; } float Activation::Linear(float inputValue, float bias, float range) { diff --git a/ControlledMotor.cpp b/ControlledMotor.cpp index 2b5625b..f8145d4 100644 --- a/ControlledMotor.cpp +++ b/ControlledMotor.cpp @@ -1,4 +1,4 @@ -#include +#include "ControlledMotor.h" ControlledMotor::ControlledMotor() {} diff --git a/ControlledMotor.h b/ControlledMotor.h index 5d15ce1..a283fce 100644 --- a/ControlledMotor.h +++ b/ControlledMotor.h @@ -1,7 +1,7 @@ #pragma once -#include -#include +#include "Encoder.h" +#include "Motor.h" class ControlledMotor : public Thing { public: @@ -18,7 +18,7 @@ class ControlledMotor : public Thing { void SetTargetVelocity(float rotationsPerSecond); float GetActualVelocity() { - return rotationDirection * encoder->GetRotationsPerSecond(); + return (int)rotationDirection * encoder->GetRotationsPerSecond(); } // in rotations per second protected: diff --git a/DistanceSensor.h b/DistanceSensor.h index 15e744a..b18b30f 100644 --- a/DistanceSensor.h +++ b/DistanceSensor.h @@ -5,9 +5,12 @@ /// @brief A sensor which can measure the distance the the nearest object class DistanceSensor : public Sensor { public: + DistanceSensor() { isDistanceSensor = true; } + DistanceSensor(float triggerDistance) { isDistanceSensor = true; this->triggerDistance = triggerDistance; } /// @brief Determine the distance to the nearest object /// @return the measured distance in meters to the nearest object - virtual float GetDistance() = 0; + virtual float GetDistance() { return distance; }; + virtual void SetDistance(float distance) { this->distance = distance; }; // for simulation purposes /// @brief The distance at which ObjectNearby triggers float triggerDistance = 1; @@ -21,4 +24,6 @@ class DistanceSensor : public Sensor { bool isOff = GetDistance() > triggerDistance; return isOff; } +protected: + float distance = 0; }; diff --git a/Encoder.cpp b/Encoder.cpp index 709140d..9b3aba8 100644 --- a/Encoder.cpp +++ b/Encoder.cpp @@ -1,4 +1,4 @@ -#include +#include "Encoder.h" volatile unsigned char Encoder::transitionCount = 0; diff --git a/Motor.cpp b/Motor.cpp index 597b711..7842cfa 100644 --- a/Motor.cpp +++ b/Motor.cpp @@ -1,7 +1,7 @@ -#include +#include "Motor.h" Motor::Motor() { - this->isSensor = false; + this->isMotor = true; } // Motor::Motor(uint8_t pinIn1, uint8_t pinIn2) { @@ -23,3 +23,11 @@ Motor::Motor() { // analogWrite(pinIn1, speed); // analogWrite(pinIn2, 255 - speed); // } + +float Motor::GetSpeed() { + return this->currentSpeed; +} + +void Motor::SetSpeed(float speed) { + this->currentSpeed = speed; +} \ No newline at end of file diff --git a/Motor.h b/Motor.h index a0833be..921171d 100644 --- a/Motor.h +++ b/Motor.h @@ -12,7 +12,7 @@ class Motor : public Thing { /// @brief Set the turning direction of the motor // void SetDirection(Direction direction); - virtual void SetSpeed(float speed) = 0; + virtual void SetSpeed(float speed); float GetSpeed(); protected: diff --git a/Placement.cpp b/Placement.cpp index de7f092..24e9b7b 100644 --- a/Placement.cpp +++ b/Placement.cpp @@ -1,4 +1,4 @@ -#include +#include "Placement.h" Placement::Placement() { this->position = Vector3::zero; @@ -15,11 +15,11 @@ Placement::Placement(Vector2 direction, Sensor* thing) { this->thing = thing; } -Placement::Placement(Vector3 position, Sensor* thing) { - this->position = position; - this->direction = Vector2::zero; - this->thing = thing; -} +//Placement::Placement(Vector3 position, Sensor* thing) { +// this->position = position; +// this->direction = Vector2::zero; +// this->thing = thing; +//} Placement::Placement(Vector3 position, Motor* thing) { this->position = position; @@ -27,13 +27,13 @@ Placement::Placement(Vector3 position, Motor* thing) { this->thing = thing; } -Placement::Placement(Vector3 position, ControlledMotor* thing) { - this->position = position; - this->direction = Vector2::zero; - this->thing = thing; -} - -Placement::Placement(Thing* thing, Vector3 position) { - this->thing = thing; - this->position = position; -} \ No newline at end of file +//Placement::Placement(Vector3 position, ControlledMotor* thing) { +// this->position = position; +// this->direction = Vector2::zero; +// this->thing = thing; +//} +// +//Placement::Placement(Thing* thing, Vector3 position) { +// this->thing = thing; +// this->position = position; +//} \ No newline at end of file diff --git a/Placement.h b/Placement.h index 81b3abe..6c10806 100644 --- a/Placement.h +++ b/Placement.h @@ -1,21 +1,21 @@ #pragma once -#include -#include -#include -#include -#include -#include "Sensor.h" +#include "ControlledMotor.h" +#include "Motor.h" +#include "Thing.h" +#include "Vector2.h" +#include "Vector3.h" +#include "DistanceSensor.h" class Placement { public: Placement(); Placement(Vector2 direction, Sensor* sensor); - Placement(Vector3 position, Sensor* sensor); + //Placement(Vector3 position, Sensor* sensor); Placement(Vector3 position, Motor* motor); - Placement(Vector3 position, ControlledMotor* motor); - Placement(Thing* thing, Vector3 position); + /*Placement(Vector3 position, ControlledMotor* motor); + Placement(Thing* thing, Vector3 position);*/ Placement* parent = nullptr; Placement** children = nullptr; diff --git a/Propulsion.cpp b/Propulsion.cpp index 79c4e9f..64b75a2 100644 --- a/Propulsion.cpp +++ b/Propulsion.cpp @@ -1,21 +1,51 @@ -#include -#include +#include "ControlledMotor.h" +#include "Propulsion.h" -#include +#include "FloatSingle.h" Propulsion::Propulsion() { - this->motors = nullptr; + this->placement = nullptr; this->motorCount = 0; } -void Propulsion::AddMotors(MotorPlacement* motors, unsigned int motorCount) { - this->motors = motors; - this->motorCount = motorCount; +//void Propulsion::AddMotors(MotorPlacement* motors, unsigned int motorCount) { +// this->palce = motors; +// this->motorCount = motorCount; +//} + +void Propulsion::AddMotors(Placement* things, unsigned int thingCount) { + //this->placement = motors; + //this->motorCount = motorCount; + this->motorCount = 0; + for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { + Thing* thing = things[thingIx].thing; + if (thing->isMotor) + motorCount++; + } + this->placement = new Placement[motorCount]; + + unsigned int motorIx = 0; + for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { + Thing* thing = things[thingIx].thing; + if (thing->isMotor) + this->placement[motorIx++] = things[thingIx]; + } + } -void Propulsion::AddMotors(Placement* motors, unsigned int motorCount) { - this->placement = motors; - this->motorCount = motorCount; +unsigned int Propulsion::GetMotorCount() { + return this->motorCount; +} + +Motor* Propulsion::GetMotor(unsigned int motorId) { + if (motorId >= this->motorCount) + return nullptr; + + Thing* thing = this->placement[motorId].thing; + if (thing->isMotor) + return (Motor*)thing; + + return nullptr; } void Propulsion::Update() { @@ -25,18 +55,18 @@ void Propulsion::Update() { // lastMillis = curMillis; for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { - MotorPlacement placement = motors[motorIx]; + //Placement placement = placement[motorIx]; // placement.controlledMotor->Update(timeStep); } } void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) { for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { - Motor* motor = motors[motorIx].motor; + Motor* motor = (Motor*) placement[motorIx].thing; if (motor == nullptr) continue; - float xPosition = motors[motorIx].position.x; + float xPosition = placement[motorIx].position.x; if (xPosition < 0) motor->SetSpeed(leftSpeed); else if (xPosition > 0) @@ -46,11 +76,11 @@ void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) { void Propulsion::SetDiffDriveVelocities(float leftVelocity, float rightVelocity) { for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { - MotorPlacement placement = motors[motorIx]; - if (placement.position.x < 0) - placement.controlledMotor->SetTargetVelocity(leftVelocity); - else if (placement.position.x > 0) - placement.controlledMotor->SetTargetVelocity(rightVelocity); + //Placement placement = placement[motorIx]; + //if (placement.position.x < 0) + // placement.controlledMotor->SetTargetVelocity(leftVelocity); + //else if (placement.position.x > 0) + // placement.controlledMotor->SetTargetVelocity(rightVelocity); }; } diff --git a/Propulsion.h b/Propulsion.h index 23da4f8..b8c6d48 100644 --- a/Propulsion.h +++ b/Propulsion.h @@ -1,16 +1,16 @@ #pragma once -#include -#include -#include +#include "ControlledMotor.h" +#include "Placement.h" +#include "Vector2.h" #include -struct MotorPlacement { - Motor* motor; - ControlledMotor* controlledMotor; - Vector2 position; -}; +//struct MotorPlacement { +// Motor* motor; +// ControlledMotor* controlledMotor; +// Vector2 position; +//}; class Propulsion { public: @@ -19,9 +19,12 @@ class Propulsion { void Update(); - void AddMotors(MotorPlacement* motors, unsigned int motorCount); + //void AddMotors(MotorPlacement* motors, unsigned int motorCount); void AddMotors(Placement* motors, unsigned int motorCount); + unsigned int GetMotorCount(); + Motor* GetMotor(unsigned int motorIx); + void SetDiffDriveSpeed(float leftSpeed, float rightSpeed); void SetDiffDriveVelocities(float leftVelocity, float rightVelocity); @@ -33,8 +36,8 @@ class Propulsion { void SetLinearSpeed(Vector3 direction); protected: - unsigned long lastMillis; - MotorPlacement* motors = nullptr; + //unsigned long lastMillis; + //MotorPlacement* motors = nullptr; Placement* placement = nullptr; unsigned int motorCount = 0; }; diff --git a/Roboid.cpp b/Roboid.cpp index c3e355c..2d6bc9c 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -1,4 +1,4 @@ -#include +#include "Roboid.h" Roboid::Roboid() { this->configuration = nullptr; @@ -10,4 +10,5 @@ Roboid::Roboid(Placement configuration[], unsigned int thingCount) { this->thingCount = thingCount; sensing.AddSensors(configuration, thingCount); + propulsion.AddMotors(configuration, thingCount); } \ No newline at end of file diff --git a/Roboid.h b/Roboid.h index f2f156d..759d039 100644 --- a/Roboid.h +++ b/Roboid.h @@ -1,9 +1,9 @@ #pragma once -#include -#include -#include -#include +#include "Activation.h" +#include "Placement.h" +#include "Propulsion.h" +#include "Sensing.h" class Roboid { public: diff --git a/Sensing.cpp b/Sensing.cpp index f2fedbf..472949a 100644 --- a/Sensing.cpp +++ b/Sensing.cpp @@ -1,6 +1,6 @@ -#include -#include -#include +#include "DistanceSensor.h" +#include "Sensing.h" +//#include #include #include @@ -41,6 +41,21 @@ void Sensing::AddSensors(Placement* things, unsigned int thingCount) { } } +unsigned int Sensing::GetSensorCount() { + return this->sensorCount; +} + +Sensor* Sensing::GetSensor(unsigned int sensorId) { + if (sensorId >= this->sensorCount) + return nullptr; + + Thing* thing = this->sensorPlacements[sensorId].thing; + if (thing->isSensor) + return (Sensor*)thing; + + return nullptr; +} + float Sensing::DistanceForward(float angle) { float minDistance = INFINITY; for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { diff --git a/Sensing.h b/Sensing.h index 849321f..e676152 100644 --- a/Sensing.h +++ b/Sensing.h @@ -1,6 +1,6 @@ #pragma once -#include +#include "Placement.h" #include @@ -27,6 +27,9 @@ class Sensing { // void AddSensors(SensorPlacement* sensors, unsigned int sensorCount); void AddSensors(Placement* sensors, unsigned int sensorCount); + unsigned int GetSensorCount(); + Sensor* GetSensor(unsigned int sensorId); + float DistanceForward(float angle = 90); /// @brief Distance to the closest object on the left diff --git a/Sensor.h b/Sensor.h index d452e0e..f49bfe6 100644 --- a/Sensor.h +++ b/Sensor.h @@ -1,6 +1,6 @@ #pragma once -#include +#include "Thing.h" class Sensor : public Thing { public: diff --git a/Thing.h b/Thing.h index 8ea7661..610542c 100644 --- a/Thing.h +++ b/Thing.h @@ -1,6 +1,8 @@ #pragma once class Thing { - public: - bool isSensor; +public: + Thing() { isSensor = false, isMotor = false; } + bool isSensor; + bool isMotor; }; \ No newline at end of file