def __init__(self, dev='/dev/ttyACM0'):
self.mbed=SerialRPC(dev, 115200)
a1=PwmOut(self.mbed, p21)
a2=PwmOut(self.mbed, p22)
b1=PwmOut(self.mbed, p23)
b2=PwmOut(self.mbed, p24)
self.m_left = Motor(a1, a2)
self.m_right = Motor(b1, b2)
self.an = AnalogIn(self.mbed, p20)
self.imu_vars = [RPCVariable(self.mbed,name) for name in imu_names]
self.enc_vars = [RPCVariable(self.mbed,name) for name in enc_names]
self.rlock=threading.Lock()
self.time = time.time()
self.duration = 0
self.enc = self.read_enc()
self.denc = [0,0]
self.cal_data_l_up = [0.0]*41
self.cal_data_l_dn = [0.0]*41
self.cal_data_r_up = [0.0]*41
self.cal_data_r_dn = [0.0]*41
self.v_desired_l = 0.0
self.v_desired_r = 0.0
self.vl_dir = 'u'
self.vr_dir = 'u'
self.v_desired_l_limit = [0.0,0.0]
self.v_desired_r_limit = [0.0,0.0]
self.shutdown = False
self.feedforward_flag = False
self.PID_l = PID(0.0002,0.00004,0.00014,0,0,100000,-100000)
self.PID_r = PID(0.0002,0.00007,0.00025,0,0,100000,-100000)
self.alpha = 0.35
self.PWM_l = 0
self.PWM_r = 0
def __init__(self, rate=10):
""" Initializes publishers and subscribers, sets initial values for vars
:param rate: the rate at which the setpoint_velocity is published
"""
assert rate > 2 # make sure rate is high enough, if new setpoint recieved within .5 seconds, robot will switch back to POSCTL
self.rate = rospy.Rate(rate) # set rate for updating
mavros.set_namespace()
self.bridge = CvBridge()
# self.pub_local_velocity_setpoint = rospy.Publisher("/mavros/setpoint_velocity/cmd_vel", TwistStamped, queue_size=1) # send velocity
self.sub_line_param = rospy.Subscriber(
"/line/param", Line,
self.line_param_cb) # get camera data from this object
self.pub_error = rospy.Publisher(
"/line/error", Vector3,
queue_size=1) # send error with this object
self.line_vel = rospy.Publisher("/mavros/setpoint_velocity/cmd_vel",
TwistStamped,
queue_size=1)
# Variables dealing with publishing setpoint
self.current_state = State()
self.sub_state = rospy.Subscriber("/mavros/state", State,
self.state_cb) # get drne state
self.offboard_point_streaming = False
# Setpoint field expressed as the desired velocity of the body-down frame
# with respect to the world frame parameterized in the body-down frame
self.velocity_setpoint = TwistStamped() # desired velocity
# while not rospy.is_shutdown() and self.current_state == None:
# pass # Wait for connection
# create PID controllers
self.controlX = PID(kp=0.01, ki=0, kd=0, smooth=False)
self.controlY = PID(kp=0.001, ki=0, kd=0.0, smooth=False)
self.controlYAW = PID(kp=0.05, ki=0, kd=0.0, smooth=False)
class Zumy:
def __init__(self, dev='/dev/ttyACM0'):
self.mbed=SerialRPC(dev, 115200)
a1=PwmOut(self.mbed, p21)
a2=PwmOut(self.mbed, p22)
b1=PwmOut(self.mbed, p23)
b2=PwmOut(self.mbed, p24)
self.m_left = Motor(a1, a2)
self.m_right = Motor(b1, b2)
self.an = AnalogIn(self.mbed, p20)
self.imu_vars = [RPCVariable(self.mbed,name) for name in imu_names]
self.enc_vars = [RPCVariable(self.mbed,name) for name in enc_names]
self.rlock=threading.Lock()
self.time = time.time()
self.duration = 0
self.enc = self.read_enc()
self.denc = [0,0]
self.cal_data_l_up = [0.0]*41
self.cal_data_l_dn = [0.0]*41
self.cal_data_r_up = [0.0]*41
self.cal_data_r_dn = [0.0]*41
self.v_desired_l = 0.0
self.v_desired_r = 0.0
self.vl_dir = 'u'
self.vr_dir = 'u'
self.v_desired_l_limit = [0.0,0.0]
self.v_desired_r_limit = [0.0,0.0]
self.shutdown = False
self.feedforward_flag = False
self.PID_l = PID(0.0002,0.00004,0.00014,0,0,100000,-100000)
self.PID_r = PID(0.0002,0.00007,0.00025,0,0,100000,-100000)
self.alpha = 0.35
self.PWM_l = 0
self.PWM_r = 0
def cmd(self, left, right): #will take ~8ms
self.rlock.acquire()
# As of Rev. F, positive command is sent to both left and right
try:
self.m_left.cmd(left)
self.m_right.cmd(right)
except SerialException:
pass
self.rlock.release()
def read_voltage(self):
self.rlock.acquire()
try:
ain=self.an.read()*3.3
except SerialException:
pass
self.rlock.release()
volt=ain*(4.99+15.8) / 4.99
return volt
def read_enc(self): #will take ~4ms
self.rlock.acquire()
try:
rval = [int(self.enc_vars[0].read()), -int(self.enc_vars[1].read())]
except SerialException:
pass
self.rlock.release()
return rval
def read_imu(self):
self.rlock.acquire()
try:
rval = [float(var.read()) for var in self.imu_vars]
except SerialException:
pass
self.rlock.release()
return rval
def update_time(self):
self.duration = time.time() - self.time
self.time = time.time()
def update_enc_denc(self): # will take ~4ms
old_denc = self.denc[:]
self.denc = [(self.read_enc()[ii] - self.enc[ii]) for ii in range(2)]
self.enc = [(self.denc[ii]+self.enc[ii]) for ii in range(2)]
self.denc = [(self.alpha*self.denc[ii]+(1-self.alpha)*old_denc[ii]) for ii in range(2)]
def update_desired(self, vl, vr):
self.PID_l.setPoint(vl)
self.PID_r.setPoint(vr)
# if vl>self.v_desired_l:
# self.vl_dir = 'u'
# else:
# self.vl_dir = 'd'
# if vr>self.v_desired_r:
# self.vr_dir = 'u'
# else:
# self.vr_dir = 'd'
# if abs(vl-self.v_desired_l)>0.1*(self.v_desired_l_limit[1]-self.v_desired_l_limit[0]):
# self.v_desired_l = vl
# self.feedforward_flag = True
# else:
# self.v_desired_l = vl
# if abs(vr-self.v_desired_r)>0.1*(self.v_desired_r_limit[1]-self.v_desired_r_limit[0]):
# self.v_desired_r = vr
# self.feedforward_flag = True
# else:
# self.v_desired_r = vr
def calibration(self):
calibration_input = range(0,20)+range(20,-1,-1)
#calibration_input_neg = [-b for b in calibration_input_pos]
calibration_repeat = 5
calibration_step = 0.01
cal_data_l = [None]*calibration_repeat
cal_data_r = [None]*calibration_repeat
self.update_enc_denc()
self.update_time()
for curr_cal_num in range(calibration_repeat):
cal_data_l[curr_cal_num] = []
cal_data_r[curr_cal_num] = []
for curr_cal_bias in calibration_input:
cal_speed = curr_cal_bias*calibration_step
self.cmd(cal_speed, -cal_speed)
time.sleep(0.032)
self.update_enc_denc()
self.update_time()
cal_data_l[curr_cal_num].append(float(self.denc[0])/self.duration)
cal_data_r[curr_cal_num].append(float(self.denc[1])/self.duration)
for curr_cal_bias in calibration_input:
cal_speed = curr_cal_bias*calibration_step
self.cmd(-cal_speed, cal_speed)
time.sleep(0.032)
self.update_enc_denc()
self.update_time()
cal_data_l[curr_cal_num].append(float(self.denc[0])/self.duration)
cal_data_r[curr_cal_num].append(float(self.denc[0])/self.duration)
cal_data_l_avg = average(cal_data_l)
cal_data_r_avg = average(cal_data_r)
self.cal_data_l_up = cal_data_l_avg[-21:]+cal_data_l_avg[1:21]
self.cal_data_l_dn = cal_data_l_avg[-21:-41:-1]+cal_data_l_avg[40:19:-1]
self.cal_data_r_up = cal_data_r_avg[-21:]+cal_data_r_avg[1:21]
self.cal_data_r_dn = cal_data_r_avg[-21:-41:-1]+cal_data_r_avg[40:19:-1]
self.v_desired_l_limit = [min(cal_data_l_avg),max(cal_data_l_avg)]
self.v_desired_r_limit = [min(cal_data_r_avg),max(cal_data_r_avg)]
def feedforward(self, desired, wheel, direction):
if wheel == 'l' and direction == 'u':
curve = self.cal_data_l_up
elif wheel == 'l' and direction == 'd':
curve = self.cal_data_l_dn
elif wheel == 'r' and direction == 'u':
curve = self.cal_data_r_up
elif wheel == 'r' and direction == 'd':
curve = self.cal_data_r_dn
x = np.linspace(-0.2, 0.2, 41)
return np.interp(desired, curve, x)
def feedback(self, desired, feedback, curr_input, wheel, direction):
if wheel == 'l' and direction == 'u':
curve = self.cal_data_l_up
elif wheel == 'l' and direction == 'd':
curve = self.cal_data_l_dn
elif wheel == 'r' and direction == 'u':
curve = self.cal_data_r_up
elif wheel == 'r' and direction == 'd':
curve = self.cal_data_r_dn
err = desired - feedback
curr_input_pos = math.floor((curr_input+0.2)/0.01)
slope = (curve[curr_input_pos+1] - curve[curr_input_pos])/0.01
new_input = curr_input + err/slope
return new_input
