Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Sign in
Toggle navigation
Menu
Open sidebar
Jan Koniarik
schpin-robot-lib
Commits
92104ff1
Commit
92104ff1
authored
Dec 22, 2021
by
Jan Koniarik
Browse files
added leg config and udpates to joint config
parent
15a3f564
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/Schpin/Walker.hs
View file @
92104ff1
...
...
@@ -89,13 +89,26 @@ data ExtendedLink = ExtendedLink
}
deriving
Show
data
ActuatorType
=
Lewan
{
lw_goto_p
::
Maybe
Double
,
lw_goto_i
::
Maybe
Double
,
lw_goto_d
::
Maybe
Double
,
lw_vel_p
::
Maybe
Double
,
lw_vel_i
::
Maybe
Double
,
lw_vel_d
::
Maybe
Double
data
ActuatorType
=
Lewan
{
lw_goto_p
::
Maybe
Double
,
lw_goto_i
::
Maybe
Double
,
lw_goto_d
::
Maybe
Double
,
lw_vel_p
::
Maybe
Double
,
lw_vel_i
::
Maybe
Double
,
lw_vel_d
::
Maybe
Double
}
|
LewanTest
|
Dynamixel
{
dn_goto_p
::
Maybe
Double
,
dn_goto_i
::
Maybe
Double
,
dn_goto_d
::
Maybe
Double
,
dn_vel_p
::
Maybe
Double
,
dn_vel_i
::
Maybe
Double
}
|
LewanTest
{
lwt_goto_p
::
Maybe
Double
,
lwt_goto_i
::
Maybe
Double
,
lwt_goto_d
::
Maybe
Double
,
lwt_vel_p
::
Maybe
Double
,
lwt_vel_i
::
Maybe
Double
,
lwt_vel_d
::
Maybe
Double
}
|
Dynamixel
{
dn_goto_p
::
Maybe
Int
,
dn_goto_i
::
Maybe
Int
,
dn_goto_d
::
Maybe
Int
,
dn_vel_p
::
Maybe
Int
,
dn_vel_i
::
Maybe
Int
}
deriving
(
Show
);
data
Actuator
=
Actuator
{
ac_name
::
String
,
...
...
@@ -137,6 +150,7 @@ data ExtendedJoint = ExtendedJoint
data
Leg
j
=
Leg
{
leg_name
::
LegName
,
root_joint
::
j
,
neutral_dir
::
Axis
}
deriving
Show
...
...
src/Schpin/Walker/Config.hs
View file @
92104ff1
...
...
@@ -5,9 +5,10 @@ module Schpin.Walker.Config where
import
Data.ByteString.Lazy.UTF8
(
toString
)
import
Data.Maybe
(
catMaybes
)
import
Data.Scientific
import
Data.Foldable
(
toList
)
import
Data.Text
(
pack
)
import
Data.YAML
import
Numeric.Units.Dimensional.Prelude
(
radian
,
(
/~
))
import
Numeric.Units.Dimensional.Prelude
(
radian
,
(
/~
)
,
meter
)
import
Schpin.Package
import
Schpin.Shared
import
Schpin.Walker
...
...
@@ -15,7 +16,7 @@ import Schpin.Walker
acTypeToString
::
ActuatorType
->
String
acTypeToString
t
=
case
t
of
Lewan
{}
->
"LEWAN"
LewanTest
->
"LEWAN_TEST"
LewanTest
{}
->
"LEWAN_TEST"
Dynamixel
{}
->
"DYNMX"
writeControlConfig
::
ExtendedWalker
->
PackageConfig
->
IO
()
...
...
@@ -38,9 +39,28 @@ writeControlConfig walker cfg = do
[
pack
"joint_state"
.=
pack
"/control/joint_states"
,
pack
"diag_freq"
.=
freq
,
pack
"diag_topic"
.=
pack
"/diagnostics"
,
pack
"legs"
.=
legsp
,
pack
"servos"
.=
servos
]
legsp
::
Node
()
legsp
=
mapping
$
(
\
l
->
(
pack
(
"leg/"
++
leg_name
l
))
.=
cfg
l
)
<$>
(
legs
walker
)
where
cfg
::
ExtendedLeg
->
Node
()
cfg
Leg
{
root_joint
,
neutral_dir
}
=
mapping
$
[
pack
"neutral_angles"
.=
((
\
x
->
x
/~
radian
)
<$>
neutral_angles
root_joint
)
,
pack
"neutral_dir"
.=
((
\
x
->
x
/~
meter
)
<$>
toList
neutral_dir
)
]
neutral_angles
::
ExtendedJoint
->
[
Angle
]
neutral_angles
root_joint
=
catMaybes
$
mapJoints
f
root_joint
where
f
::
ExtendedJoint
->
Maybe
Angle
f
ExtendedJoint
{
joint
=
Joint
{
jtype
}}
=
case
jtype
of
RevoluteJoint
{
default_angle
}
->
Just
$
default_angle
_
->
Nothing
servos
::
Node
()
servos
=
mapping
...
...
@@ -75,7 +95,14 @@ writeControlConfig walker cfg = do
,
opt_arg
"vel_i"
$
lw_vel_i
atype
,
opt_arg
"vel_d"
$
lw_vel_d
atype
]
LewanTest
->
[]
LewanTest
{}
->
catMaybes
[
opt_arg
"goto_p"
$
lwt_goto_p
atype
,
opt_arg
"goto_i"
$
lwt_goto_i
atype
,
opt_arg
"goto_d"
$
lwt_goto_d
atype
,
opt_arg
"vel_p"
$
lwt_vel_p
atype
,
opt_arg
"vel_i"
$
lwt_vel_i
atype
,
opt_arg
"vel_d"
$
lwt_vel_d
atype
]
Dynamixel
{}
->
catMaybes
[
opt_arg
"goto_p"
$
dn_goto_p
atype
,
opt_arg
"goto_i"
$
dn_goto_i
atype
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment