diff --git a/DifferentialDrive.cpp b/DifferentialDrive.cpp index 2f2d1ea..f7ab168 100644 --- a/DifferentialDrive.cpp +++ b/DifferentialDrive.cpp @@ -29,17 +29,6 @@ void DifferentialDrive::SetTargetSpeeds(float leftSpeed, float rightSpeed) { motor->SetSpeed(leftSpeed); else if (xPosition > 0) motor->SetSpeed(rightSpeed); - - } else if (thing->type == Thing::ControlledMotorType) { - ControlledMotor* motor = (ControlledMotor*)placement[motorIx].thing; - if (motor == nullptr) - continue; - - float xPosition = placement[motorIx].position.x; - if (xPosition < 0) - motor->SetTargetSpeed(leftSpeed); - else if (xPosition > 0) - motor->SetTargetSpeed(rightSpeed); } }; } @@ -49,10 +38,3 @@ void DifferentialDrive::SetTwistSpeed(float forward, float yaw) { float rightSpeed = Float::Clamp(forward + yaw, -1, 1); SetTargetSpeeds(leftSpeed, rightSpeed); } - -// void DifferentialDrive::SetTwistSpeed(float forward, float yaw, float pitch) -// { -// float leftSpeed = Float::Clamp(forward - yaw, -1, 1); -// float rightSpeed = Float::Clamp(forward + yaw, -1, 1); -// SetTargetSpeeds(leftSpeed, rightSpeed); -// } \ No newline at end of file diff --git a/Perception.cpp b/Perception.cpp index 6d4a2d3..788e950 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -1,5 +1,6 @@ #include "Perception.h" #include "Angle.h" +#include "DistanceSensor.h" #include "Switch.h" Perception::Perception() {} @@ -203,18 +204,14 @@ void Perception::SetDepthMap(float angle, float distance) { depthMap[depthMapIx] = distance; } -DistanceSensor* Perception::GetSensor(float angle) { +Sensor* Perception::GetSensor(float angle) { angle = Angle::Normalize(angle); for (unsigned int ix = 0; ix < this->sensorCount; ix++) { Placement placement = this->sensorPlacements[ix]; if (abs(placement.horizontalDirection - angle) < 0.01F) - return (DistanceSensor*)placement.thing; + return (Sensor*)placement.thing; } - DistanceSensor* distanceSensor = new DistanceSensor(); - Placement* newPlacement = new Placement(distanceSensor, angle); - AddSensors(newPlacement, 1); - - return distanceSensor; + return nullptr; } \ No newline at end of file diff --git a/Perception.h b/Perception.h index cf5b7b8..ae5ee0c 100644 --- a/Perception.h +++ b/Perception.h @@ -14,7 +14,7 @@ class Perception { /// @brief Template to make it possible to leave out ths sensorCount /// @tparam sensorCount /// @param sensors An array of sensor placements - template + template inline Perception(Placement (&sensors)[sensorCount]) { Perception(sensors, sensorCount); } @@ -73,7 +73,7 @@ class Perception { unsigned int ToDepthMapIndex(float angle); void SetDepthMap(float angle, float distance); - DistanceSensor* GetSensor(float angle); + Sensor* GetSensor(float angle); protected: // SensorPlacement* sensors = nullptr; diff --git a/Placement.h b/Placement.h index 6526653..1fe1c5d 100644 --- a/Placement.h +++ b/Placement.h @@ -1,8 +1,5 @@ #pragma once -#include "ControlledMotor.h" -#include "DistanceSensor.h" -#include "Motor.h" #include "Thing.h" #include "Vector2.h" #include "Vector3.h" diff --git a/Propulsion.cpp b/Propulsion.cpp index 625b375..dada9fc 100644 --- a/Propulsion.cpp +++ b/Propulsion.cpp @@ -1,5 +1,4 @@ #include "Propulsion.h" -#include "ControlledMotor.h" #include "FloatSingle.h" @@ -16,8 +15,6 @@ void Propulsion::AddMotors(Placement* things, unsigned int thingCount) { Thing* thing = things[thingIx].thing; if (thing->IsMotor()) motorCount++; - if (thing->type == Thing::ControlledMotorType) - hasOdometer = true; } this->placement = new Placement[motorCount]; @@ -56,13 +53,6 @@ Placement* Propulsion::GetMotorPlacement(unsigned int motorId) { } void Propulsion::Update(float currentTimeMs) { - for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { - Thing* thing = placement[motorIx].thing; - if (thing->type == Thing::ControlledMotorType) { - ControlledMotor* motor = (ControlledMotor*)thing; - motor->Update(currentTimeMs); - } - } this->lastUpdateTime = currentTimeMs; } @@ -78,57 +68,3 @@ void Propulsion::SetTwistSpeed(Vector3 linear, float yaw, float pitch, float roll) {} - -/// @brief Odometer returns the total distance traveled since start -/// @return The total distance -/// This returns the average distance of all wheels. The odometer cannot be -/// reset. When using a non-directional encoder, the distance is always -/// increasing. When using a directional encoder, the distance may go down when -/// the robot is driving backward. -/// When no wheel encoder is present, this function always returns zero. -float Propulsion::GetOdometer() { - float odometer = 0; - for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { - Thing* thing = placement[motorIx].thing; - if ((thing->type & Thing::ControlledMotorType) != 0) { - ControlledMotor* motor = (ControlledMotor*)thing; - odometer += motor->encoder->GetDistance() / this->motorCount; - } - } - return odometer; -} - -bool Propulsion::Drive(Vector3 point, float rotation, float currentTimeMs) { - if (!this->driving) { - this->startTime = time(NULL); - this->targetDistance = point.magnitude(); - if (hasOdometer) - this->startOdometer = GetOdometer(); - this->driving = true; - } - if (hasOdometer) { - float distance = GetOdometer() - this->startOdometer; - if (distance >= this->targetDistance) { - this->driving = false; - point = Vector3::zero; - rotation = 0; - } - } else { - double duration = difftime(time(NULL), this->startTime); - if (duration >= this->targetDistance) { - this->driving = false; - point = Vector3::zero; - rotation = 0; - } - } - - if (rotation > 0) - rotation = 1; - if (rotation < 0) - rotation = -1; - SetTwistSpeed(point.normalized() * this->maxSpeed, rotation); - - Update(currentTimeMs); - - return (!this->driving); -} \ No newline at end of file diff --git a/Propulsion.h b/Propulsion.h index b167dd6..7d58263 100644 --- a/Propulsion.h +++ b/Propulsion.h @@ -1,12 +1,9 @@ #pragma once -#include "ControlledMotor.h" +#include "Motor.h" #include "Placement.h" -// #include "Quadcopter.h" #include "Vector2.h" -// #include - namespace Passer { namespace RoboidControl { @@ -41,11 +38,6 @@ class Propulsion { float pitch = 0.0F, float roll = 0.0F); // 3D - // Position control (or actually, distance control) - bool Drive(Vector3 point, float rotation, float currentTimeMs); - - float GetOdometer(); - Placement* placement = nullptr; unsigned int motorCount = 0; @@ -54,8 +46,6 @@ class Propulsion { bool driving = false; float targetDistance = 0; - bool hasOdometer = false; - float startOdometer; time_t startTime; float lastUpdateTime; }; diff --git a/Roboid.cpp b/Roboid.cpp index d171086..3b41bf1 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -1,47 +1,9 @@ #include "Roboid.h" #include -Roboid::Roboid() { - // this->configuration = nullptr; - // this->thingCount = 0; -} - -// Roboid::Roboid(Placement configuration[], unsigned int thingCount) { -// // this->configuration = configuration; -// // this->thingCount = thingCount; - -// perception.AddSensors(configuration, thingCount); -// propulsion.AddMotors(configuration, thingCount); -// } +Roboid::Roboid() {} Roboid::Roboid(Perception* perception, Propulsion* propulsion) { this->perception = perception; this->propulsion = propulsion; } - -bool Roboid::Drive(Waypoint* waypoint, float currentTimeMs) { - bool finished = - propulsion->Drive(waypoint->point, waypoint->rotation, currentTimeMs); - return finished; -} - -void Roboid::FollowTrajectory(Trajectory* trajectory) { - this->trajectory = trajectory; - this->waypointIx = 0; -} - -void Roboid::Update(float currentTimeMs) { - if (this->trajectory == nullptr) - return; - - Waypoint* waypoint = &this->trajectory->waypoints[this->waypointIx]; - if (Drive(waypoint, currentTimeMs)) { - this->waypointIx++; - if (this->waypointIx >= this->trajectory->waypointCount) { - this->trajectory = nullptr; - this->waypointIx = 0; - // stop - // propulsion.SetTwistSpeed(0, 0); - } - } -} \ No newline at end of file diff --git a/Roboid.h b/Roboid.h index 22f903e..4859c5e 100644 --- a/Roboid.h +++ b/Roboid.h @@ -1,42 +1,11 @@ #pragma once -#include "Activation.h" #include "Perception.h" -// #include "Placement.h" -// #include "Propulsion.h" -#include "DifferentialDrive.h" +#include "Propulsion.h" namespace Passer { namespace RoboidControl { -class Waypoint { - public: - Waypoint(float forwardDistance, float rotation) { - this->point = Vector3(0, 0, forwardDistance); - this->distance = forwardDistance; - this->rotation = rotation; - } - float distance = 0; - Vector3 point = Vector3(0, 0, 0); - float rotation = 0; -}; -class Trajectory { - public: - Trajectory(){}; - Trajectory(Waypoint* waypoints, unsigned int waypointCount) { - this->waypoints = waypoints; - this->waypointCount = waypointCount; - } - - Waypoint* waypoints; - unsigned int waypointCount; -}; - -class Acceleration { - public: - float GetMagnitude() { return 0; }; -}; - class Roboid { public: Roboid(); @@ -45,19 +14,8 @@ class Roboid { Perception* perception; Propulsion* propulsion; - Acceleration acceleration; - - // Placement* configuration; - // unsigned int thingCount; void Update(float currentTimeMs); - - bool Drive(Waypoint* waypoint, float currentTimeMs); - void FollowTrajectory(Trajectory* Trajectory); - - public: - Trajectory* trajectory; - unsigned int waypointIx = 0; }; } // namespace RoboidControl } // namespace Passer