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

Using Angle::PI

Browse Source
This commit is contained in:
Pascal Serrarens 2024-01-09 12:28:39 +01:00
parent edf264c302
commit a21485857d
2 changed files with 7 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
DifferentialDrive.cpp
View File

@ -1,4 +1,5 @@
#include "DifferentialDrive.h" #include "DifferentialDrive.h"
#include "VectorAlgebra/Angle.h"
#include "VectorAlgebra/FloatSingle.h" #include "VectorAlgebra/FloatSingle.h"
#include <math.h> #include <math.h>
@ -22,8 +23,6 @@ DifferentialDrive::DifferentialDrive(Motor *leftMotor, Motor *rightMotor,
rightMotor->position.distance = distance; rightMotor->position.distance = distance;
} }
#include <Arduino.h>
void DifferentialDrive::SetTargetSpeeds(float leftSpeed, float rightSpeed) { void DifferentialDrive::SetTargetSpeeds(float leftSpeed, float rightSpeed) {
for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
Motor *motor = motors[motorIx]; Motor *motor = motors[motorIx];
@ -53,15 +52,13 @@ void DifferentialDrive::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
SetTwistSpeed(linear.z, yaw); SetTwistSpeed(linear.z, yaw);
} }
#include <Arduino.h>
Polar DifferentialDrive::GetVelocity() { Polar DifferentialDrive::GetVelocity() {
Motor *leftMotor = motors[0]; Motor *leftMotor = motors[0];
Motor *rightMotor = motors[1]; Motor *rightMotor = motors[1];
float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second
float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second
leftSpeed = leftSpeed * wheelDiameter * M_PI; // in meters per second leftSpeed = leftSpeed * wheelDiameter * Angle::PI; // in meters per second
rightSpeed = rightSpeed * wheelDiameter * M_PI; // in meters per second rightSpeed = rightSpeed * wheelDiameter * Angle::PI; // in meters per second
float speed = (leftSpeed + rightSpeed) / 2; float speed = (leftSpeed + rightSpeed) / 2;
float direction = speed >= 0 ? 0.0F : 180.0F; float direction = speed >= 0 ? 0.0F : 180.0F;
@ -75,24 +72,14 @@ float DifferentialDrive::GetAngularVelocity() {
Motor *rightMotor = motors[1]; Motor *rightMotor = motors[1];
float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second
float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second
leftSpeed = leftSpeed * wheelDiameter * M_PI; // in meters per second leftSpeed = leftSpeed * wheelDiameter * Angle::PI; // in meters per second
rightSpeed = rightSpeed * wheelDiameter * M_PI; // in meters per second rightSpeed = rightSpeed * wheelDiameter * Angle::PI; // in meters per second
float angularSpeed = (rightSpeed - leftSpeed) / 2; float angularSpeed = (rightSpeed - leftSpeed) / 2;
float angularDistance = wheelSeparation / 2 * M_PI; float angularDistance = wheelSeparation / 2 * Angle::PI;
float rotationsPerSecond = angularSpeed / angularDistance; float rotationsPerSecond = angularSpeed / angularDistance;
float degreesPerSecond = 360 * rotationsPerSecond; float degreesPerSecond = 360 * rotationsPerSecond;
float angularVelocity = degreesPerSecond; float angularVelocity = degreesPerSecond;
// Serial.print(leftSpeed);
// Serial.print(" - ");
// Serial.println(rightSpeed);
// Serial.print(angularSpeed);
// Serial.print(" ");
// Serial.print(angularDistance);
// Serial.print(" ");
// Serial.println(rotationsPerSecond);
return angularVelocity; return angularVelocity;
} }

2
VectorAlgebra

@ -1 +1 @@
Subproject commit 96bcc4a405e4df95b7f54ba99fcd2c60079e0ae6 Subproject commit 78ed576e16bbe479209c0b0927378b27d31dd5f4