def __init__(self):
rospy.init_node('hardware_node', anonymous=True)
rospy.on_shutdown(self.shutdown_handler)
# Setup subscribers
#self.command_sub = rospy.Subscriber('/gait_cmd', GaitCmdMsg, self.command_callback) #nama topic, tipe msg, interrupt
self.setpoint_sub = rospy.Subscriber('/RF2_pos_controller/command',
Float64,
self.command_callback_RF2)
self.setpoint_sub = rospy.Subscriber('/RF3_pos_controller/command',
Float64,
self.command_callback_RF3)
# Setup timer events
# every 1/10 secs execute self.do_something
self.frequency = 10
self.process_timer = rospy.Timer(
rospy.Duration.from_sec(1.0 / self.frequency),
self.do_something) #bikin timer interrupt, ngelakuin do something
self.setpoint_RF2 = 0
self.serpoint_RF3 = 0
# Find a connected ODrive (this will block until you connect one)
print("finding an odrive...")
self.my_drive = odrive.find_any()
# Find an ODrive that is connected on the serial port /dev/ttyUSB0
self.my_drive.axis0.controller.pos_setpoint = -1 * 0.167356116281 * (
24 * 8192 / 6.28)
self.my_drive.axis0.encoder.set_linear_count(-1 * 0.167356116281 *
(24 * 8192 / 6.28))
self.my_drive.axis1.controller.pos_setpoint = -1 * 0.167356116281 * (
24 * 8192 / 6.28)
self.my_drive.axis1.encoder.set_linear_count(-6.29654472 *
(24 * 8192 / 6.28))
# Calibrate motor and wait for it to finish
print("starting calibration...")
start_liveplotter(lambda: [
self.my_drive.axis0.encoder.pos_estimate * 360 / 8192, -1 * self.
my_drive.axis1.encoder.pos_estimate * 360 / 8192
])
self.my_drive.axis0.encoder.is_ready = True
self.my_drive.axis1.encoder.is_ready = True
self.my_drive.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
self.my_drive.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
import time
import math
import numpy
import time
# Find a connected ODrive (this will block until you connect one)
print("finding an odrive...")
my_drive = odrive.find_any()
dump_errors(my_drive,True)
# Find an ODrive that is connected on the serial port /dev/ttyUSB0
#my_drive = odrive.find_any("serial:/dev/ttyUSB0")
#use the plotter to monitor the current and position
# start_liveplotter(lambda: [(my_drive.axis0.motor.current_control.Iq_measured*100),my_drive.axis0.encoder.count_in_cpr])
start_liveplotter(lambda: [(my_drive.axis0.controller.input_pos),my_drive.axis0.encoder.pos_estimate])
# Calibrate motor and wait for it to finish
print("starting calibration...")
my_drive.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
while my_drive.axis0.current_state != AXIS_STATE_IDLE:
time.sleep(0.1)
my_drive.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
time.sleep(5)
# EXAMPLE
# Read and print the voltage property
print("Bus voltage is " + str(my_drive.vbus_voltage) + "V")
def pos_plotter(odrvAxis):
utils.start_liveplotter(lambda: [
odrvAxis.encoder.pos_estimate, odrvAxis.controller.pos_setpoint,
odrvAxis.controller.pos_setpoint - odrvAxis.encoder.pos_estimate
])
import odrive
from odrive.enums import *
from odrive.utils import start_liveplotter
import time
import math
import numpy
# Find a connected ODrive (this will block until you connect one)
print("finding an odrive...")
my_drive = odrive.find_any()
# Find an ODrive that is connected on the serial port /dev/ttyUSB0
#my_drive = odrive.find_any("serial:/dev/ttyUSB0")
#use the plotter to monitor the current and position
start_liveplotter(lambda: [(my_drive.axis0.motor.current_control.Iq_measured *
100), my_drive.axis0.encoder.count_in_cpr])
# Calibrate motor and wait for it to finish
print("starting calibration...")
my_drive.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
while my_drive.axis0.current_state != AXIS_STATE_IDLE:
time.sleep(0.1)
my_drive.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
# EXAMPLE
# Read and print the voltage property
print("Bus voltage is " + str(my_drive.vbus_voltage) + "V")
# EXAMPLE
# change a value, just assign to the property
