Commit 92104ff1 authored by Jan Koniarik's avatar Jan Koniarik
Browse files

added leg config and udpates to joint config

parent 15a3f564
......@@ -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
......
......@@ -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
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment