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

Get Polar velocity

Browse Source
This commit is contained in:
Pascal Serrarens 2023-12-28 09:08:13 +01:00
parent a502af99d4
commit 30c74c44a4
2 changed files with 20 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
DifferentialDrive.cpp
View File

@ -1,6 +1,6 @@
#include "DifferentialDrive.h" #include "DifferentialDrive.h"
#include "FloatSingle.h" #include "FloatSingle.h"
#include <Arduino.h>
DifferentialDrive::DifferentialDrive(){}; DifferentialDrive::DifferentialDrive(){};
@ -10,17 +10,18 @@ DifferentialDrive::DifferentialDrive(Placement leftMotorPlacement,
this->placement = new Placement[2]; this->placement = new Placement[2];
this->placement[0] = leftMotorPlacement; this->placement[0] = leftMotorPlacement;
this->placement[1] = rightMotorPlacement; this->placement[1] = rightMotorPlacement;
Motor* leftMotor = (Motor*)leftMotorPlacement.thing; Motor *leftMotor = (Motor *)leftMotorPlacement.thing;
leftMotor->direction = Motor::Direction::CounterClockwise; leftMotor->direction = Motor::Direction::CounterClockwise;
Motor* rightMotor = (Motor*)rightMotorPlacement.thing; Motor *rightMotor = (Motor *)rightMotorPlacement.thing;
rightMotor->direction = Motor::Direction::Clockwise; rightMotor->direction = Motor::Direction::Clockwise;
} }
void DifferentialDrive::SetTargetSpeeds(float leftSpeed, float rightSpeed) { void DifferentialDrive::SetTargetSpeeds(float leftSpeed, float rightSpeed) {
Serial.printf("motor count %d\n", this->motorCount);
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
Thing* thing = placement[motorIx].thing; Thing *thing = placement[motorIx].thing;
if (thing->type == Thing::UncontrolledMotorType) { if (thing->type == Thing::UncontrolledMotorType) {
Motor* motor = (Motor*)thing; Motor *motor = (Motor *)thing;
if (motor == nullptr) if (motor == nullptr)
continue; continue;
@ -43,9 +44,19 @@ void DifferentialDrive::SetTwistSpeed(Vector2 linear, float yaw) {
SetTwistSpeed(linear.y, yaw); SetTwistSpeed(linear.y, yaw);
} }
void DifferentialDrive::SetTwistSpeed(Vector3 linear, void DifferentialDrive::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
float yaw,
float pitch,
float roll) { float roll) {
SetTwistSpeed(linear.z, yaw); SetTwistSpeed(linear.z, yaw);
} }
Polar DifferentialDrive::GetVelocity() {
Motor *leftMotor = (Motor *)placement[0].thing;
Motor *rightMotor = (Motor *)placement[1].thing;
float leftSpeed = leftMotor->GetSpeed();
float rightSpeed = rightMotor->GetSpeed();
float speed = (leftSpeed + rightSpeed) / 2;
float direction = speed >= 0 ? 0 : 180;
float distance = abs(speed);
Polar velocity = Polar(direction, distance);
return velocity;
}

1
DifferentialDrive.h
View File

@ -56,6 +56,7 @@ class DifferentialDrive : public Propulsion {
float yaw = 0.0F, float yaw = 0.0F,
float pitch = 0.0F, float pitch = 0.0F,
float roll = 0.0F); float roll = 0.0F);
virtual Polar GetVelocity() override;
}; };
} // namespace RoboidControl } // namespace RoboidControl