Mirrors_Framework/g2
1
0
Fork 0
mirror of https://github.com/synthetos/g2.git synced 2026-09-20 17:55:43 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Fixes for join <-> axis mapping

Browse Source
This commit is contained in:
Rob Giseburt
2020-01-14 16:45:35 -06:00
parent 3d724cf7a2
commit ded5de3e9d
2 changed files with 18 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
g2core/kinematics.cpp
+16 -20
View File
@@ -107,21 +107,17 @@ struct CartesianKinematics : KinematicsBase<axes, motors> {
float steps_per_unit[motors];
float offset[motors];
bool needs_sync_encoders = true; // if true, we need to update the steps_offset
uint8_t joint_map[joints]; // for each joint, which motor or -1
int8_t motor_map[motors]; // for each motor, which joint it maps from
float joint_position[joints];
void configure(const float new_steps_per_unit[motors], const int8_t motor_map[motors]) override
void configure(const float new_steps_per_unit[motors], const int8_t new_motor_map[motors]) override
{
for (uint8_t joint = 0; joint < joints; joint++) {
joint_map[joint] = -1;
joint_position[joint] = 0;
}
for (uint8_t motor = 0; motor < motors; motor++) {
auto joint = motor_map[motor];
if (joint >= 0) {
joint_map[joint] = motor;
}
motor_map[motor] = new_motor_map[motor];
steps_per_unit[motor] = new_steps_per_unit[motor];
}
needs_sync_encoders = true;
@@ -131,13 +127,14 @@ struct CartesianKinematics : KinematicsBase<axes, motors> {
const float end_velocity, const float segment_time, float steps[motors]) override
{
// joint == axis in cartesian kinematics
for (uint8_t joint = 0; joint < joints; joint++) {
uint8_t motor = joint_map[joint];
if (motor == -1) { continue; }
for (uint8_t motor = 0; motor < motors; motor++) {
int8_t joint = motor_map[motor];
if (joint == -1) {
continue;
}
// if (needs_sync_encoders) {
// put the difference in offset
offset[motor] += (joint_position[joint] - position[joint]);
// put the difference in offset
offset[motor] += (joint_position[joint] - position[joint]);
// }
steps[motor] = (target[joint]+offset[motor]) * steps_per_unit[motor];
@@ -163,13 +160,12 @@ struct CartesianKinematics : KinematicsBase<axes, motors> {
}
for (uint8_t motor = 0; motor < motors; motor++) {
best_steps_per_unit[motor] = -1.0;
}
int8_t joint = motor_map[motor];
if (joint == -1) {
continue;
}
// joint == motor in cartesian kinematics
for (uint8_t joint = 0; joint < joints; joint++) {
auto axis = joint; // cartesian, baby!
auto motor = joint_map[joint];
if (motor == -1) { continue; }
auto axis = joint; // it's cartesian, baby!
// If this motor has a better (or the only) resolution, then we use this motor's value
if (best_steps_per_unit[axis] < steps_per_unit[motor]) {
@@ -182,7 +178,7 @@ struct CartesianKinematics : KinematicsBase<axes, motors> {
}
joint_position[joint] = position[joint];
} // for joint
}
}
void sync_encoders() override { needs_sync_encoders = true; }
g2core/stepper.cpp
+2 -3
View File
@@ -1006,12 +1006,11 @@ stat_t st_set_ma(nvObj_t *nv)
uint8_t external_axis = nv->value_int;
#if (AXES == 9)
uint8_t remap_axis[9] = { 0,1,2,6,7,8,3,4,5 };
#else
uint8_t remap_axis[6] = { 0,1,2,3,4,5 };
#endif
nv->value_int = remap_axis[nv->value_int];
#endif
ritorno(set_integer(nv, st_cfg.mot[_motor(nv->index)].motor_map, 0, AXES));
nv->value_int = external_axis;
kn_config_changed();
return(STAT_OK);
}