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

Real trajectory

Browse Source
This commit is contained in:
Pascal Serrarens 2023-11-24 17:32:21 +01:00
parent 6058d27079
commit d2b240a514
2 changed files with 59 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
Roboid.cpp
View File

@ -13,7 +13,34 @@ Roboid::Roboid(Placement configuration[], unsigned int thingCount) {
propulsion.AddMotors(configuration, thingCount); propulsion.AddMotors(configuration, thingCount);
} }
bool Roboid::Drive(float forwardDistance) { // bool Roboid::Drive(float forwardDistance) {
// bool finished = true;
// for (unsigned int motorIx = 0; motorIx < propulsion.motorCount; motorIx++)
// {
// bool motorFinished = false;
// Thing* thing = propulsion.placement[motorIx].thing;
// switch (thing->type) {
// case Thing::Type::ControlledMotor: {
// ControlledMotor* controlledMotor = (ControlledMotor*)thing;
// motorFinished = controlledMotor->Drive(forwardDistance);
// break;
// }
// case Thing::Type::Motor: {
// Motor* motor = (Motor*)thing;
// motorFinished = motor->Drive(forwardDistance);
// break;
// }
// default:
// break;
// }
// if (motorFinished == false)
// finished = false;
// }
// return finished;
// }
bool Roboid::Drive(Waypoint* waypoint) {
bool finished = true; bool finished = true;
for (unsigned int motorIx = 0; motorIx < propulsion.motorCount; motorIx++) { for (unsigned int motorIx = 0; motorIx < propulsion.motorCount; motorIx++) {
bool motorFinished = false; bool motorFinished = false;
@ -22,44 +49,40 @@ bool Roboid::Drive(float forwardDistance) {
switch (thing->type) { switch (thing->type) {
case Thing::Type::ControlledMotor: { case Thing::Type::ControlledMotor: {
ControlledMotor* controlledMotor = (ControlledMotor*)thing; ControlledMotor* controlledMotor = (ControlledMotor*)thing;
motorFinished = controlledMotor->Drive(forwardDistance); motorFinished = controlledMotor->Drive(waypoint->point.z);
break; break;
} }
case Thing::Type::Motor: { case Thing::Type::Motor: {
Motor* motor = (Motor*)thing; Motor* motor = (Motor*)thing;
motorFinished = motor->Drive(forwardDistance); motorFinished = motor->Drive(waypoint->point.z);
break; break;
} }
default: default:
break; break;
} }
if (motorFinished == false) { if (motorFinished == false)
finished = false; finished = false;
} else
Serial.printf("Motor finished\n");
} }
return finished; return finished;
} }
float waypointArray[] = {1, -1}; void Roboid::FollowTrajectory(Trajectory* trajectory) {
void Roboid::FollowTrajectory(Trajectory* Trajectory) { this->trajectory = trajectory;
this->waypoints = waypointArray;
this->waypointCount = 2;
this->waypointIx = 0; this->waypointIx = 0;
} }
void Roboid::Update() { void Roboid::Update() {
if (this->waypoints == nullptr) if (this->trajectory == nullptr)
return; return;
// Serial.printf("Driving waypoints %d %f\n", this->waypointIx, Waypoint* waypoint = &this->trajectory->waypoints[this->waypointIx];
// waypoints[waypointIx]); // Serial.printf("Driving waypoints %d: %f %f\n", this->waypointIx,
if (Drive(this->waypoints[this->waypointIx])) { // waypoint->point.z, waypoint->rotation);
if (Drive(waypoint)) {
this->waypointIx++; this->waypointIx++;
if (this->waypointIx == this->waypointCount) { if (this->waypointIx == this->trajectory->waypointCount) {
this->waypoints = nullptr; this->trajectory = nullptr;
this->waypointIx = 0; this->waypointIx = 0;
this->waypointCount = 0;
propulsion.SetTwistSpeed(0, 0); propulsion.SetTwistSpeed(0, 0);
} }
} }

24
Roboid.h
View File

@ -5,13 +5,27 @@
#include "Placement.h" #include "Placement.h"
#include "Propulsion.h" #include "Propulsion.h"
class WayPoint { class Waypoint {
public: public:
Waypoint(float forwardDistance, float rotation) {
this->point = Vector3(0, 0, forwardDistance);
this->distance = forwardDistance;
this->rotation = rotation;
}
float distance = 0; float distance = 0;
Vector3 point = Vector3(0, 0, 0);
float rotation = 0;
}; };
class Trajectory { class Trajectory {
public: public:
float* waypoints; Trajectory(){};
Trajectory(Waypoint* waypoints, unsigned int waypointCount) {
this->waypoints = waypoints;
this->waypointCount = waypointCount;
}
Waypoint* waypoints;
unsigned int waypointCount;
}; };
class Acceleration { class Acceleration {
@ -33,11 +47,11 @@ class Roboid {
void Update(); void Update();
bool Drive(float forwardDistance); // bool Drive(float forwardDistance);
bool Drive(Waypoint* waypoint);
void FollowTrajectory(Trajectory* Trajectory); void FollowTrajectory(Trajectory* Trajectory);
public: public:
float* waypoints = nullptr; Trajectory* trajectory;
unsigned int waypointIx = 0; unsigned int waypointIx = 0;
unsigned int waypointCount = 0;
}; };