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

Added component

Browse Source
This commit is contained in:
Pascal Serrarens 2023-11-21 14:38:45 +01:00
parent c2478ce057
commit 2ff984b5bc
2 changed files with 44 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

91
Propulsion.cpp
View File

@ -1,59 +1,54 @@
#include "ControlledMotor.h"
#include "Propulsion.h"
#include "ControlledMotor.h"
#include "FloatSingle.h"
extern "C" {
#include "debug.h"
}
Propulsion::Propulsion() {
this->placement = nullptr;
this->motorCount = 0;
}
//void Propulsion::AddMotors(MotorPlacement* motors, unsigned int motorCount) {
// this->palce = motors;
// this->motorCount = motorCount;
//}
// void Propulsion::AddMotors(MotorPlacement* motors, unsigned int motorCount) {
// this->palce = motors;
// this->motorCount = motorCount;
// }
void Propulsion::AddMotors(Placement* things, unsigned int thingCount) {
//this->placement = motors;
//this->motorCount = motorCount;
this->motorCount = 0;
for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
Thing* thing = things[thingIx].thing;
if (thing->isMotor)
motorCount++;
}
this->placement = new Placement[motorCount];
unsigned int motorIx = 0;
for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
Thing* thing = things[thingIx].thing;
if (thing->isMotor)
this->placement[motorIx++] = things[thingIx];
}
// this->placement = motors;
// this->motorCount = motorCount;
this->motorCount = 0;
for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
Thing* thing = things[thingIx].thing;
if (thing->isMotor)
motorCount++;
}
this->placement = new Placement[motorCount];
unsigned int motorIx = 0;
for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
Thing* thing = things[thingIx].thing;
if (thing->isMotor)
this->placement[motorIx++] = things[thingIx];
}
}
void Propulsion::AddQuadcopter(Quadcopter* quadcopter) {
this->quadcopter = quadcopter;
this->quadcopter = quadcopter;
}
unsigned int Propulsion::GetMotorCount() {
return this->motorCount;
return this->motorCount;
}
Motor* Propulsion::GetMotor(unsigned int motorId) {
if (motorId >= this->motorCount)
return nullptr;
Thing* thing = this->placement[motorId].thing;
if (thing->isMotor)
return (Motor*)thing;
if (motorId >= this->motorCount)
return nullptr;
Thing* thing = this->placement[motorId].thing;
if (thing->isMotor)
return (Motor*)thing;
return nullptr;
}
void Propulsion::Update() {
@ -63,14 +58,14 @@ void Propulsion::Update() {
// lastMillis = curMillis;
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
//Placement placement = placement[motorIx];
// placement.controlledMotor->Update(timeStep);
// Placement placement = placement[motorIx];
// placement.controlledMotor->Update(timeStep);
}
}
void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
Motor* motor = (Motor*) placement[motorIx].thing;
Motor* motor = (Motor*)placement[motorIx].thing;
if (motor == nullptr)
continue;
@ -84,11 +79,11 @@ void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
void Propulsion::SetDiffDriveVelocities(float leftVelocity, float rightVelocity) {
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
//Placement placement = placement[motorIx];
//if (placement.position.x < 0)
// placement.controlledMotor->SetTargetVelocity(leftVelocity);
//else if (placement.position.x > 0)
// placement.controlledMotor->SetTargetVelocity(rightVelocity);
// Placement placement = placement[motorIx];
// if (placement.position.x < 0)
// placement.controlledMotor->SetTargetVelocity(leftVelocity);
// else if (placement.position.x > 0)
// placement.controlledMotor->SetTargetVelocity(rightVelocity);
};
}
@ -105,13 +100,13 @@ void Propulsion::SetTwistSpeed(float forward, float yaw, float pitch) {
SetDiffDriveSpeed(leftSpeed, rightSpeed);
if (quadcopter != nullptr) {
quadcopter->SetTwistSpeed(forward, yaw, pitch);
quadcopter->SetTwistSpeed(forward, yaw, pitch);
}
}
void Propulsion::SetTwistSpeed(Vector3 linear, float yaw) {
if (quadcopter != nullptr)
quadcopter->SetTwistSpeed(linear, yaw);
if (quadcopter != nullptr)
quadcopter->SetTwistSpeed(linear, yaw);
}
void Propulsion::SetTwistVelocity(float forwardVelocity, float turningVelocity) {
@ -121,10 +116,10 @@ void Propulsion::SetTwistVelocity(float forwardVelocity, float turningVelocity)
}
void Propulsion::SetLinearSpeed(Vector3 velocity, float yawSpeed, float rollSpeed) {
if (quadcopter != nullptr)
quadcopter->LinearMotion(velocity, yawSpeed, rollSpeed);
if (quadcopter != nullptr)
quadcopter->LinearMotion(velocity, yawSpeed, rollSpeed);
}
Quadcopter* Propulsion::GetQuadcopter() {
return quadcopter;
return quadcopter;
}

1
component.mk Normal file
View File

@ -0,0 +1 @@
COMPONENT_ADD_INCLUDEDIRS = ./VectorAlgebra/include