RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Make DiffDrive subclass of propulsion

Browse Source
This commit is contained in:
Pascal Serrarens 2023-12-04 12:47:18 +01:00
parent 07389498dd
commit 67ff10769a
9 changed files with 205 additions and 105 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
DifferentialDrive.cpp Normal file
View File

@ -0,0 +1,57 @@
#include "DifferentialDrive.h"
#include "FloatSingle.h"
DifferentialDrive::DifferentialDrive(){};
DifferentialDrive::DifferentialDrive(Placement leftMotorPlacement,
Placement rightMotorPlacement) {
this->motorCount = 2;
this->placement = new Placement[2];
this->placement[0] = leftMotorPlacement;
this->placement[1] = rightMotorPlacement;
Motor* leftMotor = (Motor*)leftMotorPlacement.thing;
leftMotor->direction = Motor::Direction::CounterClockwise;
Motor* rightMotor = (Motor*)rightMotorPlacement.thing;
rightMotor->direction = Motor::Direction::Clockwise;
}
void DifferentialDrive::SetTargetSpeeds(float leftSpeed, float rightSpeed) {
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
Thing* thing = placement[motorIx].thing;
if (thing->type == Thing::UncontrolledMotorType) {
Motor* motor = (Motor*)thing;
if (motor == nullptr)
continue;
float xPosition = placement[motorIx].position.x;
if (xPosition < 0)
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);
}
};
}
void DifferentialDrive::SetTwistSpeed(float forward, float yaw) {
float leftSpeed = Float::Clamp(forward - yaw, -1, 1);
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);
}

21
DifferentialDrive.h Normal file
View File

@ -0,0 +1,21 @@
#pragma once
#include "Propulsion.h"
namespace Passer {
namespace RoboidControl {
class DifferentialDrive : public Propulsion {
public:
DifferentialDrive();
DifferentialDrive(Placement leftMotorPlacement,
Placement rightMotorPlacement);
void SetTargetSpeeds(float leftSpeed, float rightSpeed);
virtual void SetTwistSpeed(float forward, float yaw) override;
virtual void SetTwistSpeed(float forward, float yaw, float pitch) override;
};
} // namespace RoboidControl
} // namespace Passer
using namespace Passer::RoboidControl;

15
Perception.cpp
View File

@ -4,6 +4,11 @@
Perception::Perception() {}
Perception::Perception(Placement* sensors, unsigned int sensorCount) {
this->sensorCount = sensorCount;
this->sensorPlacements = sensors;
}
void Perception::AddSensors(Placement* things, unsigned int thingCount) {
sensorCount = 0;
for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
@ -32,7 +37,7 @@ Sensor* Perception::GetSensor(unsigned int sensorId) {
return nullptr;
Thing* thing = this->sensorPlacements[sensorId].thing;
if (thing->type & Thing::SensorType != 0)
if ((thing->type & Thing::SensorType) != 0)
return (Sensor*)thing;
return nullptr;
@ -43,7 +48,7 @@ float Perception::DistanceForward(float angle) {
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Placement placement = sensorPlacements[sensorIx];
Sensor* sensor = (Sensor*)placement.thing;
if (sensor->type & Thing::SensorType != 0)
if ((sensor->type & Thing::SensorType) != 0)
continue;
DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
@ -60,7 +65,7 @@ float Perception::DistanceLeft(float angle) {
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Placement placement = sensorPlacements[sensorIx];
Sensor* sensor = (Sensor*)placement.thing;
if (sensor->type & Thing::SensorType != 0)
if ((sensor->type & Thing::SensorType) != 0)
continue;
DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
@ -95,7 +100,7 @@ float Perception::DistanceUp(float angle) {
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Placement placement = sensorPlacements[sensorIx];
Sensor* sensor = (Sensor*)placement.thing;
if (sensor->type & Thing::SensorType != 0)
if ((sensor->type & Thing::SensorType) != 0)
continue;
DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
@ -112,7 +117,7 @@ float Perception::DistanceDown(float angle) {
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Placement placement = sensorPlacements[sensorIx];
Sensor* sensor = (Sensor*)placement.thing;
if (sensor->type & Thing::SensorType != 0)
if ((sensor->type & Thing::SensorType) != 0)
continue;
DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;

1
Perception.h
View File

@ -10,6 +10,7 @@ class Perception {
public:
/// @brief Setup perception
Perception();
Perception(Placement* sensors, unsigned int sensorCount);
// void AddSensors(SensorPlacement* sensors, unsigned int sensorCount);
void AddSensors(Placement* sensors, unsigned int sensorCount);

139
Propulsion.cpp
View File

@ -10,10 +10,10 @@ Propulsion::Propulsion() {
this->motorCount = 0;
}
void Propulsion::AddMotors(Placement *things, unsigned int thingCount) {
void Propulsion::AddMotors(Placement* things, unsigned int thingCount) {
this->motorCount = 0;
for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
Thing *thing = things[thingIx].thing;
Thing* thing = things[thingIx].thing;
if ((thing->type & Thing::MotorType) != 0)
motorCount++;
if (thing->type == (int)Thing::Type::ControlledMotor)
@ -23,34 +23,36 @@ void Propulsion::AddMotors(Placement *things, unsigned int thingCount) {
unsigned int motorIx = 0;
for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
Thing *thing = things[thingIx].thing;
Thing* thing = things[thingIx].thing;
if ((thing->type & Thing::MotorType) != 0)
this->placement[motorIx++] = things[thingIx];
}
}
void Propulsion::AddQuadcopter(Quadcopter *quadcopter) {
void Propulsion::AddQuadcopter(Quadcopter* quadcopter) {
this->quadcopter = quadcopter;
}
unsigned int Propulsion::GetMotorCount() { return this->motorCount; }
unsigned int Propulsion::GetMotorCount() {
return this->motorCount;
}
Motor *Propulsion::GetMotor(unsigned int motorId) {
Motor* Propulsion::GetMotor(unsigned int motorId) {
if (motorId >= this->motorCount)
return nullptr;
Thing *thing = this->placement[motorId].thing;
Thing* thing = this->placement[motorId].thing;
if ((thing->type & Thing::MotorType) != 0)
return (Motor *)thing;
return (Motor*)thing;
return nullptr;
}
Placement *Propulsion::GetMotorPlacement(unsigned int motorId) {
Placement* Propulsion::GetMotorPlacement(unsigned int motorId) {
if (motorId >= this->motorCount)
return nullptr;
Placement *placement = &this->placement[motorId];
Placement* placement = &this->placement[motorId];
if ((placement->thing->type & Thing::MotorType) != 0)
return placement;
@ -59,72 +61,74 @@ 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;
Thing* thing = placement[motorIx].thing;
if (thing->type == Thing::ControlledMotorType) {
ControlledMotor *motor = (ControlledMotor *)thing;
ControlledMotor* motor = (ControlledMotor*)thing;
motor->Update(currentTimeMs);
}
}
this->lastUpdateTime = currentTimeMs;
}
void Propulsion::SetMaxSpeed(float maxSpeed) { this->maxSpeed = abs(maxSpeed); }
void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
Thing *thing = placement[motorIx].thing;
if (thing->type == Thing::UncontrolledMotorType) {
Motor *motor = (Motor *)thing;
if (motor == nullptr)
continue;
float xPosition = placement[motorIx].position.x;
if (xPosition < 0)
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);
}
};
void Propulsion::SetMaxSpeed(float maxSpeed) {
this->maxSpeed = abs(maxSpeed);
}
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)
// placement.controlledMotor->SetTargetVelocity(leftVelocity);
// else if (placement.position.x > 0)
// placement.controlledMotor->SetTargetVelocity(rightVelocity);
};
}
// void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
// for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
// Thing *thing = placement[motorIx].thing;
// if (thing->type == Thing::UncontrolledMotorType) {
// Motor *motor = (Motor *)thing;
// if (motor == nullptr)
// continue;
// float xPosition = placement[motorIx].position.x;
// if (xPosition < 0)
// 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);
// }
// };
// }
// 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)
// // placement.controlledMotor->SetTargetVelocity(leftVelocity);
// // else if (placement.position.x > 0)
// // placement.controlledMotor->SetTargetVelocity(rightVelocity);
// };
// }
void Propulsion::SetTwistSpeed(float forward, float yaw) {
// 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);
// float leftSpeed = Float::Clamp(forward - yaw, -1, 1);
// float rightSpeed = Float::Clamp(forward + yaw, -1, 1);
// SetDiffDriveSpeed(leftSpeed, rightSpeed);
}
void Propulsion::SetTwistSpeed(float forward, float yaw, float pitch) {
float leftSpeed = Float::Clamp(forward - yaw, -1, 1);
float rightSpeed = Float::Clamp(forward + yaw, -1, 1);
SetDiffDriveSpeed(leftSpeed, rightSpeed);
// float leftSpeed = Float::Clamp(forward - yaw, -1, 1);
// float rightSpeed = Float::Clamp(forward + yaw, -1, 1);
// SetDiffDriveSpeed(leftSpeed, rightSpeed);
if (quadcopter != nullptr) {
quadcopter->SetTwistSpeed(forward, yaw, pitch);
}
// if (quadcopter != nullptr) {
// quadcopter->SetTwistSpeed(forward, yaw, pitch);
// }
}
void Propulsion::SetTwistSpeed(Vector3 linear, float yaw) {
@ -136,18 +140,21 @@ void Propulsion::SetTwistSpeed(Vector3 linear, float yaw) {
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);
// 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,
void Propulsion::SetLinearSpeed(Vector3 velocity,
float yawSpeed,
float rollSpeed) {
if (quadcopter != nullptr)
quadcopter->LinearMotion(velocity, yawSpeed, rollSpeed);
}
Quadcopter *Propulsion::GetQuadcopter() { return quadcopter; }
Quadcopter* Propulsion::GetQuadcopter() {
return quadcopter;
}
/// @brief Odometer returns the total distance traveled since start
/// @return The total distance
@ -159,9 +166,9 @@ Quadcopter *Propulsion::GetQuadcopter() { return quadcopter; }
float Propulsion::GetOdometer() {
float odometer = 0;
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
Thing *thing = placement[motorIx].thing;
Thing* thing = placement[motorIx].thing;
if ((thing->type & Thing::ControlledMotorType) != 0) {
ControlledMotor *motor = (ControlledMotor *)thing;
ControlledMotor* motor = (ControlledMotor*)thing;
odometer += motor->encoder->GetDistance() / this->motorCount;
}
}

40
Propulsion.h
View File

@ -1,28 +1,29 @@
#pragma once
#include "ControlledMotor.h"
#include "Placement.h"
#include "Quadcopter.h"
#include "Vector2.h"
#include <time.h>
// #include <time.h>
namespace Passer {
namespace RoboidControl {
class Propulsion {
public:
public:
/// @brief Setup sensing
Propulsion();
void Update(float currentTimeMs);
void AddMotors(Placement *motors, unsigned int motorCount);
void AddQuadcopter(Quadcopter *quadcopter);
Quadcopter *GetQuadcopter();
void AddMotors(Placement* motors, unsigned int motorCount);
void AddQuadcopter(Quadcopter* quadcopter);
Quadcopter* GetQuadcopter();
unsigned int GetMotorCount();
Motor *GetMotor(unsigned int motorIx);
Placement *GetMotorPlacement(unsigned int motorIx);
Motor* GetMotor(unsigned int motorIx);
Placement* GetMotorPlacement(unsigned int motorIx);
/// @brief Set the maximum linear speed.
/// @param maxSpeed The new maximum linear speed
@ -33,16 +34,17 @@ public:
void SetMaxSpeed(float maxSpeed);
// Velocity control
void SetDiffDriveSpeed(float leftSpeed, float rightSpeed);
void SetDiffDriveVelocities(float leftVelocity, float rightVelocity);
// void SetDiffDriveSpeed(float leftSpeed, float rightSpeed);
// void SetDiffDriveVelocities(float leftVelocity, float rightVelocity);
void SetTwistSpeed(float forward, float yaw);
void SetTwistSpeed(float forward, float yaw, float pitch);
void SetTwistSpeed(Vector3 linear, float yaw);
void SetTwistVelocity(float forward, float yaw);
virtual void SetTwistSpeed(float forward, float yaw);
virtual void SetTwistSpeed(float forward, float yaw, float pitch);
virtual void SetTwistSpeed(Vector3 linear, float yaw);
virtual void SetTwistVelocity(float forward, float yaw);
// Think: drones
void SetLinearSpeed(Vector3 direction, float yawSpeed = 0.0F,
void SetLinearSpeed(Vector3 direction,
float yawSpeed = 0.0F,
float rollSpeed = 0.0F);
// Position control (or actually, distance control)
@ -50,11 +52,11 @@ public:
float GetOdometer();
Placement *placement = nullptr;
Placement* placement = nullptr;
unsigned int motorCount = 0;
protected:
Quadcopter *quadcopter = nullptr;
protected:
Quadcopter* quadcopter = nullptr;
float maxSpeed = 1;
@ -66,6 +68,6 @@ protected:
float lastUpdateTime;
};
} // namespace RoboidControl
} // namespace Passer
} // namespace RoboidControl
} // namespace Passer
using namespace Passer::RoboidControl;

21
Roboid.cpp
View File

@ -2,21 +2,26 @@
#include <Arduino.h>
Roboid::Roboid() {
this->configuration = nullptr;
this->thingCount = 0;
// this->configuration = nullptr;
// this->thingCount = 0;
}
Roboid::Roboid(Placement configuration[], unsigned int thingCount) {
this->configuration = configuration;
this->thingCount = thingCount;
// Roboid::Roboid(Placement configuration[], unsigned int thingCount) {
// // this->configuration = configuration;
// // this->thingCount = thingCount;
perception.AddSensors(configuration, thingCount);
propulsion.AddMotors(configuration, thingCount);
// perception.AddSensors(configuration, thingCount);
// propulsion.AddMotors(configuration, thingCount);
// }
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);
propulsion->Drive(waypoint->point, waypoint->rotation, currentTimeMs);
return finished;
}

14
Roboid.h
View File

@ -2,8 +2,9 @@
#include "Activation.h"
#include "Perception.h"
#include "Placement.h"
#include "Propulsion.h"
// #include "Placement.h"
// #include "Propulsion.h"
#include "DifferentialDrive.h"
namespace Passer {
namespace RoboidControl {
@ -40,13 +41,14 @@ class Roboid {
public:
Roboid();
Roboid(Placement configuration[], unsigned int thingCount);
Roboid(Perception* perception, Propulsion* propulsion);
Perception perception;
Propulsion propulsion;
Perception* perception;
Propulsion* propulsion;
Acceleration acceleration;
Placement* configuration;
unsigned int thingCount;
// Placement* configuration;
// unsigned int thingCount;
void Update(float currentTimeMs);

2
VectorAlgebra

@ -1 +1 @@
Subproject commit 493a3f748907b4fb7e64177f94b7cb98a951af4c
Subproject commit 006ea046e4bb57b033823ba5a3fad0dfc7f5dfba