commit 45b26fec23df99c5079db1f6fc6aab47001420f6 Author: Juan Miguel Jimeno Date: Sat Dec 17 13:01:21 2016 +0800 first commit diff --git a/Kinematics.cpp b/Kinematics.cpp new file mode 100644 index 0000000..92c8b24 --- /dev/null +++ b/Kinematics.cpp @@ -0,0 +1,136 @@ +/* + Copyright (c) 2016, Juan Jimeno + Source: http://research.ijcaonline.org/volume113/number3/pxc3901586.pdf + All rights reserved. + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + Neither the name of nor the names of its contributors may be used to + endorse or promote products derived from this software without specific + prior written permission. + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORTPPIPI (INCLUDING NEGLIGENCE OR OTHERWISE) + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. +*/ + +#include "Arduino.h" +#include "Kinematics.h" + +Kinematics::Kinematics(int motor_max_rpm, float wheel_diameter, float base_width, int pwm_bits) +{ + 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) +{ + //convert m/s to m/min + linear_vel_x_mins_ = linear_x * 60; + linear_vel_y_mins_ = linear_y * 60; + + //convert rad/s to rad/min + angular_vel_z_mins_ = angular_z * 60; + + //Vt = ω * radius + tangential_vel_ = angular_vel_z_mins_ * base_width_; + + x_rpm_ = linear_vel_x_mins_ / circumference_; + y_rpm_ = linear_vel_y_mins_ / circumference_; + tan_rpm_ = tangential_vel_ / circumference_; + + Kinematics::output rpm; + + //calculate for the target motor RPM and direction + //front-left motor + rpm.motor1 = x_rpm_ - y_rpm_ - tan_rpm_; + //rear-left motor + rpm.motor3 = x_rpm_ + y_rpm_ - tan_rpm_; + + //front-right motor + rpm.motor2 = x_rpm_ + y_rpm_ + tan_rpm_; + //rear-right motor + rpm.motor4 = x_rpm_ - y_rpm_ + tan_rpm_; + + return rpm; +} + +Kinematics::output Kinematics::getPWM(float linear_x, float linear_y, float angular_z) +{ + Kinematics::output rpm; + Kinematics::output pwm; + + rpm = getRPM(linear_x, linear_y, angular_z); + + //convert from RPM to PWM + //front-left motor + pwm.motor1 = rpmToPWM(rpm.motor1); + //rear-left motor + pwm.motor2 = rpmToPWM(rpm.motor2); + + //front-right motor + pwm.motor3 = rpmToPWM(rpm.motor3); + //rear-right motor + pwm.motor4 = rpmToPWM(rpm.motor4); + + return pwm; +} + +Kinematics::velocities Kinematics::getVelocities(int motor1, int motor2) +{ + Kinematics::velocities vel; + + double average_rpm_x = (motor1 + motor2) / 2; // RPM + //convert revolutions per minute to revolutions per second + double average_rps_x = average_rpm_x / 60; // RPS + vel.linear_x = (average_rps_x * (wheel_diameter_ * PI)); // m/s + + double average_rpm_a = (motor2 - motor1) / 2; + //convert revolutions per minute to revolutions per second + double average_rps_a = average_rpm_a / 60; + vel.angular_z = (average_rps_a * (wheel_diameter_ * PI)) / base_width_; + + return vel; +} + +Kinematics::velocities Kinematics::getVelocities(int motor1, int motor2, int motor3, int motor4) +{ + Kinematics::velocities vel; + + double average_rpm_x = (motor1 + motor2 + motor3 + motor4) / 4; // RPM + //convert revolutions per minute to revolutions per second + double average_rps_x = average_rpm_x / 60; // RPS + vel.linear_x = (average_rps_x * (wheel_diameter_ * PI)); // m/s + + double average_rpm_y = (-motor1 + motor2 + motor3 - motor4) / 4; // RPM + //convert revolutions per minute in y axis to revolutions per second + double average_rps_y = average_rpm_y / 60; // RPS + vel.linear_y = (average_rps_y * (wheel_diameter_ * PI)); // m/s + + double average_rpm_a = (-motor1 + motor2 - motor3 + motor4) / 4; + //convert revolutions per minute to revolutions per second + double average_rps_a = average_rpm_a / 60; + vel.angular_z = (average_rps_a * (wheel_diameter_ * PI)) / base_width_; + + return vel; +} + +int Kinematics::rpmToPWM(int rpm) +{ + //remap scale of target RPM vs MAX_RPM to PWM + return (double)((rpm / max_rpm_) * 255); +} diff --git a/Kinematics.h b/Kinematics.h new file mode 100644 index 0000000..2d1864a --- /dev/null +++ b/Kinematics.h @@ -0,0 +1,71 @@ +/* + Copyright (c) 2016, Juan Jimeno + Source: http://research.ijcaonline.org/volume113/number3/pxc3901586.pdf + All rights reserved. + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + Neither the name of nor the names of its contributors may be used to + endorse or promote products derived from this software without specific + prior written permission. + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORTPPIPI (INCLUDING NEGLIGENCE OR OTHERWISE) + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef KINEMATICS_H +#define KINEMATICS_H + +#include "Arduino.h" + +class Kinematics +{ + public: + struct output + { + int motor1; + int motor2; + int motor3; + int motor4; + }; + struct velocities + { + float linear_x; + float linear_y; + float angular_z; + }; + Kinematics(int motor_max_rpm, float wheel_diameter, float base_width, int pwm_bits); + velocities getVelocities(int motor1, int motor2); + velocities getVelocities(int motor1, int motor2, int motor3, int motor4); + output getRPM(float linear_x, float linear_y, float angular_z); + output getPWM(float linear_x, float linear_y, float angular_z); + int rpmToPWM(int rpm); + + private: + float linear_vel_x_mins_; + float linear_vel_y_mins_; + float angular_vel_z_mins_; + float circumference_; + float tangential_vel_; + float x_rpm_; + float y_rpm_; + float tan_rpm_; + int max_rpm_; + double wheel_diameter_; + float base_width_; + double pwm_res_; +}; + +#endif diff --git a/README.md b/README.md new file mode 100644 index 0000000..3f84b3b --- /dev/null +++ b/README.md @@ -0,0 +1,67 @@ +# kinematics +Arduino Kinematics library for differential drive(2WD, 4WD) and mecanum drive robots. + +The library requires the following robot's specification as an input: + + - Robot's maximum RPM + - Distance between wheels (base width) + - Wheel's Diameter + +## Functions + +#### 1. Kinematics::Kinematics(int motor_max_rpm, float wheel_diameter, float base_width, int pwm_bits) +Object constructor which requires the robot's specification. + - motor_max_rpm : Maximum RPM of the motor + - wheel_diameter : Robot wheel's diameter expressed in meters + - base_width : Distance between wheels + - pwm_bits : PWM resolution of the Microcontroller. Arduino Uno/Mega, Teensy is 8 bits by default. + +#### 2. output getRPM(float linear_x, float linear_y, float angular_z) +Returns a Vector of Motor RPMs from a given linear velocity in x and y axis and angular velocity in z axis using right hand rule. The returned values can be used in a PID controller as "setpoint" vs a wheel encoder's feedback expressed in RPM. + - linear_x : target linear speed of the robot in x axis (forward or reverse) expressed in m/s. + - linear_y : target linear speed of the robot in y axis (strafing left or strafing right for mecanum drive) expressed in m/s. + - angular_z : target angular speed of the robot in z axis (rotating CCW or CW) rad/sec. + +#### 3. output getPWM(float linear_x, float linear_y, float angular_z) +The same output as getRPM() function converted to a PWM value. The returned values can be used to drive motor drivers using the PWM signals. + +#### 4. velocities getVelocities(int motor1, int motor2) +This is the inverse of getRPM(). Returns linear velocities in x and y axis, and angular velocity in z axis given two measured RPMs on each motor of a 2WD robot. The returned values can be used to calculate the distance traveled in a specific axis - where distance traveled is the product of the change in velocity and change in time. + - motor1: left motor's measured RPM + - motor2: right motor's measured RPM + +*each motor's RPM value must be signed. + to signify forward direction and - to signify reverse direction of the motor. + +#### 5. velocities getVelocities(int motor1, int motor2, int motor3, int motor4) +The same output as No.4 but requires 4 measured RPMs on each motor of a 4WD robot. This can be used for both 4 wheeled differential drive and mecanum drive robots. + - motor1: front left motor's measured RPM + - motor2: front right motor's measured RPM + - motor3: front right motor's measured RPM + - motor4: front right motor's measured RPM + +*each motor's RPM value must be signed. + to signify forward direction and - to signify reverse direction of the motor. + +## Data structures +#### 1. output + Struct returned by getRPM() and getPWM used to store PWM or RPM values. + ``` + struct output{ + int motor1; + int motor2; + int motor3; + int motor4; + }; + ``` +* each returned motor RPM or PWM value is signed to signify the motor's direction. + forward direction ; - reverse direction. + +#### 2. velocities + Struct returned by getVelocities() used to store linear velocities in x and y axis, and angular velocity in z axis (right hand rule). + ``` + struct velocities + { + float linear_x; + float linear_y; + float angular_z; + }; + ``` + * linear_x and linear_y are expressed in m/s. angular_z is expressed in rad/s diff --git a/kinematics.ino b/kinematics.ino new file mode 100644 index 0000000..a54fffb --- /dev/null +++ b/kinematics.ino @@ -0,0 +1,89 @@ +/* + Copyright (c) 2016, Juan Jimeno + Source: http://research.ijcaonline.org/volume113/number3/pxc3901586.pdf + All rights reserved. + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + Neither the name of nor the names of its contributors may be used to + endorse or promote products derived from this software without specific + prior written permission. + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORTPPIPI (INCLUDING NEGLIGENCE OR OTHERWISE) + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. +*/ + +#include "Kinematics.h" + +/*Kinematics(int motor_max_rpm, float wheel_diameter, float base_width, int pwm_bits) + motor_max_rpm = motor's maximum rpm + wheel_diameter = robot's wheel diameter expressed in meters + base_width = distance between two wheels expressed in meters + pwm_bits = microcontroller's PWM pin resolution. Arduino Uno/Mega Teensy is using 8 bits(0-255) +*/ +Kinematics kinematics(90, 0.2, 0.5, 8); + +void setup() +{ + Serial.begin(9600); +} + +void loop() +{ + Kinematics::output rpm; + + /*kinematics.getRPM(linear_x, linear_y, angular_z); + linear_x = target linear velocity in x axis (right hand rule) + linear_y = target linear velocity in y axis (right hand rule) + angular_z = target angular velocity in z axis (right hand rule) + */ + //target velocities + float linear_vel_x = 1; + float linear_vel_y = 0; + float angular_vel_z = 1; + rpm = kinematics.getRPM(linear_vel_x, linear_vel_y, angular_vel_z); + + Serial.print(" FRONT LEFT MOTOR: "); + Serial.print(rpm.motor1); + + Serial.print(" FRONT RIGHT MOTOR: "); + Serial.print(rpm.motor2); + + Serial.print(" REAR LEFT MOTOR: "); + Serial.print(rpm.motor3); + + Serial.print(" REAR RIGHT MOTOR: "); + Serial.println(rpm.motor4); + + delay(5000); + + + int motor1_feedback = rpm.motor1;//in rpm + int motor2_feedback = rpm.motor2; //in rpm + int motor3_feedback = rpm.motor3; //in rpm + int motor4_feedback = rpm.motor4; //in rpm + + Kinematics::velocities vel; + vel = kinematics.getVelocities(motor1_feedback, motor2_feedback, motor3_feedback, motor4_feedback); + Serial.print(" VEL X: "); + Serial.print(vel.linear_x, 4); + + Serial.print(" VEL_Y: "); + Serial.print(vel.linear_y, 4); + + Serial.print(" ANGULAR_Z: "); + Serial.println(vel.angular_z, 4); + Serial.println(""); +}