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

AVR compatibility

Browse Source
This commit is contained in:
Pascal Serrarens 2023-11-29 17:03:49 +01:00
parent 32909f20be
commit c233488885
12 changed files with 86 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ControlledMotor.cpp
View File

@ -1,7 +1,7 @@
#include "ControlledMotor.h" #include "ControlledMotor.h"
#include <Arduino.h> #include <Arduino.h>
ControlledMotor::ControlledMotor() { ControlledMotor::ControlledMotor() {
this->type = Type::ControlledMotor; this->type = Thing::ControlledMotorType;
} }
ControlledMotor::ControlledMotor(Motor* motor, Encoder* encoder) ControlledMotor::ControlledMotor(Motor* motor, Encoder* encoder)
@ -12,11 +12,13 @@ ControlledMotor::ControlledMotor(Motor* motor, Encoder* encoder)
void ControlledMotor::SetTargetSpeed(float velocity) { void ControlledMotor::SetTargetSpeed(float velocity) {
this->targetVelocity = velocity; this->targetVelocity = velocity;
this->rotationDirection = (targetVelocity < 0) ? Direction::Reverse : Direction::Forward; this->rotationDirection =
(targetVelocity < 0) ? Direction::Reverse : Direction::Forward;
} }
void ControlledMotor::Update(float currentTimeMs) { void ControlledMotor::Update(float currentTimeMs) {
actualVelocity = (int)rotationDirection * encoder->GetRevolutionsPerSecond(currentTimeMs); actualVelocity =
(int)rotationDirection * encoder->GetRevolutionsPerSecond(currentTimeMs);
float error = targetVelocity - velocity; float error = targetVelocity - velocity;
float timeStep = currentTimeMs - lastUpdateTime; float timeStep = currentTimeMs - lastUpdateTime;
@ -27,7 +29,7 @@ void ControlledMotor::Update(float currentTimeMs) {
} }
float ControlledMotor::GetActualSpeed() { float ControlledMotor::GetActualSpeed() {
return actualVelocity; //(int)rotationDirection * encoder->GetRevolutionsPerSecond(currentTimeMs); return actualVelocity;
} }
bool ControlledMotor::Drive(float distance) { bool ControlledMotor::Drive(float distance) {
@ -36,10 +38,7 @@ bool ControlledMotor::Drive(float distance) {
startDistance = encoder->GetDistance(); startDistance = encoder->GetDistance();
driving = true; driving = true;
} }
// else
// targetDistance = encoder->GetDistance(); // encoder->RestartCountingRevolutions();
float totalDistance = encoder->GetDistance() - startDistance; float totalDistance = encoder->GetDistance() - startDistance;
Serial.printf("total distance = %f\n", totalDistance);
bool completed = totalDistance > targetDistance; bool completed = totalDistance > targetDistance;
return completed; return completed;
} }

3
ControlledMotor.h
View File

@ -14,6 +14,9 @@ class ControlledMotor : public Thing {
ControlledMotor(); ControlledMotor();
ControlledMotor(Motor* motor, Encoder* encoder); ControlledMotor(Motor* motor, Encoder* encoder);
inline static bool CheckType(Thing* thing) {
return (thing->type & (int)Thing::Type::ControlledMotor) != 0;
}
float velocity; float velocity;
float pidP = 1; float pidP = 1;

21
DistanceSensor.cpp
View File

@ -1,34 +1,27 @@
#include "DistanceSensor.h" #include "DistanceSensor.h"
DistanceSensor::DistanceSensor() DistanceSensor::DistanceSensor() {
{ this->type = Thing::DistanceSensorType;
isDistanceSensor = true;
} }
DistanceSensor::DistanceSensor(float triggerDistance) DistanceSensor::DistanceSensor(float triggerDistance) {
{
isDistanceSensor = true;
this->triggerDistance = triggerDistance; this->triggerDistance = triggerDistance;
} }
float DistanceSensor::GetDistance() float DistanceSensor::GetDistance() {
{
return distance; return distance;
}; };
void DistanceSensor::SetDistance(float distance) void DistanceSensor::SetDistance(float distance) {
{
this->distance = distance; this->distance = distance;
}; // for simulation purposes }; // for simulation purposes
bool DistanceSensor::IsOn() bool DistanceSensor::IsOn() {
{
bool isOn = GetDistance() <= triggerDistance; bool isOn = GetDistance() <= triggerDistance;
return isOn; return isOn;
} }
bool DistanceSensor::isOff() bool DistanceSensor::isOff() {
{
bool isOff = GetDistance() > triggerDistance; bool isOff = GetDistance() > triggerDistance;
return isOff; return isOff;
} }

2
Motor.cpp
View File

@ -4,7 +4,7 @@
// #include <Arduino.h> // #include <Arduino.h>
Motor::Motor() { Motor::Motor() {
type = Type::Motor; type = Thing::MotorType;
} }
float Motor::GetSpeed() { float Motor::GetSpeed() {

32
Propulsion.cpp
View File

@ -14,10 +14,9 @@ void Propulsion::AddMotors(Placement* things, unsigned int thingCount) {
this->motorCount = 0; this->motorCount = 0;
for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
Thing* thing = things[thingIx].thing; Thing* thing = things[thingIx].thing;
if (thing->type == Thing::Type::Motor || if ((thing->type & Thing::MotorType) != 0)
thing->type == Thing::Type::ControlledMotor)
motorCount++; motorCount++;
if (thing->type == Thing::Type::ControlledMotor) if (thing->type == (int)Thing::Type::ControlledMotor)
hasOdometer = true; hasOdometer = true;
} }
this->placement = new Placement[motorCount]; this->placement = new Placement[motorCount];
@ -25,8 +24,7 @@ void Propulsion::AddMotors(Placement* things, unsigned int thingCount) {
unsigned int motorIx = 0; unsigned int motorIx = 0;
for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
Thing* thing = things[thingIx].thing; Thing* thing = things[thingIx].thing;
if (thing->type == Thing::Type::Motor || if ((thing->type & Thing::MotorType) != 0)
thing->type == Thing::Type::ControlledMotor)
this->placement[motorIx++] = things[thingIx]; this->placement[motorIx++] = things[thingIx];
} }
} }
@ -44,8 +42,7 @@ Motor* Propulsion::GetMotor(unsigned int motorId) {
return nullptr; return nullptr;
Thing* thing = this->placement[motorId].thing; Thing* thing = this->placement[motorId].thing;
// if (thing->isMotor) if ((thing->type & Thing::MotorType) != 0)
if (thing->type == Thing::Type::Motor)
return (Motor*)thing; return (Motor*)thing;
return nullptr; return nullptr;
@ -53,13 +50,13 @@ Motor* Propulsion::GetMotor(unsigned int motorId) {
void Propulsion::Update(float currentTimeMs) { void Propulsion::Update(float currentTimeMs) {
// time_t currentTime = time(NULL); // time_t currentTime = time(NULL);
float timeStep = // float timeStep =
currentTimeMs - // currentTimeMs -
this->lastUpdateTime; // difftime(currentTime, this->lastUpdateTime); // this->lastUpdateTime; // difftime(currentTime, this->lastUpdateTime);
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
Thing* thing = placement[motorIx].thing; Thing* thing = placement[motorIx].thing;
if (thing->type == Thing::Type::ControlledMotor) { if (thing->type == Thing::ControlledMotorType) {
ControlledMotor* motor = (ControlledMotor*)thing; ControlledMotor* motor = (ControlledMotor*)thing;
motor->Update(currentTimeMs); motor->Update(currentTimeMs);
} }
@ -74,8 +71,8 @@ void Propulsion::SetMaxSpeed(float maxSpeed) {
void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) { void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
Thing* thing = placement[motorIx].thing; Thing* thing = placement[motorIx].thing;
if (thing->type == Thing::Type::Motor) { if (thing->type == Thing::UncontrolledMotorType) {
Motor* motor = (Motor*)placement[motorIx].thing; Motor* motor = (Motor*)thing;
if (motor == nullptr) if (motor == nullptr)
continue; continue;
@ -85,8 +82,7 @@ void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
else if (xPosition > 0) else if (xPosition > 0)
motor->SetSpeed(rightSpeed); motor->SetSpeed(rightSpeed);
Serial.printf("motor %d, speed = %f\n", motorIx, motor->GetSpeed()); } else if (thing->type == Thing::ControlledMotorType) {
} else if (thing->type == Thing::Type::ControlledMotor) {
ControlledMotor* motor = (ControlledMotor*)placement[motorIx].thing; ControlledMotor* motor = (ControlledMotor*)placement[motorIx].thing;
if (motor == nullptr) if (motor == nullptr)
continue; continue;
@ -96,9 +92,6 @@ void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
motor->SetTargetSpeed(leftSpeed); motor->SetTargetSpeed(leftSpeed);
else if (xPosition > 0) else if (xPosition > 0)
motor->SetTargetSpeed(rightSpeed); motor->SetTargetSpeed(rightSpeed);
// Serial.printf("controlled motor %d, speed = %f\n", motorIx,
// motor->GetActualSpeed());
} }
}; };
} }
@ -168,7 +161,7 @@ float Propulsion::GetOdometer() {
float odometer = 0; float odometer = 0;
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
Thing* thing = placement[motorIx].thing; Thing* thing = placement[motorIx].thing;
if (thing->type == Thing::Type::ControlledMotor) { if ((thing->type & Thing::ControlledMotorType) != 0) {
ControlledMotor* motor = (ControlledMotor*)thing; ControlledMotor* motor = (ControlledMotor*)thing;
odometer += motor->encoder->GetDistance() / this->motorCount; odometer += motor->encoder->GetDistance() / this->motorCount;
} }
@ -186,7 +179,6 @@ bool Propulsion::Drive(Vector3 point, float rotation, float currentTimeMs) {
} }
if (hasOdometer) { if (hasOdometer) {
float distance = GetOdometer() - this->startOdometer; float distance = GetOdometer() - this->startOdometer;
Serial.printf("Odometer = %f\n", distance);
if (distance >= this->targetDistance) { if (distance >= this->targetDistance) {
this->driving = false; this->driving = false;
point = Vector3::zero; point = Vector3::zero;

2
Roboid.cpp
View File

@ -30,8 +30,6 @@ void Roboid::Update(float currentTimeMs) {
return; return;
Waypoint* waypoint = &this->trajectory->waypoints[this->waypointIx]; Waypoint* waypoint = &this->trajectory->waypoints[this->waypointIx];
Serial.printf("Driving waypoints %d: %f %f\n", this->waypointIx,
waypoint->point.z, waypoint->rotation);
if (Drive(waypoint, currentTimeMs)) { if (Drive(waypoint, currentTimeMs)) {
this->waypointIx++; this->waypointIx++;
if (this->waypointIx >= this->trajectory->waypointCount) { if (this->waypointIx >= this->trajectory->waypointCount) {

46
Sensing.cpp
View File

@ -1,9 +1,8 @@
#include "Sensing.h" #include "Sensing.h"
#include "DistanceSensor.h" #include "DistanceSensor.h"
// #include <Switch.h> #include "Switch.h"
#include <math.h> #include <math.h>
#include <algorithm>
SensorPlacement::SensorPlacement(DistanceSensor* distanceSensor, SensorPlacement::SensorPlacement(DistanceSensor* distanceSensor,
Vector2 direction) { Vector2 direction) {
@ -19,18 +18,11 @@ SensorPlacement::SensorPlacement(Switch* switchSensor, Vector2 direction) {
Sensing::Sensing() {} Sensing::Sensing() {}
// void Sensing::AddSensors(SensorPlacement* sensors, unsigned int sensorCount)
// {
// this->sensors = sensors;
// this->sensorCount = sensorCount;
// }
void Sensing::AddSensors(Placement* things, unsigned int thingCount) { void Sensing::AddSensors(Placement* things, unsigned int thingCount) {
sensorCount = 0; sensorCount = 0;
for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
Thing* thing = things[thingIx].thing; Thing* thing = things[thingIx].thing;
// if (thing->isSensor) if ((thing->type & Thing::SensorType) != 0)
if (thing->type == Thing::Type::Sensor)
sensorCount++; sensorCount++;
} }
@ -39,11 +31,11 @@ void Sensing::AddSensors(Placement* things, unsigned int thingCount) {
unsigned int sensorIx = 0; unsigned int sensorIx = 0;
for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) { for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
Thing* thing = things[thingIx].thing; Thing* thing = things[thingIx].thing;
// if (thing->isSensor) if ((thing->type & Thing::SensorType) != 0) {
if (thing->type == Thing::Type::Sensor)
sensorPlacements[sensorIx++] = things[thingIx]; sensorPlacements[sensorIx++] = things[thingIx];
} }
} }
}
unsigned int Sensing::GetSensorCount() { unsigned int Sensing::GetSensorCount() {
return this->sensorCount; return this->sensorCount;
@ -54,8 +46,7 @@ Sensor* Sensing::GetSensor(unsigned int sensorId) {
return nullptr; return nullptr;
Thing* thing = this->sensorPlacements[sensorId].thing; Thing* thing = this->sensorPlacements[sensorId].thing;
// if (thing->isSensor) if (thing->type & Thing::SensorType != 0)
if (thing->type == Thing::Type::Sensor)
return (Sensor*)thing; return (Sensor*)thing;
return nullptr; return nullptr;
@ -66,7 +57,7 @@ float Sensing::DistanceForward(float angle) {
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Placement placement = sensorPlacements[sensorIx]; Placement placement = sensorPlacements[sensorIx];
Sensor* sensor = (Sensor*)placement.thing; Sensor* sensor = (Sensor*)placement.thing;
if (sensor->isDistanceSensor == false) if (sensor->type & Thing::SensorType != 0)
continue; continue;
DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
@ -83,7 +74,7 @@ float Sensing::DistanceLeft(float angle) {
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Placement placement = sensorPlacements[sensorIx]; Placement placement = sensorPlacements[sensorIx];
Sensor* sensor = (Sensor*)placement.thing; Sensor* sensor = (Sensor*)placement.thing;
if (sensor->isDistanceSensor == false) if (sensor->type & Thing::SensorType != 0)
continue; continue;
DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
@ -101,7 +92,7 @@ float Sensing::DistanceRight(float angle) {
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Placement placement = sensorPlacements[sensorIx]; Placement placement = sensorPlacements[sensorIx];
Sensor* sensor = (Sensor*)placement.thing; Sensor* sensor = (Sensor*)placement.thing;
if (sensor->isDistanceSensor == false) if (sensor->type & Thing::DistanceSensorType != 0)
continue; continue;
DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
@ -119,7 +110,7 @@ float Sensing::DistanceUp(float angle) {
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Placement placement = sensorPlacements[sensorIx]; Placement placement = sensorPlacements[sensorIx];
Sensor* sensor = (Sensor*)placement.thing; Sensor* sensor = (Sensor*)placement.thing;
if (sensor->isDistanceSensor == false) if (sensor->type & Thing::SensorType != 0)
continue; continue;
DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
@ -136,7 +127,7 @@ float Sensing::DistanceDown(float angle) {
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Placement placement = sensorPlacements[sensorIx]; Placement placement = sensorPlacements[sensorIx];
Sensor* sensor = (Sensor*)placement.thing; Sensor* sensor = (Sensor*)placement.thing;
if (sensor->isDistanceSensor == false) if (sensor->type & Thing::SensorType != 0)
continue; continue;
DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
@ -154,16 +145,21 @@ bool Sensing::SwitchOn(float fromAngle, float toAngle) {
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) { for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
Placement placement = sensorPlacements[sensorIx]; Placement placement = sensorPlacements[sensorIx];
float angle = placement.direction.x; float angle = placement.direction.y;
if (angle > fromAngle && angle < toAngle) { if (angle > fromAngle && angle < toAngle) {
DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing; Thing* thing = placement.thing;
if (thing == nullptr)
continue;
// if (placement.switchSensor != nullptr && if ((thing->type & (int)Thing::Type::DistanceSensor) != 0) {
// placement.switchSensor->IsOn()) DistanceSensor* distanceSensor = (DistanceSensor*)thing;
// return true;
// else
if (distanceSensor != nullptr && distanceSensor->IsOn()) if (distanceSensor != nullptr && distanceSensor->IsOn())
return true; return true;
} else if ((thing->type & (int)Thing::Type::Switch) != 0) {
Switch* switchSensor = (Switch*)thing;
if (switchSensor != nullptr && switchSensor->IsOn())
return true;
}
} }
} }
return false; return false;

2
Sensing.h
View File

@ -3,7 +3,7 @@
#include "Placement.h" #include "Placement.h"
#include "Sensor.h" #include "Sensor.h"
#include <list> // #include <list>
namespace Passer::RoboidControl { namespace Passer::RoboidControl {

2
Sensor.cpp
View File

@ -2,5 +2,5 @@
Sensor::Sensor() { Sensor::Sensor() {
// this->isSensor = true; // this->isSensor = true;
type = Type::Sensor; type = Thing::SensorType;
} }

1
Sensor.h
View File

@ -9,7 +9,6 @@ namespace RoboidControl {
class Sensor : public Thing { class Sensor : public Thing {
public: public:
Sensor(); Sensor();
bool isDistanceSensor = false;
}; };
} // namespace RoboidControl } // namespace RoboidControl

31
Thing.h
View File

@ -6,16 +6,29 @@ namespace RoboidControl {
/// @brief A thing is a functional component on a robot /// @brief A thing is a functional component on a robot
class Thing { class Thing {
public: public:
Thing() { Thing() { type = (int)Type::Undetermined; }
type = Type::Undetermined;
// isSensor = false, isMotor = false;
}
enum class Type { Undetermined, Sensor, Motor, ControlledMotor }; enum class Type {
Type type = Type::Undetermined; Undetermined,
// bool isSensor; /// Sensor,
// bool isMotor; Switch,
// bool isControlledMotor; DistanceSensor,
// Motor,
ControlledMotor,
UncontrolledMotor,
};
static const unsigned int MotorType = 0x8000;
static const unsigned int SensorType = 0x4000;
static const unsigned int SwitchType =
SensorType | (unsigned int)Type::Switch;
static const unsigned int DistanceSensorType =
SensorType | (unsigned int)Type::DistanceSensor;
static const unsigned int ControlledMotorType =
MotorType | (unsigned int)Type::ControlledMotor;
static const unsigned int UncontrolledMotorType =
MotorType | (unsigned int)Type::UncontrolledMotor;
unsigned int type = (int)Type::Undetermined;
}; };
} // namespace RoboidControl } // namespace RoboidControl

2
VectorAlgebra

@ -1 +1 @@
Subproject commit 493a3f748907b4fb7e64177f94b7cb98a951af4c Subproject commit 365b1773e501566a685a88ebc9334c35814f3ce0