Skip to content

Commit

Permalink
working D1
Browse files Browse the repository at this point in the history
  • Loading branch information
@Johannes-Thiel
Johannes-Thiel committed Sep 9, 2024
1 parent c3c76d4 commit fd54f8b
Show file tree
Hide file tree
Showing 4 changed files with 25 additions and 28 deletions.
8 changes: 5 additions & 3 deletions config/f15_config_f15/hardware.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@
'min_position': -0.11,
'max_position': 0.11,
'axis_offset': 0.115,
'reversed_direction': False,
},
'z_axis': {
'version': 'axis_d1',
Expand All @@ -38,9 +39,10 @@
'profile_acceleration': 500000,
'profile_velocity': 40000,
'profile_deceleration': 500000,
'min_position': 0.230,
'max_position': 0,
'axis_offset': 0.01,
'min_position': -0.230,
'max_position': 0.0,
'axis_offset': -0.01,
'reversed_direction': True,
},
'estop': {
'name': 'estop',
Expand Down
4 changes: 2 additions & 2 deletions field_friend/hardware/axis.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,14 +57,14 @@ async def try_reference(self) -> bool:
return True

def compute_steps(self, position: float) -> int:
"""Compute the number of steps to move the y axis to the given position.
"""Compute the number of steps to move the axis to the given position.
The position is given in meters.
"""
return int((position + self.axis_offset) * self.steps_per_m) * (-1 if self.reversed_direction else 1)

def compute_position(self, steps: int) -> float:
return steps / self.steps_per_m - self.axis_offset * (-1 if self.reversed_direction else 1)
return steps / self.steps_per_m * (-1 if self.reversed_direction else 1) - self.axis_offset

@property
def position(self) -> float:
Expand Down
35 changes: 14 additions & 21 deletions field_friend/hardware/axis_D1.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@ def __init__(self, robot_brain: rosys.hardware.RobotBrain, *,
profile_velocity: int = 20,
profile_acceleration: int = 200,
profile_deceleration: int = 400,
reverse_direction: bool = False,


** kwargs
Expand All @@ -32,7 +33,7 @@ def __init__(self, robot_brain: rosys.hardware.RobotBrain, *,
"""
self.name = name
self.statusword: int = 0
self._position: int = 0
self.steps: int = 0
self.velocity: int = 0

# flags of the Statusword for more information refer to the CANopen standard and D1 manual
Expand All @@ -56,7 +57,7 @@ def __init__(self, robot_brain: rosys.hardware.RobotBrain, *,
{self.name}_motor.homing_acceleration = {homing_acceleration}
{self.name}_motor.switch_search_speed = {homing_velocity}
{self.name}_motor.zero_search_speed = {homing_velocity}
{self.name}_motor.profile_acceleration = {profile_acceleration}
{self.name}_motor.profile_acceleration = {profile_acceleration}
{self.name}_motor.profile_deceleration = {profile_deceleration}
{self.name}_motor.profile_velocity = {profile_velocity}
''')
Expand All @@ -75,24 +76,18 @@ def __init__(self, robot_brain: rosys.hardware.RobotBrain, *,
min_position=min_position,
axis_offset=axis_offset,
reference_speed=homing_velocity,
steps_per_m=0,
reversed_direction=False,
steps_per_m=D1_STEPS_P_M,
reversed_direction=reverse_direction,
**kwargs)

@property
def position(self) -> float:
return (self._position / D1_STEPS_P_M) - self.axis_offset

async def stop(self):
pass

async def move_to(self, position: float, speed: int | None = None) -> None:
if self.is_referenced:
await self.robot_brain.send(f'{self.name}_motor.profile_position({self._compute_steps(position)});')
if not self.is_referenced:
self.log.error(f'AxisD1 {self.name} is not refernced')
return
while abs(self.position - position) > 0.02:
await super().move_to(position)
await self.robot_brain.send(f'{self.name}_motor.profile_position({self.compute_steps(position)});')
while (abs(self.position - position)) > 0.005:

await rosys.sleep(0.1)

async def enable_motor(self):
Expand All @@ -104,26 +99,27 @@ async def enable_motor(self):
async def reset_error(self):
if self.fault:
await self.robot_brain.send(f'{self.name}_motor.reset()')
else: self.log.error(f'AxisD1 {self.name} is not in fault state')
else:
self.log.error(f'AxisD1 {self.name} is not in fault state')

async def try_reference(self) -> bool:
if not self._valid_status():
await self.enable_motor()
if self.is_referenced:
self.log.error(f'AxisD1 {self.name} is already referenced')
else:
#due to some timing issues, the homing command is sent twice
# due to some timing issues, the homing command is sent twice
await self.robot_brain.send(f'{self.name}_motor.home()')
await self.robot_brain.send(f'{self.name}_motor.home()')
return self.is_referenced

async def speed_Mode(self, speed: int):
#due to some timing issues, the speed command is sent twice
# due to some timing issues, the speed command is sent twice
await self.robot_brain.send(f'{self.name}_motor.profile_velocity({speed});')
await self.robot_brain.send(f'{self.name}_motor.profile_velocity({speed});')

def handle_core_output(self, time: float, words: list[str]) -> None:
self._position = int(words.pop(0))
self.steps = int(words.pop(0))
self.velocity = int(words.pop(0))
self.statusword = int(words.pop(0))
self.is_referenced = int(words.pop(0)) == 1
Expand All @@ -132,9 +128,6 @@ def handle_core_output(self, time: float, words: list[str]) -> None:
def _valid_status(self) -> bool:
return self.ready_to_switch_on and self.switched_on and self.operation_enabled and self.quick_stop

def _compute_steps(self, position: float) -> int:
return int(abs(position + self.axis_offset) * D1_STEPS_P_M)

def _split_statusword(self) -> None:
self.ready_to_switch_on = ((self.statusword >> 0) & 1) == 1
self.switched_on = ((self.statusword >> 1) & 1) == 1
Expand Down
6 changes: 4 additions & 2 deletions field_friend/hardware/field_friend_hardware.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -121,7 +121,8 @@ def __init__(self) -> None:
profile_deceleration=config_hardware['y_axis']['profile_deceleration'],
max_position=config_hardware['y_axis']['max_position'],
min_position=config_hardware['y_axis']['min_position'],
axis_offset=config_hardware['y_axis']['axis_offset'],)
axis_offset=config_hardware['y_axis']['axis_offset'],
reverse_direction=config_hardware['y_axis']['reversed_direction'],)
elif config_hardware['y_axis']['version'] == 'chain_axis':
y_axis = ChainAxisHardware(robot_brain,
expander=expander,
Expand Down Expand Up @@ -220,7 +221,8 @@ def __init__(self) -> None:
profile_deceleration=config_hardware['z_axis']['profile_deceleration'],
max_position=config_hardware['z_axis']['max_position'],
min_position=config_hardware['z_axis']['min_position'],
axis_offset=config_hardware['z_axis']['axis_offset'],)
axis_offset=config_hardware['z_axis']['axis_offset'],
reverse_direction=config_hardware['z_axis']['reversed_direction'],)
elif config_hardware['z_axis']['version'] == 'tornado':
z_axis = TornadoHardware(robot_brain,
expander=expander,
Expand Down

0 comments on commit fd54f8b

Please sign in to comment.