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

fix objId == 0

Browse Source
This commit is contained in:
Pascal Serrarens 2024-04-19 14:44:10 +02:00
parent f6a38aaa04
commit 855ad81345
2 changed files with 16 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
Perception.cpp
View File

@ -6,14 +6,14 @@
#include <math.h>
#define RC_DEBUG2
// #define RC_DEBUG2
#ifdef RC_DEBUG2
#include <Arduino.h>
#endif
unsigned char Perception::maxObjectCount = 70; // 7 is typically the maximum
// number of object which can
// be tracked by a human
unsigned char Perception::maxObjectCount = 7; // 7 is typically the maximum
// number of object which can
// be tracked by a human
Perception::Perception() {
this->trackedObjects = new InterestingThing *[maxObjectCount];
@ -124,6 +124,7 @@ bool Perception::ObjectNearby(float direction, float range) {
return false;
}
#include <WifiSync.h>
void Perception::AddTrackedObject(Sensor *sensor, Polar position,
unsigned char objectType) {
InterestingThing *obj = new InterestingThing(sensor, position);
@ -162,19 +163,20 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position,
if (availableSlotIx < maxObjectCount) {
// a slot is available
this->trackedObjects[availableSlotIx] = obj;
obj->id = availableSlotIx;
obj->id = availableSlotIx + 1;
#ifdef RC_DEBUG2
Serial.print((int)obj->id);
Serial.println(": new tracked object");
#endif
roboid->networkSync->NewObject(obj);
((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj);
}
// If this object is closer than the farthest object, then replace it
else if (obj->position.distance <
this->trackedObjects[farthestObjIx]->position.distance) {
delete this->trackedObjects[farthestObjIx];
this->trackedObjects[farthestObjIx] = obj;
obj->id = availableSlotIx;
obj->id = availableSlotIx + 1;
#ifdef RC_DEBUG2
Serial.print((int)obj->id);
Serial.println(": replaced tracked object");
@ -226,7 +228,7 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
if (availableSlotIx < maxObjectCount) {
// a slot is available
this->trackedObjects[availableSlotIx] = obj;
obj->id = availableSlotIx;
obj->id = availableSlotIx + 1;
#ifdef RC_DEBUG2
Serial.print((int)obj->id);
Serial.println(": new tracked object");
@ -238,7 +240,7 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
this->trackedObjects[farthestObjIx]->position.distance) {
delete this->trackedObjects[farthestObjIx];
this->trackedObjects[farthestObjIx] = obj;
obj->id = availableSlotIx;
obj->id = availableSlotIx + 1;
#ifdef RC_DEBUG2
Serial.print((int)obj->id);
Serial.println(": replaced tracked object");

7
Roboid.cpp
View File

@ -16,6 +16,8 @@ Roboid::Roboid() {
this->perception->roboid = this;
this->propulsion = nullptr;
this->actuationRoot = nullptr;
this->worldPosition = Vector3::zero;
this->worldOrientation = Quaternion::identity;
}
Roboid::Roboid(Perception *perception, Propulsion *propulsion) : Roboid() {
@ -46,9 +48,11 @@ void Roboid::Update(float currentTimeMs) {
if (propulsion != nullptr) {
propulsion->Update(currentTimeMs);
float deltaTime = (currentTimeMs - lastUpdateTimeMs) / 1000;
SetPosition(this->worldPosition +
this->worldOrientation * Vector3::forward *
this->propulsion->GetVelocity().distance);
this->propulsion->GetVelocity().distance * deltaTime);
SetOrientation(this->worldOrientation *
Quaternion::AngleAxis(this->propulsion->GetAngularVelocity(),
Vector3::up));
@ -59,6 +63,7 @@ void Roboid::Update(float currentTimeMs) {
if (actuationRoot != nullptr)
actuationRoot->Update(currentTimeMs);
lastUpdateTimeMs = currentTimeMs;
}
Vector3 Roboid::GetPosition() { return this->worldPosition; }