def run_physics(self,dt):
#self.boat_position_x = self.boat_position_x + dt
self.force_list = []
torque_list = [] #not used
self.wind_abs[1] = WindDirSlider['value']
self.wind_abs[0] = WindSpeedSlider['value']
self.calculate_relative_wind()
if (Auto_Sail["indicatorValue"] == 1):
self.Sail_1_rel_angle = boat_handling.desired_sail_angle(self.wind_rel[1]) #doesn't (yet) care about wind speed
self.Sail_2_rel_angle = boat_handling.desired_sail_angle(self.wind_rel[1])
CourseSlider['value'] = self.Sail_1_rel_angle
else:
self.Sail_1_rel_angle = CourseSlider['value']
self.Sail_2_rel_angle = CourseSlider['value']
self.Sail_1_abs_angle = self.Sail_1_rel_angle + self.boat_orientation[0] #+ 90
self.Sail_2_abs_angle = self.Sail_2_rel_angle + self.boat_orientation[0] #+ 90
#Sail_Angle_Of_Attack = within_360(self,self.sail_1_AOA)
#print self.wind_rel[0]
sail_force = self.sail_force(self.Sail_1_abs_angle,0)
self.sail_1_force_dir = sail_force[1]
#self.sail_force_dir = sail_force[0] #+WindDirSlider['value']
self.force_list.append((sail_force[0],within_360(self,sail_force[1]),"Sail 1"))
self.force_list.append((sail_force[0],within_360(self,sail_force[1]),"Sail 2"))
#print "boat angle %.2f boat force %.2f" % (force_list[0][0],force_list[0][1])
#self.boat_position[1] = self.boat_position[1] + dt * .5 * self.WingCDs(Sail_Angle_Of_Attack)[1]
#print "Force Forward %.2f Force Sideways %.2f" % (sail_force[1] * math.cos(math.radians(sail_force[0])),(sail_force[1] * math.sin(math.radians(sail_force[0])) ))
self.generate_water_forces()
#self.generate_rudder_forces()
self.process_forces()
self.process_accelerations()
self.process_velocities(dt)
self.process_motions(dt)
self.process_torques(dt)
#print self.boat_acceleration_matrix
print "---------------------"
pulseLength /= 4096 # 12 bits of resolution
print "%d us per bit" % pulseLength
pulse *= 1000
pulse /= pulseLength
pwm.setPWM(channel, 0, pulse)
pwm.setPWMFreq(60) # Set frequency to 60 Hz
servo_history = 0
backlash_servo_out = 0
servo_hysteresis = 0
while (True):
# Change speed of continuous servo on channel O
wind_angle = (read_wind() - 4) % 360 #calibration
sail_angle = boat_handling.desired_sail_angle(wind_angle)
servo_out = int(serv_center + sail_angle * servo_units_per_degree)
if (servo_out < servo_history):
servo_hysteresis = 2
print 'bl'
if servo_hysteresis > 0:
backlash_servo_out = servo_out - 5
servo_hysteresis -= 1
else:
backlash_servo_out = servo_out
servo_history = servo_out
print "wind_angle %.2f sail_angle %.2f difference %.2f servo_out %s backlash_servo_out %s hysteresis %s" % (wind_angle,sail_angle % 360, (wind_angle - sail_angle) % 360 ,servo_out,backlash_servo_out,servo_hysteresis)
pwm.setPWM(0, 0, int(backlash_servo_out))
time.sleep(.1)
