RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki 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

15
ControlledMotor.cpp
View File

@ -1,7 +1,7 @@
#include "ControlledMotor.h"
#include <Arduino.h>
ControlledMotor::ControlledMotor() {
this->type = Type::ControlledMotor;
this->type = Thing::ControlledMotorType;
}
ControlledMotor::ControlledMotor(Motor* motor, Encoder* encoder)
@ -12,22 +12,24 @@ ControlledMotor::ControlledMotor(Motor* motor, Encoder* encoder)
void ControlledMotor::SetTargetSpeed(float 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) {
actualVelocity = (int)rotationDirection * encoder->GetRevolutionsPerSecond(currentTimeMs);
actualVelocity =
(int)rotationDirection * encoder->GetRevolutionsPerSecond(currentTimeMs);
float error = targetVelocity - velocity;
float timeStep = currentTimeMs - lastUpdateTime;
float acceleration =
error * timeStep * pidP; // Just P is used at this moment
error * timeStep * pidP; // Just P is used at this moment
motor->SetSpeed(targetVelocity + acceleration); // or something like that
this->lastUpdateTime = currentTimeMs;
}
float ControlledMotor::GetActualSpeed() {
return actualVelocity; //(int)rotationDirection * encoder->GetRevolutionsPerSecond(currentTimeMs);
return actualVelocity;
}
bool ControlledMotor::Drive(float distance) {
@ -36,10 +38,7 @@ bool ControlledMotor::Drive(float distance) {
startDistance = encoder->GetDistance();
driving = true;
}
// else
// targetDistance = encoder->GetDistance(); // encoder->RestartCountingRevolutions();
float totalDistance = encoder->GetDistance() - startDistance;
Serial.printf("total distance = %f\n", totalDistance);
bool completed = totalDistance > targetDistance;
return completed;
}

3
ControlledMotor.h
View File

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

37
DistanceSensor.cpp
View File

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

2
Motor.cpp
View File

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

32
Propulsion.cpp
View File

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

2
Roboid.cpp
View File

@ -30,8 +30,6 @@ void Roboid::Update(float currentTimeMs) {
return;
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)) {
this->waypointIx++;
if (this->waypointIx >= this->trajectory->waypointCount) {

50
Sensing.cpp
View File

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

2
Sensing.h
View File

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

2
Sensor.cpp
View File

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

1
Sensor.h
View File

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

31
Thing.h
View File

@ -6,16 +6,29 @@ namespace RoboidControl {
/// @brief A thing is a functional component on a robot
class Thing {
public:
Thing() {
type = Type::Undetermined;
// isSensor = false, isMotor = false;
}
Thing() { type = (int)Type::Undetermined; }
enum class Type { Undetermined, Sensor, Motor, ControlledMotor };
Type type = Type::Undetermined;
// bool isSensor;
// bool isMotor;
// bool isControlledMotor;
enum class Type {
Undetermined,
/// Sensor,
Switch,
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

2
VectorAlgebra

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