Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

hotfix for lizard issue #59 #157

Merged
merged 2 commits into from
Aug 26, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 14 additions & 14 deletions field_friend/hardware/chain_axis.py
Original file line number Diff line number Diff line change
Expand Up @@ -153,44 +153,44 @@ def __init__(self, robot_brain: rosys.hardware.RobotBrain, *,
bool {name}_resetting = false

when {name}_ref_r_is_referencing and {name}_ref_t_stop_enabled and {name}_ref_t.level == 1 then
{name}.stop();
{name}.stop()
end

when {name}_ref_r_is_referencing and !{name}_ref_t_stop_enabled and {name}_ref_t.level == 0 then
{name}.stop();
{name}.stop()
end

when {name}_ref_l_is_referencing and {name}_ref_t_stop_enabled and {name}_ref_t.level == 1 then
{name}.stop();
{name}.stop()
end

when {name}_ref_l_is_referencing and !{name}_ref_t_stop_enabled and {name}_ref_t.level == 0 then
{name}.stop();
{name}.stop()
end

when {name}_ref_r_return and {name}_ref_t.level == 1 then
{name}_ref_t_stop_enabled = true;
{name}_ref_t_stop_enabled = true
end

when {name}_ref_r_return and {name}_ref_t_stop_enabled and {name}_ref_t.level == 0 then
{name}.stop();
{name}_ref_r_return = false;
{name}_ref_t_stop_enabled = false;
{name}.stop()
{name}_ref_r_return = false
{name}_ref_t_stop_enabled = false
end

when {name}_ref_l_return and {name}_ref_t.level == 1 then
{name}_ref_t_stop_enabled = true;
{name}_ref_t_stop_enabled = true
end

when {name}_ref_l_return and {name}_ref_t_stop_enabled and {name}_ref_t.level == 0 then
{name}.stop();
{name}_ref_l_return = false;
{name}_ref_t_stop_enabled = false;
{name}.stop()
{name}_ref_l_return = false
{name}_ref_t_stop_enabled = false
end

when {name}_resetting and {name}_ref_t.level == 0 then
{name}.stop();
{name}_resetting = false;
{name}.stop()
{name}_resetting = false
end
''')
core_message_fields = [
Expand Down
32 changes: 16 additions & 16 deletions field_friend/hardware/tornado.py
Original file line number Diff line number Diff line change
Expand Up @@ -147,36 +147,36 @@ def __init__(self, robot_brain: rosys.hardware.RobotBrain, *,
{name}_motor_z.reversed = {'true' if is_z_reversed else 'false'}
{name}_motor_turn.reversed = {'true' if is_turn_reversed else 'false'}
{name}_end_top = {expander.name + "." if end_stops_on_expander or end_top_pin_expander and expander else ""}Input({end_top_pin})
{name}_end_top.inverted = true;
{name}_end_top.inverted = true
{name}_end_bottom = {expander.name + "." if end_stops_on_expander or end_bottom_pin_expander and expander else ""}Input({end_bottom_pin})
{name}_end_bottom.inverted = true;
{name}_end_bottom.inverted = true
{name}_ref_motor = {expander.name + "." if end_stops_on_expander or ref_motor_pin_expander and expander else ""}Input({ref_motor_pin})
{name}_ref_motor.inverted = true;
{name}_ref_motor.inverted = true
{name}_ref_gear = {expander.name + "." if end_stops_on_expander or ref_gear_pin_expander and expander else ""}Input({ref_gear_pin})
{name}_ref_gear.inverted = false;
{name}_ref_gear.inverted = false
{name}_ref_knife_stop = {expander.name + "." if end_stops_on_expander or ref_knife_stop_pin_expander and expander else ""}Input({ref_knife_stop_pin})
{name}_ref_knife_stop.inverted = false;
{name}_ref_knife_stop.inverted = false
{name}_ref_knife_ground = {expander.name + "." if end_stops_on_expander or ref_knife_ground_pin_expander and expander else ""}Input({ref_knife_ground_pin})
{name}_ref_knife_ground.inverted = true;
{name}_ref_knife_ground.inverted = true
{name}_z = {expander.name + "." if motors_on_expander and expander else ""}MotorAxis({name}_motor_z, {name + "_end_bottom" if is_z_reversed else name + "_end_top"}, {name + "_end_top" if is_z_reversed else name + "_end_bottom"})

bool {name}_is_referencing = false;
bool {name}_ref_motor_enabled = false;
bool {name}_ref_gear_enabled = false;
bool {name}_is_referencing = false
bool {name}_ref_motor_enabled = false
bool {name}_ref_gear_enabled = false
when {name}_ref_motor_enabled and {name}_is_referencing and {name}_ref_motor.level == 0 then
{name}_motor_turn.speed(0);
{name}_motor_turn.speed(0)
end
when {name}_ref_gear_enabled and {name}_is_referencing and {name}_ref_gear.level == 1 then
{name}_motor_turn.speed(0);
{name}_motor_turn.speed(0)
end
bool {name}_knife_ground_enabled = false;
bool {name}_knife_stop_enabled = false;
bool {name}_knife_ground_enabled = false
bool {name}_knife_stop_enabled = false
when {name}_knife_ground_enabled and {name}_ref_knife_ground.level == 1 then
{name}_motor_z.off();
{name}_motor_z.off()
end
when {name}_knife_stop_enabled and {name}_ref_knife_stop.level == 1 then
en3.off();
{name}_knife_stop_enabled = false;
en3.off()
{name}_knife_stop_enabled = false
end
''') # tornado axis references in positive direction, in contrast to all other axis
core_message_fields = [
Expand Down
Loading