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

24
Roboid.h
View File

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