aplotter_ros2's Introduction

aplotter_ros2

Pen Plotter using ODrive motor control and ROS2

This repository is dependend on a ROS2 CAN ODrive package found at https://github.com/camrbuss/ros2_odrive_can

Inverse Kinematics

Implement in C++ with a(t) and b(t) coming in as discrete measurements, x'(t) and y'(t) being set by the user, and a'(t) and b'(t) being sent to the motor controllers.

  float j = -L3 * (std::pow(L1, 2) - std::pow(L2, 2) - std::pow(a, 2) + 2.0 * a * b - std::pow(b, 2));
  float k = A1 + std::acos((1.0 / 2.0) * (std::pow(L1, 2) - std::pow(L2, 2) + std::pow(a - b, 2)) / (L1 * (-a + b)));
  float l = 2.0 * L1 * sqrt(1 - 1.0 / 4.0 * std::pow(std::pow(L1, 2) - std::pow(L2, 2) + std::pow(a - b, 2), 2) / (std::pow(L1, 2) * std::pow(a - b, 2))) * std::pow(a - b, 2);
  float f = j * std::sin(k) / l;
  float g = j * std::cos(k) / l;

  this->a_vel_setpoint_ = x_vel_ - ((-1.0f + f) * y_vel_ / g);
  this->b_vel_setpoint_ = x_vel_ - ((f * y_vel_) / g);

Microsoft Xbox 360 Wired Controller for Linux Interface

The controls needed to achieve velocity control are sequential. The left bumper correspond to axis 1 and the right bumber to axis 0.

ID	Button Combination	Function
1	<Left/Right> Bumper + X	E-Stop
2	<Left/Right> Bumper + Y	Calibrate Axis
3	<Left/Right> Bumper + A	Request Closed Loop Control
4	<Left/Right> Bumper + B	Home Axis
5	<Left/Right> Bumper + Back	Clear Axis Error
6	Menu	Start Command Loop
7	<Left/Right> Bumper + D-Pad Left	Set Velocity Control Mode
8	D-Pad Up	Increment Max Velocity
9	D-Pad Down	Decrement Max Velocity
10	Left Joystick	X, Y Velocity Control
11	Left Joystick Button	Toggle Pen