def __init__(self, m_front_left, m_front_right, m_rear_left, m_rear_right): """ Takes 4 Servo motor objects """ self.sensor = AcceleroGryroSensor() self.sensing_thread = None self.m_front_left = m_front_left self.m_front_right = m_front_right self.m_rear_left = m_rear_left self.m_rear_right = m_rear_right self.all_motors = { 'front_left': self.m_front_left, 'front_right': self.m_front_right, 'rear_left': self.m_rear_left, 'rear_right': self.m_rear_right }
def __init__(self, m_front_left, m_front_right, m_rear_left, m_rear_right): """ Takes 4 Servo motor objects """ self.sensor = AcceleroGryroSensor() self.sensing_thread = None self.m_front_left = m_front_left self.m_front_right = m_front_right self.m_rear_left = m_rear_left self.m_rear_right = m_rear_right self.all_motors = {'front_left': self.m_front_left, 'front_right': self.m_front_right, 'rear_left': self.m_rear_left, 'rear_right': self.m_rear_right}
class QuadCopter(object): """ QuadCopter controller class """ short_names = {"FL": "front_left", "FR": "front_right", "RL": "rear_left", "RR": "rear_right"} MAX_ANGLE = 30 # maximum X or Y tilt angle that is allowed for the QC ANGLE_TOLERANCE = 1 # maximum angle tolerance when QC stops adjusting def __init__(self, m_front_left, m_front_right, m_rear_left, m_rear_right): """ Takes 4 Servo motor objects """ self.sensor = AcceleroGryroSensor() self.sensing_thread = None self.m_front_left = m_front_left self.m_front_right = m_front_right self.m_rear_left = m_rear_left self.m_rear_right = m_rear_right self.all_motors = {'front_left': self.m_front_left, 'front_right': self.m_front_right, 'rear_left': self.m_rear_left, 'rear_right': self.m_rear_right} def setup_ESCs(self): "setup motor controllers" print("!!!!!! MOTORS SETUP !!!!!!") print('***Disconnect ESC power') print('***then press ENTER') raw_input() for servo in self.all_motors.values(): servo.start() servo.set_speed(100) print('***Connect ESC Power') print('***Wait beep-beep') print('***then press ENTER') raw_input() for servo in self.all_motors.values(): servo.set_speed(0) print('***Wait N beep for battery cell') print('***Wait beeeeeep for ready') print('***then press ENTER') raw_input() def setup_gyro(self): "Setup accelero and gryo sensor" print("!!!!!! GYRO SETUP !!!!!!") print("Place the copter in a flat balanced surface and press enter") raw_input() self.sensor.set_offsets(-3.5, 10) print("X offset: {0}, Y offset: {1}".format( self.sensor.x_offset, self.sensor.y_offset)) def start(self): "Start quadcopter - starts all motors" self.setup_gyro() self.setup_ESCs() self.sensing_thread = Thread( target=AcceleroGryroSensor.start_sensing, args=(self.sensor, )) self.sensing_thread.start() def stop(self): "Stop quadcopter - stops all motors" self.sensor.is_sensing = False self.sensing_thread.join() for servo in self.all_motors.values(): servo.stop() def set_speed(self, new_speed, target_motor_names): """ Sets speed of one or multiple motors. Example calls:: set_speed(50, "FL,FR") set_speed(40, "ALL") set_speed(75, "RL") FL = Front Left FR = Front Right RL = Rear Left RR = Rear Right ALL = All motors """ if "ALL" == target_motor_names: for servo in self.all_motors.values(): servo.set_speed(new_speed) else: m_names = target_motor_names.split(',') for m_name in m_names: real_name = self.short_names[m_name] servo = self.all_motors[real_name] servo.set_speed(new_speed) def __str__(self): ret = "Rot X: {0}, Rot Y: {1}, Offset X,Y: {2},{3}\n\n".format( self.sensor.rotation_x, self.sensor.rotation_y, self.sensor.x_offset, self.sensor.y_offset) for servo in self.all_motors.values(): ret += str(servo) + "\n" return ret def hover(self): "Maintain current position and altitude" self.set_speed(50, "ALL") def maintain_vector(self, average_speed, x_angle, y_angle): """ Tries to maintain the given angles and average speed. Adjusts motor speeds and attempts to reach the given angles and speed """ # get current x and y angles current_x = self.sensor.rotation_x current_y = self.sensor.rotation_y new_x = x_angle - current_x new_y = y_angle - current_y FLx, FLy, FRx, FRy, RLx, RLy, RRx, RRy = [0, 0, 0, 0, 0, 0, 0, 0] adjust_x = False adjust_y = False if abs(new_x) > self.ANGLE_TOLERANCE: adjust_x = True step = abs(int(new_x/2)) if new_x > 0: # +(FL & RL), -(FR, RR) FLx = average_speed + step RLx = average_speed + step FRx = average_speed - step RRx = average_speed - step elif new_x < 0: # -(FL & RL), +(FR, RR) FLx = average_speed - step RLx = average_speed - step FRx = average_speed + step RRx = average_speed + step if abs(new_y) > self.ANGLE_TOLERANCE: adjust_y = True step = abs(int(new_y/2)) if new_y > 0: # +(RR & RL), -(FR, FL) FLy = average_speed - step FRy = average_speed - step RLy = average_speed + step RRy = average_speed + step elif new_y < 0: # -(RR & RL), +(FR, FL) FLy = average_speed + step FRy = average_speed + step RLy = average_speed - step RRy = average_speed - step if adjust_x and adjust_y: self.m_front_left.set_speed((FLx + FLy) / 2) self.m_front_right.set_speed((FRx + FRy) / 2) self.m_rear_left.set_speed((RLx + RLy) / 2) self.m_rear_right.set_speed((RRx + RRy) / 2) elif adjust_x: self.m_front_left.set_speed(FLx) self.m_front_right.set_speed(FRx) self.m_rear_left.set_speed(RLx) self.m_rear_right.set_speed(RRx) elif adjust_y: self.m_front_left.set_speed(FLy) self.m_front_right.set_speed(FRy) self.m_rear_left.set_speed(RLy) self.m_rear_right.set_speed(RRy)
class QuadCopter(object): """ QuadCopter controller class """ short_names = { "FL": "front_left", "FR": "front_right", "RL": "rear_left", "RR": "rear_right" } MAX_ANGLE = 30 # maximum X or Y tilt angle that is allowed for the QC ANGLE_TOLERANCE = 1 # maximum angle tolerance when QC stops adjusting def __init__(self, m_front_left, m_front_right, m_rear_left, m_rear_right): """ Takes 4 Servo motor objects """ self.sensor = AcceleroGryroSensor() self.sensing_thread = None self.m_front_left = m_front_left self.m_front_right = m_front_right self.m_rear_left = m_rear_left self.m_rear_right = m_rear_right self.all_motors = { 'front_left': self.m_front_left, 'front_right': self.m_front_right, 'rear_left': self.m_rear_left, 'rear_right': self.m_rear_right } def setup_ESCs(self): "setup motor controllers" print("!!!!!! MOTORS SETUP !!!!!!") print('***Disconnect ESC power') print('***then press ENTER') raw_input() for servo in self.all_motors.values(): servo.start() servo.set_speed(100) print('***Connect ESC Power') print('***Wait beep-beep') print('***then press ENTER') raw_input() for servo in self.all_motors.values(): servo.set_speed(0) print('***Wait N beep for battery cell') print('***Wait beeeeeep for ready') print('***then press ENTER') raw_input() def setup_gyro(self): "Setup accelero and gryo sensor" print("!!!!!! GYRO SETUP !!!!!!") print("Place the copter in a flat balanced surface and press enter") raw_input() self.sensor.set_offsets(-3.5, 10) print("X offset: {0}, Y offset: {1}".format(self.sensor.x_offset, self.sensor.y_offset)) def start(self): "Start quadcopter - starts all motors" self.setup_gyro() self.setup_ESCs() self.sensing_thread = Thread(target=AcceleroGryroSensor.start_sensing, args=(self.sensor, )) self.sensing_thread.start() def stop(self): "Stop quadcopter - stops all motors" self.sensor.is_sensing = False self.sensing_thread.join() for servo in self.all_motors.values(): servo.stop() def set_speed(self, new_speed, target_motor_names): """ Sets speed of one or multiple motors. Example calls:: set_speed(50, "FL,FR") set_speed(40, "ALL") set_speed(75, "RL") FL = Front Left FR = Front Right RL = Rear Left RR = Rear Right ALL = All motors """ if "ALL" == target_motor_names: for servo in self.all_motors.values(): servo.set_speed(new_speed) else: m_names = target_motor_names.split(',') for m_name in m_names: real_name = self.short_names[m_name] servo = self.all_motors[real_name] servo.set_speed(new_speed) def __str__(self): ret = "Rot X: {0}, Rot Y: {1}, Offset X,Y: {2},{3}\n\n".format( self.sensor.rotation_x, self.sensor.rotation_y, self.sensor.x_offset, self.sensor.y_offset) for servo in self.all_motors.values(): ret += str(servo) + "\n" return ret def hover(self): "Maintain current position and altitude" self.set_speed(50, "ALL") def maintain_vector(self, average_speed, x_angle, y_angle): """ Tries to maintain the given angles and average speed. Adjusts motor speeds and attempts to reach the given angles and speed """ # get current x and y angles current_x = self.sensor.rotation_x current_y = self.sensor.rotation_y new_x = x_angle - current_x new_y = y_angle - current_y FLx, FLy, FRx, FRy, RLx, RLy, RRx, RRy = [0, 0, 0, 0, 0, 0, 0, 0] adjust_x = False adjust_y = False if abs(new_x) > self.ANGLE_TOLERANCE: adjust_x = True step = abs(int(new_x / 2)) if new_x > 0: # +(FL & RL), -(FR, RR) FLx = average_speed + step RLx = average_speed + step FRx = average_speed - step RRx = average_speed - step elif new_x < 0: # -(FL & RL), +(FR, RR) FLx = average_speed - step RLx = average_speed - step FRx = average_speed + step RRx = average_speed + step if abs(new_y) > self.ANGLE_TOLERANCE: adjust_y = True step = abs(int(new_y / 2)) if new_y > 0: # +(RR & RL), -(FR, FL) FLy = average_speed - step FRy = average_speed - step RLy = average_speed + step RRy = average_speed + step elif new_y < 0: # -(RR & RL), +(FR, FL) FLy = average_speed + step FRy = average_speed + step RLy = average_speed - step RRy = average_speed - step if adjust_x and adjust_y: self.m_front_left.set_speed((FLx + FLy) / 2) self.m_front_right.set_speed((FRx + FRy) / 2) self.m_rear_left.set_speed((RLx + RLy) / 2) self.m_rear_right.set_speed((RRx + RRy) / 2) elif adjust_x: self.m_front_left.set_speed(FLx) self.m_front_right.set_speed(FRx) self.m_rear_left.set_speed(RLx) self.m_rear_right.set_speed(RRx) elif adjust_y: self.m_front_left.set_speed(FLy) self.m_front_right.set_speed(FRy) self.m_rear_left.set_speed(RLy) self.m_rear_right.set_speed(RRy)