fafo/OpenMicroManipulator
17
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Firmware Version 1.0.4

Browse source
This commit is contained in:
0x23 2026-03-13 09:10:45 +01:00
parent 18313cc6ae
commit e13cbda4cc
26 changed files with 794 additions and 256 deletions
Download patch file Download diff file Expand all files Collapse all files

7
firmware/MotionControllerRP/src/motion_control/motion_controller.cpp
View file

@ -18,7 +18,10 @@ MotionController::MotionController(PathPlanner* path_planner) {
current_time = 0.0f;
}
bool MotionController::update(float dt, float* joint_positions, float* joint_velocities) {
bool MotionController::update(float dt,
float* joint_positions,
float* joint_velocities,
float* tool_outputs) {
// increment time counter
current_time += dt;
@ -49,6 +52,6 @@ bool MotionController::update(float dt, float* joint_positions, float* joint_vel
current_path_segment.initialized = false;
// evaluate path segment
current_path_segment.evaluate(current_time, joint_positions, joint_velocities);
current_path_segment.evaluate(current_time, joint_positions, joint_velocities, tool_outputs);
return true;
}

2
firmware/MotionControllerRP/src/motion_control/motion_controller.h
View file

@ -23,7 +23,7 @@ class MotionController {
// updates the motion controller and computes new joint positions and velocities
// after dt has passed. Ouput array must hav space for 'NUM_JOINTS' entries.
bool update(float dt, float* joint_positions, float* joint_velocities);
bool update(float dt, float* joint_positions, float* joint_velocities, float* tool_outputs);
private:
PathPlanner* path_planner;

35
firmware/MotionControllerRP/src/motion_control/path_segment.cpp
View file

@ -112,12 +112,15 @@ float MotionProfileConstAcc::evaluate(float time) const {
CartesianPathSegment::CartesianPathSegment() {
dwell_time = 0.0f;
for(int i=0; i<NUM_TOOLS; i++)
tool_outputs[i] = 0.0f;
}
CartesianPathSegment::CartesianPathSegment(const Pose6DF& start_pose,
const Pose6DF& end_pose,
const LinearAngular& target_velocity,
const LinearAngular& max_acceleration)
const LinearAngular& max_acceleration,
const float tool_outputs[NUM_TOOLS])
{
CartesianPathSegment::dwell_time = 0.0f;
CartesianPathSegment::start_pose = start_pose;
@ -134,6 +137,8 @@ CartesianPathSegment::CartesianPathSegment(const Pose6DF& start_pose,
translation_delta_normalized = translation_delta.normalized();
for(int i=0; i<NUM_TOOLS; i++)
CartesianPathSegment::tool_outputs[i] = tool_outputs[i];
/*
QuaternionF rotation_delta = (end_pose.rotation * start_pose.rotation.normalized_inverse());
@ -143,7 +148,9 @@ CartesianPathSegment::CartesianPathSegment(const Pose6DF& start_pose,
rotation_delta_axis = axis; */
}
CartesianPathSegment::CartesianPathSegment(const Pose6DF& pose, float dwell_time)
CartesianPathSegment::CartesianPathSegment(const Pose6DF& pose,
const float tool_outputs[NUM_TOOLS],
float dwell_time)
{
CartesianPathSegment::dwell_time = dwell_time;
CartesianPathSegment::start_pose = pose;
@ -156,6 +163,9 @@ CartesianPathSegment::CartesianPathSegment(const Pose6DF& pose, float dwell_time
travel_distance.linear = 0.0f;
travel_distance.angular = 0.0f;
for(int i=0; i<NUM_TOOLS; i++)
CartesianPathSegment::tool_outputs[i] = tool_outputs[i];
}
@ -209,6 +219,7 @@ JointSpacePathSegment::JointSpacePathSegment() {
JointSpacePathSegment::JointSpacePathSegment(
const float start_pos[NUM_JOINTS],
const float end_pos[NUM_JOINTS],
const float tool_outputs[NUM_TOOLS],
float duration)
{
JointSpacePathSegment::duration = duration;
@ -219,20 +230,31 @@ JointSpacePathSegment::JointSpacePathSegment(
JointSpacePathSegment::end_pos[i] = end_pos[i];
}
for(int i=0; i<NUM_TOOLS; i++) {
JointSpacePathSegment::tool_outputs[i] = tool_outputs[i];
}
initialized = true;
}
void JointSpacePathSegment::evaluate(
float time,
float joint_positions[NUM_JOINTS],
float joint_velocity[NUM_JOINTS]) const
float joint_velocity[NUM_JOINTS],
float tool_outputs[NUM_TOOLS]) const
{
float t = time*inv_duration;
float s = 1.0f-t;
// perform linear interpolation
for(int i=0; i<NUM_JOINTS; i++) {
joint_positions[i] = start_pos[i]*s + end_pos[i]*t;
joint_velocity[i] = 0;
joint_velocity[i] = 0; // velocity currently not computed (TODO)
}
// copy tool putput (tool outputs are not interpolated)
for(int i=0; i<NUM_TOOLS; i++) {
tool_outputs[i] = JointSpacePathSegment::tool_outputs[i];
}
}
@ -294,7 +316,10 @@ bool JointSpacePathSegmentGenerator::generate_next(JointSpacePathSegment& js_pat
// create joint space path segment
float duration = current_time-initial_time;
js_path_segment = JointSpacePathSegment(current_joint_pos, next_joint_pos, duration);
js_path_segment = JointSpacePathSegment(current_joint_pos,
next_joint_pos,
path_segment->tool_outputs,
duration);
// update current joint pos
for(int i=0; i<NUM_JOINTS; i++)

16
firmware/MotionControllerRP/src/motion_control/path_segment.h
View file

@ -14,6 +14,7 @@
//*** CONST *****************************************************************************
constexpr int NUM_JOINTS = 3;
constexpr int NUM_TOOLS = 2;
//*** CLASS *****************************************************************************
@ -63,11 +64,16 @@ class MotionProfileConstAcc {
class CartesianPathSegment {
public:
CartesianPathSegment();
CartesianPathSegment(const Pose6DF& start_pose,
const Pose6DF& end_pose,
const LinearAngular& velocity,
const LinearAngular& max_acceleration);
CartesianPathSegment(const Pose6DF& pose,float dwell_time);
const LinearAngular& max_acceleration,
const float tool_outputs[NUM_TOOLS]);
CartesianPathSegment(const Pose6DF& pose,
const float tool_outputs[NUM_TOOLS],
float dwell_time);
void evaluate(float time, Pose6DF& pose) const;
float get_duration() const;
@ -90,6 +96,7 @@ class CartesianPathSegment {
MotionProfileConstAcc motion_profile;
float dwell_time; // stay at start position for given duration if dwell_time > 0
float tool_outputs[NUM_TOOLS];
};
//--- JointSpacePathSegment -------------------------------------------------------------
@ -100,11 +107,13 @@ class JointSpacePathSegment {
JointSpacePathSegment();
JointSpacePathSegment(const float start_pos[NUM_JOINTS],
const float end_pos[NUM_JOINTS],
const float tool_outputs[NUM_TOOLS],
const float duration);
void evaluate(float time,
float joint_positions[NUM_JOINTS],
float joint_velocity[NUM_JOINTS]) const;
float joint_velocity[NUM_JOINTS],
float tool_outputs[NUM_TOOLS]) const;
float get_duration();
@ -116,6 +125,7 @@ class JointSpacePathSegment {
float end_pos[NUM_JOINTS];
float start_velocity[NUM_JOINTS];
float end_velocity[NUM_JOINTS];
float tool_outputs[NUM_TOOLS];
float duration;
float inv_duration;