import("youbot_master_rtt")

# Set up Youbot component
loadComponent("Youbot","youbot_master::YouBotMasterComponent")
setActivity("Youbot",0.001,HighestPriority,ORO_SCHED_RT)

# Set up Timer component
loadComponent("Timer","OCL::TimerComponent")
setActivity("Timer",0.01,HighestPriority,ORO_SCHED_RT)

# Set Youbot ethercat interface
Youbot.ifname = "eth0"

# Create connections
connectPorts("Youbot.watchdog","Timer.timeout")
var ConnPolicy cp
cp.transport = 3
cp.name_id = "odometry"
stream("Youbot.odometry",cp)
cp.name_id = "cmd_vel"
stream("Youbot.cmd_twist",cp)

# Set up timers
Youbot.state_timer = 0
Youbot.command_timer = 1;
Timer.startTimer(Youbot.state_timer,0.1)
Timer.startTimer(Youbot.command_timer,0.1)

# Configuring components
Youbot.configure()

# Starting components
Timer.start()
Youbot.start()
Youbot.setControllerMode("Velocity")