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)