first commit
Juan Miguel Jimeno
commit
45b26fec23
|
|
@ -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);
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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("");
|
||||
}
|
||||
Loading…
Reference in New Issue