a2nr
/
kinematics
mirror of https://github.com/linorobot/kinematics.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

fixed equation

pull/2/head
Juan Jimeno 2018-11-23 01:10:51 +08:00
parent 0dcef71f80
commit a58d1f8ffb
2 changed files with 29 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
Kinematics.cpp
View File

@ -28,13 +28,14 @@
#include "Arduino.h" #include "Arduino.h"
#include "Kinematics.h" #include "Kinematics.h"
Kinematics::Kinematics(int motor_max_rpm, float wheel_diameter, float base_width, int pwm_bits) Kinematics::Kinematics(int motor_max_rpm, float wheel_diameter, float fr_wheels_dist, float lr_wheels_dist, int pwm_bits):
wheel_diameter_(wheel_diameter),
circumference_(PI * wheel_diameter_),
max_rpm_(motor_max_rpm),
fr_wheels_dist_(fr_wheels_dist),
lr_wheels_dist_(lr_wheels_dist),
pwm_res_ (pow(2, pwm_bits) - 1)
{ {
wheel_diameter_ = wheel_diameter;
circumference_ = PI * wheel_diameter_;
max_rpm_ = motor_max_rpm;
base_width_ = base_width;
pwm_res_ = pow(2, pwm_bits) - 1;
} }
Kinematics::output Kinematics::getRPM(float linear_x, float linear_y, float angular_z) Kinematics::output Kinematics::getRPM(float linear_x, float linear_y, float angular_z)
@ -47,7 +48,7 @@ Kinematics::output Kinematics::getRPM(float linear_x, float linear_y, float angu
angular_vel_z_mins_ = angular_z * 60; angular_vel_z_mins_ = angular_z * 60;
//Vt = ω * radius //Vt = ω * radius
tangential_vel_ = angular_vel_z_mins_ * base_width_; tangential_vel_ = angular_vel_z_mins_ * lr_wheels_dist_;
x_rpm_ = linear_vel_x_mins_ / circumference_; x_rpm_ = linear_vel_x_mins_ / circumference_;
y_rpm_ = linear_vel_y_mins_ / circumference_; y_rpm_ = linear_vel_y_mins_ / circumference_;
@ -94,15 +95,15 @@ Kinematics::velocities Kinematics::getVelocities(int motor1, int motor2)
{ {
Kinematics::velocities vel; Kinematics::velocities vel;
double average_rpm_x = (motor1 + motor2) / 2; // RPM float average_rpm_x = (float)(motor1 + motor2) / 2; // RPM
//convert revolutions per minute to revolutions per second //convert revolutions per minute to revolutions per second
double average_rps_x = average_rpm_x / 60; // RPS float average_rps_x = average_rpm_x / 60; // RPS
vel.linear_x = (average_rps_x * (wheel_diameter_ * PI)); // m/s vel.linear_x = (average_rps_x * circumference_); // m/s
double average_rpm_a = (motor2 - motor1) / 2; float average_rpm_a = (float)(motor2 - motor1) / 2;
//convert revolutions per minute to revolutions per second //convert revolutions per minute to revolutions per second
double average_rps_a = average_rpm_a / 60; float average_rps_a = average_rpm_a / 60;
vel.angular_z = (average_rps_a * (wheel_diameter_ * PI)) / base_width_; vel.angular_z = (average_rps_a * circumference_) / (lr_wheels_dist_ / 2);
return vel; return vel;
} }
@ -111,20 +112,20 @@ Kinematics::velocities Kinematics::getVelocities(int motor1, int motor2, int mot
{ {
Kinematics::velocities vel; Kinematics::velocities vel;
double average_rpm_x = (motor1 + motor2 + motor3 + motor4) / 4; // RPM float average_rpm_x = (float)(motor1 + motor2 + motor3 + motor4) / 4; // RPM
//convert revolutions per minute to revolutions per second //convert revolutions per minute to revolutions per second
double average_rps_x = average_rpm_x / 60; // RPS float average_rps_x = average_rpm_x / 60; // RPS
vel.linear_x = (average_rps_x * (wheel_diameter_ * PI)); // m/s vel.linear_x = (average_rps_x * circumference_); // m/s
double average_rpm_y = (-motor1 + motor2 + motor3 - motor4) / 4; // RPM float average_rpm_y = (float)(-motor1 + motor2 + motor3 - motor4) / 4; // RPM
//convert revolutions per minute in y axis to revolutions per second //convert revolutions per minute in y axis to revolutions per second
double average_rps_y = average_rpm_y / 60; // RPS float average_rps_y = average_rpm_y / 60; // RPS
vel.linear_y = (average_rps_y * (wheel_diameter_ * PI)); // m/s vel.linear_y = (average_rps_y * circumference_); // m/s
double average_rpm_a = (-motor1 + motor2 - motor3 + motor4) / 4; float average_rpm_a = (float)(-motor1 + motor2 - motor3 + motor4) / 4;
//convert revolutions per minute to revolutions per second //convert revolutions per minute to revolutions per second
double average_rps_a = average_rpm_a / 60; float average_rps_a = average_rpm_a / 60;
vel.angular_z = (average_rps_a * (wheel_diameter_ * PI)) / base_width_; vel.angular_z = (average_rps_a * circumference_) / ((fr_wheels_dist_ / 2) + (lr_wheels_dist_ / 2));
return vel; return vel;
} }
@ -132,5 +133,5 @@ Kinematics::velocities Kinematics::getVelocities(int motor1, int motor2, int mot
int Kinematics::rpmToPWM(int rpm) int Kinematics::rpmToPWM(int rpm)
{ {
//remap scale of target RPM vs MAX_RPM to PWM //remap scale of target RPM vs MAX_RPM to PWM
return (((double) rpm / (double) max_rpm_) * pwm_res_); return (((float) rpm / (float) max_rpm_) * pwm_res_);
} }

9
Kinematics.h
View File

@ -46,7 +46,7 @@ class Kinematics
float linear_y; float linear_y;
float angular_z; float angular_z;
}; };
Kinematics(int motor_max_rpm, float wheel_diameter, float base_width, int pwm_bits); Kinematics(int motor_max_rpm, float wheel_diameter, float fr_wheels_dist, float lr_wheels_dist, int pwm_bits);
velocities getVelocities(int motor1, int motor2); velocities getVelocities(int motor1, int motor2);
velocities getVelocities(int motor1, int motor2, int motor3, int motor4); velocities getVelocities(int motor1, int motor2, int motor3, int motor4);
output getRPM(float linear_x, float linear_y, float angular_z); output getRPM(float linear_x, float linear_y, float angular_z);
@ -63,9 +63,10 @@ class Kinematics
float y_rpm_; float y_rpm_;
float tan_rpm_; float tan_rpm_;
int max_rpm_; int max_rpm_;
double wheel_diameter_; float wheel_diameter_;
float base_width_; float fr_wheels_dist_;
double pwm_res_; float lr_wheels_dist_;
float pwm_res_;
}; };
#endif #endif