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

14
Perception.cpp
View File

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

7
Roboid.cpp
View File

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