def rc_alt(self, target_alt):
"""
Will send copter based off of throttle to 'target_alt'
"""
#Calculate delta t and set previous time ot current time
current_time = ((time.time() - self.timer) * 10)
delta_t = current_time - self.previous_time_alt
self.previous_time_alt = current_time
#Get error
#Changed to not use vicon positioning
self.distance = ultra.get_dist()
if self.distance > 3000:
self.distance = 200
else:
self.distance = self.distance
self.error_alt= target_alt - self.distance
logging.debug('ultrasonic distance is %f' %self.distance)
#print('ultrasonic distance is %f' %self.distance)
#Get error I
if abs(self.error_alt) < 1000:
self.I_error_alt += self.error_alt * delta_t
else:
pass
#Get error D
if self.previous_error_alt == None:
self.previous_error_alt = self.error_alt
#Check to make sure error and previous error are not the same so D is not 0
if self.error_alt != self.previous_error_alt:
self.D_error_alt = (self.error_alt-self.previous_error_alt)/delta_t
self.previous_error_alt = self.error_alt
#filter for D values
if self.D_error_alt != self.filter_value(5000,-5000,self.D_error_alt):
self.vidro.set_rc_throttle(self.vidro.base_rc_throttle)
self.P_error_alt = 0
self.I_error_alt = 0
return
#Send RC value
self.vidro.set_rc_throttle(round(self.vidro.base_rc_throttle + self.error_alt*self.alt_K_P + self.I_error_alt*self.alt_K_I + self.D_error_alt*self.alt_K_D))
return self.error_alt
ADC.setup()
#Setup Logging
logging.basicConfig(level=logging.INFO)
logger = logging.getLogger(__name__)
handler = logging.FileHandler(filename = 'logs/ultra1.log', mode = 'w')
handler.setLevel(logging.INFO)
logger.addHandler(handler)
logger.info('Starting Log')
#Run Forever
while(1):
#Take a reading
distance = ultra.get_dist()
error = 1000 - distance
#Print and Log Distance values
#print "Distance is %f" %distance
logging.info('Distance is %f' %distance)
#Print and Log Error values
#print "Error is %f" %error
logging.info('Error is %f' %error)
#Sleep so that we take readings at 20Hz to prevent terminal from stalling
time.sleep(0.05)