def __init__(self, sensor, dOuts): # PID self.pidParams = PIDParams(input=0, output=0, setpoint=0) self.pid = PID(params=self.pidParams, kP=0, kI=0, kD=0, direction=PID.DIRECT, debugEnabled=True) self.pid.sampleTime = self.Ts # ms self.pid.setTunings(2.5, 0.005, 0) self.pid.setOutputLimits(0, 100) self.pid.debugEnabled = False # Resistors self.dOuts = dOuts # DigitalOutputs595 instance self.lowerResistor = 0 # Desired power (%) self.upperResistor = 0 # Desired power (%) self.segmentPWM = 0 # Current segment of the PWM period self.lastPWM = 0 # Last time of the PWM execution # Temperature self.sensor = sensor # Max6675 sensor self.temps = [30.0] * self.windowSize # Temperature sliding window self.tempsHead = 0 # Pointer to the head of the sliding window self.temperature = 30.0 # Average temperature self.lastRead = 0 self.t = 0 # Status self.on = False self.ready = False self.starting = 0 self.turnOFF()
def PID_Test(): import pylab from numpy import array from PID import PID height = [] quad = Quadcopter("001", "1.0") height_pid = PID(40.0, 0.5, 0.0) height_pid.set_point = 5.0 ## loop = 0 ## while (loop < 500): ## control = height_pid.update(quad.height) ## quad.set_motors(1000+control) ## quad.update_all(0.02) ## print control,quad.height ## loop += 1 quad.set_motors(1000) quad.update_all(1) #print quad.height #quad.set_motors(1500) #quad.update_all(1) #print quad.height for i in range(0, 1000): quad.set_motors(height_pid.update(quad.height) + 1000) quad.update_all(0.01) #print quad.height height.append(quad.height) if i == 500: height_pid.set_point = 2.0 Aheight = array(height) pylab.plot(Aheight) pylab.show()
def __init__(self, driveSystem, gpsDriver): self.target_heading = 30.0 self.pid = PID(P=0.005, I=0.0, D=0.0, current_time=None) self.gpsDriver = gpsDriver self.driveSystem = driveSystem self.waypoints = [] self.target_lat = 0.0 self.target_lon = 0.0 self.prev_lat = 0.0 self.prev_long = 0.0 self.waypoint_index = 0 self.forward_throttle = 1.0 self.navigating = False #self.calculate_distance(47.730759, -117.280971, 47.691172, -117.283890) #print(self.get_bearing(47.703185, -117.280966, 47.704542, -117.278197)) thread = Thread(target=self.HeadingCtrl) thread.daemon = True thread.start() autoNavThread = Thread(target=self.AutoNavigate) autoNavThread.daemon = True autoNavThread.start()
def __init__(self, emu): self.predActive = 0 self.next_stop = -1 self.actualLimit = 0 self.pid = PID(P=70, I=0.3, D=4.0, current_time=0) self.pid.SetPoint = 30.0 self.pid.setSampleTime(0.1) self.pid.setWindup(20.0) self.pid_poz = PID(P=70.0, I=10.0, D=60.0, current_time=0) self.pid_poz.setSampleTime(0.1) self.pid_poz.setWindup(25.0) self.pid_poz.setPoint = 0 self.predIter = 0 self.predLen = 0 #Data collection: self.tv = [] self.sv = [] self.vv = [] self.zv = [] self.Pv = [] self.vlimv = [] self.predReal = [] self.predSets = []
def __init__(self): self.status = "" rospy.init_node('forward', anonymous=False) self.pid = { # 'az': PID(T * 0.001, (1.0/180.0), 0, 0, "Orientation"), 'az': PID(T / 1000.0, 2.0, 0.0, 1.0, "Orientation"), 'z': PID(T / 1000.0, 0.007, 0.000, 0.001, "Altitude"), 'x': PID(T / 1000.0, 0.007, 0.000, 0.001, "X Position"), 'y': PID(T / 1000.0, 0.007, 0.000, 0.001, "Y Position") } self.navdata = Navdata() self.odom = Odometry() self.mark = Marker() self.rate = rospy.Rate(10) self.pose = {'x': 1.0, 'y': -1.0, 'z': 1.0, 'w': 0} self.pubTakeoff = rospy.Publisher("ardrone/takeoff", Empty, queue_size=10) self.pubLand = rospy.Publisher("ardrone/land", Empty, queue_size=10) self.pubLand = rospy.Publisher("ardrone/land", Empty, queue_size=10) self.pubCommand = rospy.Publisher('cmd_vel', Twist, queue_size=10) self.command = Twist() rospy.Subscriber("/ardrone/navdata", Navdata, self.cbNavdata) # rospy.Subscriber("/ardrone/odometry", Odometry, self.cbOdom) rospy.Subscriber("/ground_truth/state", Odometry, self.cbOdom) rospy.Subscriber("/visualization_marker", Marker, self.cbMarker) self.state_change_time = rospy.Time.now() rospy.on_shutdown(self.SendLand)
def executar(): pid = PID(KP, KI, KD) pid.SetPoint = 0 botao = Button() while not botao.any(): if parece_verde(): confirme_verde() if sensor_frente.distance_centimeters < 5: Sound.beep() print('\n -- VI OBSTACULO! -- \n') ultrapassar_obstaculo() erro = get_valor_sensor('direita') - get_valor_sensor('esquerda') pid.update(erro) correcao = pid.output giro_dir = sat(TP - correcao) giro_esq = sat(TP + correcao) esq.run_forever(speed_sp=giro_esq * (-1)) dir.run_forever(speed_sp=giro_dir * (-1)) esq.stop() dir.stop()
def start(self): # Init request flag. self.request = False self.controlling = False self.alive = True # Start a timer for PID. self.timer = QtCore.QElapsedTimer() self.timer.start() # Init PID. self.pid = PID(timestamp=self.timer.elapsed()) # Load tasks from NI-MAX. self.task_ai = nidaqmx.system.storage.persisted_task.PersistedTask('TaskTemp').load() self.task_co = nidaqmx.system.storage.persisted_task.PersistedTask('TaskPow').load() self.task_do = nidaqmx.system.storage.persisted_task.PersistedTask('TaskDir').load() # Start tasks. try: self.task_ai.start() self.task_do.control(nidaqmx.constants.TaskMode.TASK_RESERVE) self.task_co.control(nidaqmx.constants.TaskMode.TASK_RESERVE) self.task_do.start() self.task_co.start() except Exception as err: print(err)
def __init__(self): self.bus = smbus.SMBus(1) self.pwm = PwmBoard(self.bus, 0x40) self.motor1 = Motor(self.pwm, 0) self.motor2 = Motor(self.pwm, 1) self.motor3 = Motor(self.pwm, 2) self.motor4 = Motor(self.pwm, 3) self.mpu6050 = MPU6050(self.bus, 0x68) self.mpu6050.start() self.networkhandler = NetworkHandler(self) self.running = True self.pgain = 5.0 self.igain = 0.01 self.dgain = 1.5 self.roll_pid = PID(self.pgain, self.igain, self.dgain) self.pitch_pid = PID(self.pgain, self.igain, self.dgain) self.yaw_pid = PID(self.pgain, self.igain, self.dgain) self.wantedroll = 0.0 self.wantedpitch = 0.0 self.throttle = 0.0 time.sleep(1)
def __init__(self, detect_model, tello, frame_width=512, skip_frames=20, confidence=0.4, output_video=None): self.detect_model = detect_model self.tello = tello self.frame_width = frame_width self.skip_frames = skip_frames self.confidence = confidence self.output_video = output_video self.frame = None self.init_area = None self.foward_pid = PID(setPoint=1.0, sample_time=0.5, P=30, I=5, D=10) self.rotate_pid = PID(setPoint=0.5, sample_time=0.5, P=50, I=8, D=15) self.frame_pool = Queue() self.stopEvent = threading.Event() self.horizontal_speed = 0 self.foward_speed = 0 self.accelerator = 0 self.rotate = 0 # meters self.distance = 0.5 self.degree = 10 self.vedio_thread = threading.Thread(target=self._videoLoop, args=()) self.vedio_thread.start() self.sending_command_thread = threading.Thread( target=self._sendingCommand)
def __init__(self): self.epsilon = 0.9 self.pid_parameter = {'kp': 0.5, 'ki': 0.5, 'kd': 0.5} self.episodes = 500 self.take_off_procedure = 50 * 4 self.env_max_steps = 1000 # to be checked self.alpha = 0.01 self.gamma = 0.9 self.number_of_actions = _get_number_of_actions() self.render_every = 100 # Initialize PID controller, environment and states self.pid = PID(self.pid_parameter['kp'], self.pid_parameter['ki'], self.pid_parameter['kd']) self.env = GameOfDrone() states = self.env.reset() # Discretize the state space get_discrete_states = StateDiscrete( states.dist_to_target, states.distance_vector.optimal_heading - states.angle, states.total_velocity) self.discrete_states = get_discrete_states.discretize() # Initialize the Q table self.Q = dict() for a in Actions: self.Q[a] = np.zeros(get_discrete_states.state_buckets) self.policy = np.zeros(get_discrete_states.state_buckets, dtype=Actions) for i in range(get_discrete_states.state_buckets[0]): for j in range(get_discrete_states.state_buckets[1]): for k in range(get_discrete_states.state_buckets[2]): self.policy[i, j, k] = Actions.ENGINES_OFF
def __init__(self, sensorConversionThread): ''' Initializes the basics of the vehicle @param sensorConversionThread - the conversion thread ''' self.sensorConversionThread = sensorConversionThread self.pwm = Adafruit_PCA9685.PCA9685() self.pwm.set_pwm_freq(Constants.PWM_SENSOR_FREQ) self.velocityPID = PID(Constants.VELOCITY_PID_P, Constants.VELOCITY_PID_I, Constants.VELOCITY_PID_D, Constants.VELOCITY_PID_WINDUP) self.steeringAnglePID = PID(Constants.STEERING_ANGLE_PID_P, Constants.STEERING_ANGLE_PID_I, Constants.STEERING_ANGLE_PID_D, Constants.STEERING_ANGLE_PID_WINDUP) self.velocityDuration = -0.0 self.steeringDuration = -0.0 self.prevTotalStripCount = 0.0 self.totalStripCount = 0.0 # TODO: Wall Follow PID self.tabs = 0 self.totalUpdateTime = 0.0 self.prevTime = time.time() self.currentTime = time.time() self.updateRate = gcd(Constants.VELOCITY_PID_UPDATE_RATE, Constants.STEERING_PID_UPDATE_RATE)
def __init__(self, imuConnectString): # go to default connect string if no string was specified if imuConnectString == None: imuConnectString ='tcp://localhost:10001/arduinoIMU/arduinoIMUData' self.m1Duty = 0 self.m2Duty = 0 #largest absolute value that the motors can be set to self.dutyMax = 5000 # control mode will be either Duty or Velocity depending on last command sent self.controlMode = 'Duty' self.canMeasureWheelVelocities = True self.pidControllerR = PID(20,0,0,0,0,0,0) self.pidControllerL = PID(-20,0,0,0,0,0,0) # try to find and connect to NRF IMU server to get wheel velocities try: self.imuGateway = RRN.ConnectService(imuConnectString) except: print "Couldn't find NRF IMU server, unable to accept velocity commands!" self.canMeasureWheelVelocities = False self._lock = threading.RLock()
def callback(self, array): if len(array.data) > 0: # Body frame: z point ups; Marker frame: z points down (both right handed frames). self.Tx = array.data[1]; self.Ty = array.data[0]; self.Tz = array.data[2]; self.yaw = array.data[3]; # Access current yaw in degrees. ## Choose shortest setpoint, based on current yaw translation ## if self.first_yaw: if self.yaw > 0: self.pid_yaw = PID(-1.5, 0, -.4, .8, 0); else: self.pid_yaw = PID(-1.5, 0, -.4, .8, 0); self.first_yaw = False; self.update(); # Update PID outputs at each array callback. else: self.vel_cmd.twist.linear.x = 0; self.vel_cmd.twist.linear.y = 0; self.vel_cmd.twist.linear.z = 0; self.vel_cmd.twist.angular.z = 0; self.pub_vel.publish(self.vel_cmd);
def PID_Test(): import pylab from numpy import array from PID import PID height = [] quad = Quadcopter("001","1.0") height_pid = PID(40.0,0.5,0.0) height_pid.set_point = 5.0 ## loop = 0 ## while (loop < 500): ## control = height_pid.update(quad.height) ## quad.set_motors(1000+control) ## quad.update_all(0.02) ## print control,quad.height ## loop += 1 quad.set_motors(1000) quad.update_all(1) #print quad.height #quad.set_motors(1500) #quad.update_all(1) #print quad.height for i in range(0,1000): quad.set_motors(height_pid.update(quad.height)+1000) quad.update_all(0.01) #print quad.height height.append(quad.height) if i == 500: height_pid.set_point = 2.0 Aheight = array(height) pylab.plot(Aheight) pylab.show()
def setUp(self): self.pidParams = PIDParams(input=70, output=0, setpoint=90) self.pid = PID(params=self.pidParams, kP=5.0, kI=0.25, kD=1.15, direction=PID.DIRECT, debugEnabled=True)
def __init__(self, y: float, right: bool, end_x: float, speed=0.2, omega=0.2): self.pid = PID(0.4, 1, 1.5, 0.2) self.y = y self.right = right self.end_x = end_x self.speed = speed self.omega = omega self.t_last = None
def __init__(self, name, motor, counter): self.name = name self.motor = motor self.counter = counter self.pid = PID(0.5, 1.4, 0.8, Integrator_max=300, Integrator_min=-300) self.lastUpdateTime = time.time() self.lastCount = self.counter.count
def __init__(self): super().__init__() self.plant = Plant("opc.tcp://192.168.1.23:4840") self.plant.start() self.pid_valv1 = PID() self.pid_valv2 = PID() self.is_running = False self.u = [0, 0]
def calcTheForce(self): # расчет сил dx = self.XYZ_d[0] - self.XYZ[0] dy = self.XYZ_d[1] - self.XYZ[1] self.ABG_d[2] = np.arctan2(dy, dx) Fx_d = 0 if -0.05 <= self.ABG_d[2] - self.ABG[2] <= 0.05: if dx < 0 and dy < 0: if dx > dy: Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0], self.XYZ[0]) * -1 else: Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1], self.XYZ[1]) * -1 if dx > 0 and dy > 0: if dx > dy: Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0], self.XYZ[0]) else: Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1], self.XYZ[1]) if dx < 0 and dy > 0: if dx > dy: Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0], self.XYZ[0]) * -1 else: Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1], self.XYZ[1]) if dx > 0 and dy < 0: if dx > dy: Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0], self.XYZ[0]) else: Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1], self.XYZ[1]) * -1 # перемещения вдоль игрек if -0.5 <= dx <= 0.5 and (self.XYZ[1] <= self.XYZ_d[1]): Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1], self.XYZ[1]) if (-0.5 <= dx <= 0.5) and (self.XYZ[1] > self.XYZ_d[1]): Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1], self.XYZ[1]) * -1 # перемещения вдоль икс if -0.5 <= dy <= 0.5 and (self.XYZ[0] <= self.XYZ_d[0]): Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0], self.XYZ[0]) if (-0.5 <= dy <= 0.5) and (self.XYZ[0] > self.XYZ_d[0]): Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0], self.XYZ[0]) * -1 else: Fx_d = 0 Fy_d = 0 Fz_d = PIDc.pidUpdate(self.stab_ctrl, self.XYZ_d[2], self.XYZ[2]) # повороты: рыскание, тангаж и крен Mx_d = PIDc.pidUpdate(self.rotation_ctrl, self.ABG_d[2], self.ABG[2]) # вокруг Z, рыскание My_d = PIDc.pidUpdate(self.pitchroll_ctrl, self.ABG_d[1], self.ABG[1]) Mz_d = PIDc.pidUpdate(self.pitchroll_ctrl, self.ABG_d[0], self.ABG[0]) forceD = [Fx_d, Fy_d, Fz_d, Mx_d, My_d, Mz_d] s_force = vrep.simxPackFloats(forceD) vrep.simxSetStringSignal(self.clientID, "ForceD", s_force, vrep.simx_opmode_oneshot)
def car_control(self): self.find_all() ####----------------Use MPC ------------------- # predict = MPC(x_start,y_start,yaw,v) if ((abs(self.yaw))> deg2rad(86)): a = 7 elif rad2deg(abs(self.yaw)) > deg2rad(84) : a = 5 elif rad2deg(abs(self.yaw)) > deg2rad(78) : a = 3 elif rad2deg(abs(self.yaw)) > deg2rad(75) : a = 2 else: a = -6 ++i print(i) self.dt = self.dt+ 1/10 self.speed = self.speed + a * self.dt ###Test #print(self.dt,'dt') print(rad2deg(self.yaw),'yaw') print(self.speed,'speed') ### ----------------Use PID-------------------- kp = .53 ki = 0.005 kd = 0.0003 """40km/h""" # kp = .92 # ki = 0.001 # kd = 0.5 # if (self.DIRECTION == 'right'): # self.cte = abs(self.cte) +5 # else: # self.cte = -abs(self.cte) - 5 control = PID(kp,ki,kd) control.update_error(self.cte) steer = control.total_error() self.angle_steer = steer # print(self.cte,'cte') #####SPEED#### # speed = 30 # self.speed = speed # print(steer,'steer') # print(self.angle_steer,'total error') print(steer,'fianl steer') self.speed_pub.publish(self.speed) self.steer_pub.publish(steer)
class LidarSteerManager: def __init__(self): self.pid = PID(LIDAR_PID_P, LIDAR_PID_I, LIDAR_PID_D) self.pid.SetPoint = 0 self.pid.setSampleTime(LIDAR_PID_SAMPLE_RATE) self.priority = "0" def steer(self, lidarData): error = self.error(lidarData) self.pid.update(error) speed = .5 + .5 * (1 - abs(self.pid.output)) return MAX_STEER * self.pid.output,speed,self.priority def error(self, points): FUTURE_WEIGHT = .65 PRESENT_WEIGHT = 1 - FUTURE_WEIGHT diff_present = self.subErrorAtAngle(points, 90 - 5) diff_future = self.subErrorAtAngle(points, 45) diff = diff_present * PRESENT_WEIGHT + diff_future * FUTURE_WEIGHT return LidarSteerManager.cappedMultiply(diff, LIDAR_STEER_MULT) def subErrorAtAngle(self, lidarPoints, angle): # angle = degrees left = self.getPerpDistance(lidarPoints, -angle) right = self.getPerpDistance(lidarPoints, angle) left, right = LidarSteerManager.normalize(left, right) # In case of emergency drop right.... if(left < .75 or right < .75): self.priority = "3" else: self.priority = "1" sub_error = right - left return sub_error def getPerpDistance(self, lidarPoints, angle): # Perpendicular index = LidarHelper.angleToLidarIndex(angle) vectorDistance = lidarPoints[index] return abs(LidarSteerManager.getXComponentOfVector(vectorDistance, 90 - angle)) @staticmethod def getXComponentOfVector(magnitude, direction): # direction = degree degree2Radian = math.pi / 180 return magnitude * math.cos(direction * degree2Radian) @staticmethod def normalize(left_distance, right_distance): max_distance = max(left_distance, right_distance) return left_distance / max_distance, right_distance / max_distance @staticmethod def cappedMultiply(value, by): if value == 0: return 0 return min(abs(value) * by, 1) * value / abs(value)
def __init__(self, drone, picture_width, desired_move): self.turn = PID(K_p=0.6, K_d=0.1) self.move = PID(K_p=0.15, K_d=0.01) self.height = PID(K_p=0.2, K_d=0.00) self.picture_width = picture_width self.desired_move = desired_move self.drone = drone time.sleep(0.05) self.drone.takeoff() time.sleep(0.05)
def __init__(self, X=True) : if X : self.pwmpin = Pin('X6', Pin.OUT_PP) self.pwmtim = Timer(2, freq=5000) self.pwm = self.pwmtim.channel(1, mode=Timer.PWM, pin=self.pwmpin) self.pwm.pulse_width(0) self.dir = Pin('X2', Pin.OUT_PP) self.dir.off() # O = forward, 1 = reverse self.sleep = Pin('X3', Pin.OUT_PP) self.sleep.value(0) # 0 = sleep, 1 = active self.enca = Pin('X4', Pin.IN, Pin.PULL_UP) self.encb = Pin('X5', Pin.IN, Pin.PULL_UP) else : self.pwmpin = Pin('Y1', Pin.OUT_PP) self.pwmtim = Timer(8, freq=5000) self.pwm = self.pwmtim.channel(1, mode=Timer.PWM, pin=self.pwmpin) self.pwm.pulse_width(0) self.dir = Pin('Y2', Pin.OUT_PP) self.dir.off() # O = forward, 1 = reverse self.sleep = Pin('Y3', Pin.OUT_PP) self.sleep.value(0) # 0 = sleep, 1 = active self.enca = Pin('Y4', Pin.IN, Pin.PULL_UP) self.encb = Pin('Y5', Pin.IN, Pin.PULL_UP) self.pwmscale = (self.pwmtim.period() + 1) // 10000 # scale factot for permyriad(10000 as it allows to get more distinct points and avoid divisions) power self.count_a = 0 # counter for impulses on the A output of the encoder self.target_a = 0 # target value for the A counter (for controlled rotation) self.count_b = 0 # counter for impulses on the B output of the encoder self.time_a = 0 # last time we got an impulse on the A output of the encoder self.time_b = 0 # last time we got an impulse on the B output of the encoder self.elapsed_a_b = 0 # time elapsed between an impulse on A and an impulse on B self.dirsensed = 0 # direction sensed through the phase of the A and B outputs self.rpm = 0 # current speed in rotations per second self.rpm_last_a = 0 # value of the A counter when we last computed the rpms self.cruise_rpm = 0 # target value for the rpms self.walking = False # Boolean that indicates if the robot is walking or stationary self.desiredDir = True# Boolean that indecates the desired direction of the motor for move function self._control = PID() # PID control for the rotation of the wheels self._control.setTunings(KP, KI, KD); self._control.setSampleTime(1); self._control.setOutputLimits(-10000, 10000) self._control_distance = PID() # PID control for the cascade of the distance self._control_distance.setTunings(KP_DISTANCE, KI_DISTANCE, KD_DISTANCE); self._control_distance.setSampleTime(1); self._control_distance.setOutputLimits(-MAXIMUM_VELOCITY, MAXIMUM_VELOCITY) ExtInt(self.enca, ExtInt.IRQ_RISING, Pin.PULL_UP, self.enca_handler) ExtInt(self.encb, ExtInt.IRQ_RISING, Pin.PULL_UP, self.encb_handler) if RomiMotor.rpmtimer is None : # create only one shared timer for all instances RomiMotor.rpmtimer = Timer(4) RomiMotor.rpmtimer.init(freq=100, callback=RomiMotor.class_rpm_handler) RomiMotor.rpm_handlers.append(self.rpm_handler) # register the handler for this instance
def __init__(self): self.pid = PID(2, 0.10, 20) # 3 0.8 20 # Trial and Error values self.pid.setWindup(75) # 100 self.top_arduino = None self.base_arduino = None self.base_target = None # Current target for PID self.thread = None self.looping = False # If thread is running self.thread_shutdown = False
def setup(): pinMode(pin, INPUT, PULLUP) attachInterrupt(pin, motorReadAndAdjustment, FALLING) print "falling edge interrupt attached to P9.12 (GPIO1_28)" pin = GPIO1_28 gpio.setup("P8_10", gpio.OUT) set_mode(self, 1) set_period(self, 100000000) set_position(self, 0) pid = PID(0.2, 0.0, 0.0) pid.update()
class PID_Posicion(object): """docstring for PID_Posicion""" def __init__(self,callback,kp = 1.5 ,ki = 0.5 ,kd = 0.0, motor_no = 4,): self.callback = callback self.pid = PID(kp,ki,kd) self.motor = Motor(pines['motor'][motor_no]) def setPoint(self , setpoint): self.pid.setPoint(setpoint) sp = int(setpoint*(10)) self.motor.avance(sp)
def setup(): pinMode(pin, INPUT, PULLUP) attachInterrupt(pin, motorReadAndAdjustment, FALLING) print "falling edge interrupt attached to P9.12 (GPIO1_28)" pin = GPIO1_28 gpio.setup("P8_10",gpio.OUT) set_mode(self,1) set_period(self,100000000) set_position(self,0) pid = PID(0.2, 0.0, 0.0) pid.update()
def send_att(self): rate = rospy.Rate(100) # Hz self.att.body_rate = Vector3(0, 0, 2) self.att.header = Header() self.att.header.frame_id = "base_footprint" self.att.orientation = Quaternion(*quaternion_from_euler(0, 0, 0)) self.att.thrust = 0.3 self.att.type_mask = 7 # ignore body rate xPID = PID(kp=0.2, kd=0.2, ki=0.1) zPID = PID(kp=0.03, kd=0.01, ki=0.01) yPID = PID(kp=0.03, kd=0.01, ki=0.01) while not rospy.is_shutdown(): try: trans = (self.udrone_state.x, self.udrone_state.y, self.udrone_state.z) print("Estimate: ", trans) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as Ex: print(Ex) self.att.header.stamp = rospy.Time.now() self.att.orientation = Quaternion( *quaternion_from_euler(0, 0, 0)) self.att.thrust = 0.3 self.att_pub.publish(self.att) self.rate.sleep() continue x_error = xPID.run(trans[0], 0) y_error = yPID.run(trans[1], 0) z_error = zPID.run(trans[2], 5) # if z_error is positive, udrone is far from boat # so increase pitch to increase the altitude if z_error < -1.5709: z_error = -1.5709 elif z_error > 1.5709: z_error = 1.5709 print(z_error) # if y_error is positive, udrone is far from boat # so increase pitch to increase the altitude if y_error < -1.5709: y_error = -1.5709 elif y_error > 1.5709: y_error = 1.5709 if x_error < 0: x_error = 0.0 elif x_error > 1: x_error = 1.0 self.att.header.stamp = rospy.Time.now() self.att.orientation = Quaternion( *quaternion_from_euler(0, -1 * z_error, y_error)) self.att.thrust = x_error self.att_pub.publish(self.att) rate.sleep()
class Control: def __init__(self, robot: Robot): self.robot = robot self.pid = PID(0.4, 1, 1.5, 0.2) def state(self) -> State: return self.robot.estimator.state() def reset(self): self.pid.clear() def update_hline(self, dt, y, right): state = self.state() d = y - state.y # angle is 90 for large d theta = (pi / 2) * (1 - math.exp(-(2 * d)**2)) if d < 0: theta = -theta if not right: theta = pi - theta dtheta = norm_angle(theta - state.theta) # reduce speed if theta is very wrong, 1 at 0, 0.2 at pi/2 speed = 0.5 * math.exp(-abs(5 * dtheta)**2) # relax towards desired _theta omega = dtheta # / 2 domega = 0 if abs(d) < 0.4: domega = -self.pid.update(d, dt) if right: domega = -domega else: # TODO: reset PID? or update PID without using result? pass print(d, theta, state.theta, dtheta, speed, domega) self.robot.set_speed_omega(speed, omega + domega) self.robot.update(dt) def arc(self, dt, radius, speed, end_theta, direction): omega = speed / radius if not direction: omega = -omega self.robot.set_speed_omega(speed, omega) # stop at first zero crossing, but not a random jump +-pi dtheta_last = norm_angle(self.state().theta - end_theta) while True: dtheta = norm_angle(self.state().theta - end_theta) if abs(dtheta) < 0.5: if dtheta_last < 0 and dtheta >= 0: break if dtheta_last > 0 and dtheta <= 0: break dtheta_last = dtheta self.robot.update(dt)
def __init__(self, configFile, channel=0): self.configFile = configFile self.IOdevice = IOdevice self.channel = channel self.readParams() self.readTimepoints() self.CO2PID = PID() self.O3PID = PID() self.CO2enable = True self.O3enable = True self.parentPipe, self.childPipe = Pipe()
def __init__( self, callback, kp=1.5, ki=0.5, kd=0.0, motor_no=4, ): self.callback = callback self.pid = PID(kp, ki, kd) self.motor = Motor(pines['motor'][motor_no])
def InitControl(self): self._P = 0.8 self._I = 14.0 self._D = 0.12 self._maxIntegralOutput = 9.0 self._pid = PID(self._P, self._I, self._D) self._pid.setWindup(100.0) self._ctrlOutputMin = 1.0 # Volts, min motor output self._ctrlOutputMax = 12.0 # Volts, max motor output return
class HLineControl(Control): def __init__(self, y: float, right: bool, end_x: float, speed=0.2, omega=0.2): self.pid = PID(0.4, 1, 1.5, 0.2) self.y = y self.right = right self.end_x = end_x self.speed = speed self.omega = omega self.t_last = None def __str__(self): return f"HLineControl({self.right},{self.end_x})" def reset(self): self.pid.clear() def update(self, t: float, state: State): # -> (speed, omega) d = self.y - state.y # angle is 90 for large d theta = (pi/2) * (1 - math.exp(-(d/0.1)**2)) if d < 0: theta = -theta if not self.right: theta = pi - theta dtheta = norm_angle(theta - state.theta) # reduce speed if theta is very wrong, 1 at 0, 0.2 at pi/2 # also reduce speed if will overshoot in one iteration: # sin(theta) * speed * 1s <= abs(d) # speed <= abs(d) / sin(theta) speed = self.speed * math.exp(-abs(4*dtheta)**2) if math.sin(theta) > 0: speed = min(speed, abs(d) / math.sin(theta) + 0.02) omega = math.copysign(min(abs(dtheta) + 0.02, self.omega), dtheta) domega = 0 if False and self.t_last is not None and abs(d) < 0.4: # only use PID if near the line # TODO: remove this discontinuity domega = -self.pid.update(d, t - self.t_last) if self.right: domega = -domega else: # TODO: reset PID? or update PID without using result? pass #print(t, d, theta, state.theta, dtheta, speed, domega) self.t_last = t return speed, omega + domega def end(self, t: float, state: State) -> bool: return self.right == (state.x >= self.end_x)
def configure(self): self.load_config() #self.myPID1 = PID(self.config['kp1'], self.config['ki1'], self.config['sp1']) self.myPID1 = PID(2000, 5, 1125) self.myPID2 = PID(self.config['kp2'], self.config['ki2'], self.config['sp2']) self.myPID2.stop() self.u1 = 0 self.y01 = 0 self.u2 = 0 self.y02 = 0 self.timer = pyb.Timer(1, freq=1000)
def __init__(self, uav, height=10, radius=None, increment=5): super(BoxSearch, self).__init__(uav, height=height) self.radius = radius self.increment = increment self.last_pose = None self.last_increment = self.increment // 2 self.last_increment_dim = "-y" self.velocity = None self.pidz = PID(1.2, 1, .001) self.pidz.SetPoint = self.height self.pidz.setSampleTime(.01) self.pidxy = None
def __init__(self, lin_pid=PID(0.05, 0, 0.01), ang_pid=PID(0.03, 0, 0.05), trajectory_generator=TrajectorySCurve(r=10)): """ :param lin_pid: Linear PID. :param ang_pid: Angular PID. :param move_type; Which type of movement it should return. It can be 'vel' for MoveByVelocities or 'pow' for MoveByPowers. :return: None. """ self.lin_pid = lin_pid self.ang_pid = ang_pid self.trajectory_generator = trajectory_generator
def __init__(self, outer_loop_params, inner_loop_params, sample_time): self.SetPoint = 0.0 self.Output = 0.0 self.SampleTime = sample_time self.positionPID = PID(outer_loop_params.Kp, outer_loop_params.Ki, outer_loop_params.Kd, outer_loop_params.Limit) self.positionPID.setSampleTime(self.SampleTime) self.speedPID = PID(inner_loop_params.Kp, inner_loop_params.Ki, inner_loop_params.Kd, inner_loop_params.Limit) self.speedPID.setSampleTime(self.SampleTime)
def __init__(self): self.temperature_samples = [] self.last_fill = 0 self.current_volume = 0 self.resistor_duty = 0 # set by LLD self.temperatures = List_max(SAMPLE_HISTORY) self.volumes = List_max(SAMPLE_HISTORY) self.powers = List_max(SAMPLE_HISTORY) self.timing = List_max(SAMPLE_HISTORY) self.recipe_index = 0 PID.__init__(self) Chrono.__init__(self)
def __init__(self, imu, m1, m2, m3): threading.Thread.__init__(self) self.imu = imu self.m1 = m1 self.m2 = m2 self.m3 = m3 self.pidX = PID() self.pidY = PID() self.pidZ = PID(P=0.0, I=0.0, D=0.0) self.pidX.setPoint(0.02) # in radians self.pidY.setPoint(0.03) self.pidZ.setPoint(0.0) self.start()
class MotorController: def __init__(self,sensors,actuators): self.sensors=sensors self.actuators=actuators #PID self.PID=PID(1, 2, 0.15) self.currentAngle=0 self.desiredAngle=0 self.motorLdrive=0 self.motorRdrive=0 self.fwdVel=0 self.wallFollowPIDResult=0 self.turnConstantRate=0 self.motorState="turnToAngle" def stop(self): self.actuators.motorL.write(1,0) self.actuators.motorR.write(1,0) def updateMotorSpeeds(self): print self.motorState if(self.motorState=="turnToAngle"): self.updateTurnToAngle() elif(self.motorState=="wallFollow"): self.updateWallFollow() elif(self.motorState=="turnConstantRate"): self.updateTurnConstantRate() def updateTurnToAngle(self): pidResult=self.PID.valuePID(self.sensors.gyro.gyroCAngle, self.desiredAngle) # print 'Angle Dif: ' + str(cAngle-self.initAngle) + '\tPID RESULT: '+ str(pidResult) # print 'Encoders:\tR: ' + str(self.encoderR.val) + '\tL: ' + str(self.encoderL.val) # print 'AVG: ' + str((self.encoderR.val + self.encoderL.val)/2.) self.motorLdrive = self.fwdVel - pidResult self.motorRdrive = self.fwdVel + pidResult self.actuators.motorL.write(self.motorLdrive < 0,abs(self.motorLdrive)) self.actuators.motorR.write(self.motorRdrive < 0,abs(self.motorRdrive)) def updateWallFollow(self): self.motorLdrive = self.fwdVel - self.wallFollowPIDResult self.motorRdrive = self.fwdVel + self.wallFollowPIDResult self.actuators.motorL.write(self.motorLdrive < 0,abs(self.motorLdrive)) self.actuators.motorR.write(self.motorRdrive < 0,abs(self.motorRdrive)) def updateTurnConstantRate(self): self.motorLdrive = -self.turnConstantRate self.motorRdrive = self.turnConstantRate self.actuators.motorL.write(self.motorLdrive < 0,abs(self.motorLdrive)) self.actuators.motorR.write(self.motorRdrive < 0,abs(self.motorRdrive))
class Actuator(object): def __init__(self, drone, picture_width, desired_move): self.turn = PID(K_p=0.6, K_d=0.1) self.move = PID(K_p=0.15, K_d=0.01) self.height = PID(K_p=0.2, K_d=0.00) self.picture_width = picture_width self.desired_move = desired_move self.drone = drone time.sleep(0.05) self.drone.takeoff() time.sleep(0.05) def step(self, wdithmid, width): desired_turn = self.picture_width / 2 actual_turn = wdithmid actual_move = width ut = self.turn.step(desired_turn, actual_turn) um = self.move.step(self.desired_move, actual_move) height = 550 nav_data = self.drone.get_navdata() nav_data = nav_data[0] uh = self.height.step(height, nav_data['altitude']) self.drone.at(libardrone.at_pcmd, True, 0, self.moveDrone(um), self.heightDrone(uh), self.turnDrone(ut)) def turnDrone(self, u): speed = - u / (self.picture_width / 2.) print "move horizontal to" + str(speed) return speed def moveDrone(self, u): speed = - u / (self.picture_width / 2.) print "move near to" + str(speed) return speed def heightDrone(self, u): speed = u / 500 print "height near to" + str(speed) return speed
def __init__(self,sensors,actuators): self.sensors=sensors self.actuators=actuators #PID self.turnToAnglePID=PID(1.2, 3.5, 0.2) self.turnAndMovePID=PID(1, 2, 0.15) self.movePID=PID(1, 2, 0.15) self.currentAngle=0 self.desiredAngle=0 self.motorLdrive=0 self.motorRdrive=0 self.fwdVel=0 self.wallFollowPIDResult=0 self.turnConstantRate=0 self.turnConstantRateDirection="Right" self.motorState="turnToAngle"
def __init__(self,sensors,actuators): self.sensors=sensors self.actuators=actuators #PID self.PID=PID(1, 2, 0.15) self.currentAngle=0 self.desiredAngle=0 self.motorLdrive=0 self.motorRdrive=0 self.fwdVel=0 self.wallFollowPIDResult=0 self.turnConstantRate=0 self.motorState="turnToAngle"
def __init__(self): # Time for timer global T T = 1000 # For debugging global _debug _debug = False # For PID # TODO: Add GUI to tune K global Kp, Ki, Kd Kp = 3.0 Ki = 0.4 Kd = 1.2 self.p = PID(3.0, 0.4, 1.2) self.p.setPoint(0) # Initialize AutoWC and WheelchairModel self.autoWC = AutoWC() self.wc = WheelchairModel() self.wc.delta_t = T / 1000. interface = gtk.Builder() interface.add_from_file('AutoWC_GUI.glade') interface.connect_signals(self) self.Interface = interface.get_object('mainWindow') self.Interface.show() self.myPort = interface.get_object('entryPort') self.myStatus = interface.get_object('lbStatus') self.txtTicks = interface.get_object('textTicks') self.LRSignal = interface.get_object('adjustmentLR') self.FBSignal = interface.get_object('adjustmentFB') self.entryDistance = interface.get_object('entryDistance') self.entryAngle = interface.get_object('entryAngle') self.entryVelocity = interface.get_object('entryVelocity') self.cbtnStop_distance = interface.get_object('cbtnStop_distance') self.cbtnStop_angle = interface.get_object('cbtnStop_angle') self.entryStop_distance = interface.get_object('entryStop_distance') self.entryStop_angle = interface.get_object('entryStop_angle')
def setup(self): self.TicpIn = 4480 / 2.875 self.encoderL = Encoder(self.tamp, 22, 23) self.encoderR = Encoder(self.tamp, 21, 20) self.init_time = time.time() # self.encoderL = Encoder(self.tamp, 23, 22) # self.encoderR = Encoder(self.tamp, 21, 20) # motor left self.motorL = Motor(self.tamp, 2, 4) self.motorLdrive = 0 self.motorL.write(1, 0) # self.deltaL = 1 # self.motorvalL = 0 # motor right self.motorR = Motor(self.tamp, 1, 3) self.motorRdrive = 0 self.motorR.write(1, 0) # self.deltaR = 1 # self.motorvalR = 0 # gyro self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True) self.initAngle = self.gyro.val print "initial angle:" + str(self.initAngle) self.prevGyro = 0 self.drift = -0.02875 self.totalDrift = 0 self.drifts = [] self.PID = PID(5, 4, 0.2) self.fwdVel = 0 self.timer = Timer() """
def __init__(self): self.port = 'COM22' # change this to the com port for your Arduino self.baud = 115200 # baudrate don't change try: self.ser = serial.Serial(self.port, self.baud) print "Successfully opened Serial" print except serial.SerialException: self.ser = None print "Could not open Serial!" print # Initialize Joystick object joystick.init() pygame.display.init() if joystick.get_count(): self.joy = joystick.Joystick(0) self.joy.init() else: self.joy = None self.x_axis = 0 # this is usually 0 self.throttle_axis = 1 # change this to the correct axis for the joystick self.last_turn_val = 0 self.last_sheet_val = 47 self.auto_enabled = False # change to true to use PID self.kite_tracker = KiteTracker(path='cam') self.pos_list = [] self.pid = PID(3.0, 0.4, 1.2) self.pid.setSetPoint(0.0) # Set-point corresponds to heading vector which has angle = 0.0 in its frame. # create log file self.logfile = datetime.datetime.strftime(datetime.datetime.now(), '%Y-%m-%d_%H_%M_%S') + '_log.txt' with open(logfile, 'w') as f: f.write('time,command,turn_val,power_val\n')
def main(self): # Creates the publisher for ROS. (name of message, data type(from geometry_msgs.msg), queue size) velocity_publisher = rospy.Publisher('mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10) velocity_vector = TwistStamped() # Ros data type for publisher x_linear = 0 y_linear = 0 z_linear = 0 altitude_PID = PID(0.5,0.1,0.01) x_ang = 0 y_ang = 0 z_ang = 0 # Creates a state publisher, for when we want to remotely disarm the system state_publisher = rospy.Publisher('mavros/state', State, queue_size = 10) state_variable = State() # sets the rate of the loop rate = rospy.Rate(10) # goal altitude altitude_goal = 3 self.altitude_current = 0 # state variable stance = 0 stance_previous = 0 count = 0 # 0 : logical element # 1 : waiting for offb controll # 2 : take off # 3 : land # 4 : search pattern while not rospy.is_shutdown(): count += 1 if count % 100 == 1: rospy.loginfo("State: " + str(stance)) ########################################################################################## ################# stance switch ########################################################### ########################################################################################## if stance == 0: x_linear = 0 y_linear = 0 z_linear = 0 x_ang = 0 y_ang = 0 z_ang = 0 stance = 1 # waiting for offb control elif stance == 1: rate.sleep() if self.state_current.guided: rospy.loginfo("I'm taking control, takeoff in " + str(countdown)) countdown -= 0.1 if countdown < 0: rospy.loginfo("Switching to Takeoff") stance_previous = 1 stance = 2 else: countdown = 5 # Take off: elif stance == 2: altitude_goal = 2 if count % 10 == 1: rospy.loginfo("Current altitude: " + str(self.altitude_current)) if self.altitude_current <= altitude_goal + 0.1 and self.altitude_current >= altitude_goal - 0.1: countdown -= 0.1 if count % 10 == 1: rospy.loginfo("At goal altitude, switching to logic state") stance_previous = 2 stance = 0 else: countdown = 5 if countdown < 0: stance = 0 # Landing elif stance == 3: if count % 10 == 1: rospy.loginfo(self.altitude) z_linear = -0.02 if self.altitude_current < 0.10: countdown -= 0.1 rospy.loginfo("WARNING: low altitude, DISARMING in: " + str(countdown)) else: countdown = 10 if self.altitude_current < 0.05 and countdown < 0: rospy.loginfo("DISARM-DISARM-DISARM") state_variable.armed = False state_variable.guided = False state_publisher.publish(state_variable) elif stance == 4: pass elif stance == 5: pass elif stance == 6: pass ########################################################################################## ################# velocity finder ######################################################## ########################################################################################## if not stance == 3: # If were not landing, this works # set the verticle speed z_linear = altitude_PID.run(self.altitude_current, altitude_goal) # need to do this for x_linear, y_linear, and possibly x,y, and z ang ########################################################################################## ################# velocity publisher ##################################################### ########################################################################################## # set linear to value velocity_vector.twist.linear.x = x_linear velocity_vector.twist.linear.y = y_linear velocity_vector.twist.linear.z = z_linear # Set angular to zero velocity_vector.twist.angular.x = x_ang velocity_vector.twist.angular.y = y_ang velocity_vector.twist.angular.z = z_ang # Get the time now for the velocity commands time = rospy.Time.now() velocity_vector.header.stamp.secs = int(time.to_sec()) velocity_vector.header.stamp.nsecs = int((time.to_sec() - int(time.to_sec())) * 1000000000) velocity_vector.header.seq = count # use the publisher velocity_publisher.publish(velocity_vector) rate.sleep()
def setup(self): self.TicpIn = 4480/2.875 self.ir_array = Ir_array(self.tamp, 16, 15, 14, 40, 11, 10) self.color = Color(self.tamp, integrationTime=Color.INTEGRATION_TIME_101MS, gain=Color.GAIN_1X) self.uIR = DigitalInput(self.tamp, 17) self.uIR.val = 1 self.servo = Servo(self.tamp, 9) self.servo.bottom = 0 self.servo.top = 200 self.servo.speed = 30 self.servo.write(self.servo.bottom) self.servoval = self.servo.bottom self.delta = 0 self.sorter = Servo(self.tamp, 5) self.sorter.center = 90 self.sorter.right = 25 self.sorter.left = 165 self.sorter.speed = 15 self.sorter.write(self.sorter.center) self.sorterval = self.sorter.center self.dsorter = 0 self.encoderL = Encoder(self.tamp, 22, 23) self.encoderR = Encoder(self.tamp, 21, 20) self.init_time = time.time() # self.encoderL = Encoder(self.tamp, 23, 22) # self.encoderR = Encoder(self.tamp, 21, 20) #motor left self.motorL = Motor(self.tamp, 2, 4) self.motorLdrive = 0 self.motorL.write(1,0) #self.deltaL = 1 #self.motorvalL = 0 #motor right self.motorR = Motor(self.tamp,1, 3) self.motorRdrive = 0 self.motorR.write(1,0) #self.deltaR = 1 #self.motorvalR = 0 #gyro self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True) self.initAngle = self.gyro.val self.newAngle = 0 self.blockAngle = 0 self.blockDistance = 0 # print "initial angle:"+str(self.initAngle) self.Follow = 'Right' self.IRs = {'Left': [0, 1, 2], 'Right': [5, 4, 3]} self.prevGyro = 0 self.drift = -.02875 self.totalDrift = 0 self.drifts = [] self.PID=PID(.5, 1, 0.15) self.IRPID = PID(10, 5, .15) self.fwdVel = 30 self.turnVel = 0 self.MoveArm, self.Top, self.Bottom = False, 0, 0 self.Sort = 0 self.SortPause = 0 self.State = 1 self.timer = Timer()
class PIDDrive(SyncedSketch): ss_pin = 10 #gyro select pin # image_pipe = './image' # if not os.path.exists(image_pipe): # os.mkfifo(image_pipe) # image_fd = os.open(image_pipe, os.O_RDONLY) def setup(self): self.TicpIn = 4480/2.875 self.ir_array = Ir_array(self.tamp, 16, 15, 14, 40, 11, 10) self.color = Color(self.tamp, integrationTime=Color.INTEGRATION_TIME_101MS, gain=Color.GAIN_1X) self.uIR = DigitalInput(self.tamp, 17) self.uIR.val = 1 self.servo = Servo(self.tamp, 9) self.servo.bottom = 0 self.servo.top = 200 self.servo.speed = 30 self.servo.write(self.servo.bottom) self.servoval = self.servo.bottom self.delta = 0 self.sorter = Servo(self.tamp, 5) self.sorter.center = 90 self.sorter.right = 25 self.sorter.left = 165 self.sorter.speed = 15 self.sorter.write(self.sorter.center) self.sorterval = self.sorter.center self.dsorter = 0 self.encoderL = Encoder(self.tamp, 22, 23) self.encoderR = Encoder(self.tamp, 21, 20) self.init_time = time.time() # self.encoderL = Encoder(self.tamp, 23, 22) # self.encoderR = Encoder(self.tamp, 21, 20) #motor left self.motorL = Motor(self.tamp, 2, 4) self.motorLdrive = 0 self.motorL.write(1,0) #self.deltaL = 1 #self.motorvalL = 0 #motor right self.motorR = Motor(self.tamp,1, 3) self.motorRdrive = 0 self.motorR.write(1,0) #self.deltaR = 1 #self.motorvalR = 0 #gyro self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True) self.initAngle = self.gyro.val self.newAngle = 0 self.blockAngle = 0 self.blockDistance = 0 # print "initial angle:"+str(self.initAngle) self.Follow = 'Right' self.IRs = {'Left': [0, 1, 2], 'Right': [5, 4, 3]} self.prevGyro = 0 self.drift = -.02875 self.totalDrift = 0 self.drifts = [] self.PID=PID(.5, 1, 0.15) self.IRPID = PID(10, 5, .15) self.fwdVel = 30 self.turnVel = 0 self.MoveArm, self.Top, self.Bottom = False, 0, 0 self.Sort = 0 self.SortPause = 0 self.State = 1 self.timer = Timer() def loop(self): #state 0 - wait for first no block, do nothing #transition: no block -> state 1 #state 1 - look for block #transition: found block -> state 2 #sate 2 - drive over block #transition: ir triggered -> state 3 #sate 3 - pick up block #transition: color sensor done -> sate 1 if self.timer.millis() > 100: # print 'Color' # print self.color.c # print self.color.r, self.color.g if self.color.c > 800 and self.Sort == 0: if self.color.r > self.color.g: self.Sort = 1 else: self.Sort = 2 # print self.uIR.val if self.uIR.val == 0: # self.MoveArm = True self.State = 0 if self.MoveArm: if self.Bottom == 2 and self.Top == 1: self.delta = 0 self.servoval = self.servo.bottom self.servo.write(self.servo.bottom) self.Bottom, self.Top = 0, 0 self.MoveArm = False elif self.servoval >= self.servo.top: time.sleep(1) self.delta = -self.servo.speed self.Top = 1 elif self.servoval <= self.servo.bottom: self.delta = self.servo.speed self.Bottom = 1 if self.Top == 1: self.Bottom = 2 if self.Sort == 1: if self.sorterval < self.sorter.left: self.dsorter = self.sorter.speed elif self.sorterval >= self.sorter.left: self.dsorter = 0 self.Sort = 3 self.SortPause = time.time() if self.Sort == 2: if self.sorterval > self.sorter.right: self.dsorter = -self.sorter.speed elif self.sorterval <= self.sorter.right: self.dsorter = 0 self.Sort = 3 self.SortPause = time.time() if self.Sort == 3: if time.time() - self.SortPause > 1: self.sorterval = self.sorter.center self.Sort = 0 self.sorterval += self.dsorter self.sorter.write(abs(self.sorterval)) self.servoval += self.delta self.servo.write(abs(self.servoval)) self.initAngle += self.turnVel self.timer.reset() #response = self.rp.read() #print "Got response %s" % response # Valid gyro status is [0,1], see datasheet on ST1:ST0 bits cAngle=self.gyro.val - self.totalDrift #corrected angle # print (self.gyro.val-self.prevGyro), self.gyro.val, self.gyro.status, cAngle, self.totalDrift self.prevGyro=self.gyro.val self.totalDrift+=self.drift # message = os.read(self.image_fd, 20) # if ParseMessage(message): # self.blockDistance, self.blockAngle = ParseMessage(message) # print 'Angle Dif: ' + str(cAngle-self.initAngle) + '\tPID RESULT: '+ str(pidResult) # print 'Encoders:\tR: ' + str(self.encoderR.val) + '\tL: ' + str(self.encoderL.val) # print 'AVG: ' + str((self.encoderR.val + self.encoderL.val)/2.) self.ir_array.update() ir = self.ir_array.ir_value ir90 = self.IRs[self.Follow][0] ir30 = self.IRs[self.Follow][1] avg = (ir[self.IRs[self.Follow][0]]+ir[self.IRs[self.Follow][1]]/2)/2 min_val = min(ir[self.IRs[self.Follow][0]], ir[self.IRs[self.Follow][1]]) if avg != float('inf'): pidResult= -self.IRPID.valuePID(4, avg) elif min_val != float('inf'): pidResult= -self.IRPID.valuePID(4, min_val) else: pidResult = -20 if ir[self.IRs[self.Follow][2]] < 4: pidResult = 20 # self.fwdVel = 0 else: self.fwdVel = 30 pidResult = self.PID.valuePID(self.initAngle, cAngle) self.motorLdrive = self.fwdVel - pidResult self.motorRdrive = self.fwdVel + pidResult print 'Distance Traveled : ' + str(self.getDistanceTraveled()) self.motorL.write(self.motorLdrive < 0,abs(self.motorLdrive)) self.motorR.write(self.motorRdrive < 0,abs(self.motorRdrive)) ''' print "ok" #response = self.rp.read() #print "Got response %s" % response ''' def ParseMessage(message): if message: if message[:2] == 'no': blockDistance, blockAngle = 0, 0 return None else: try: blockDistance, blockAngle = [number[:6] for number in message.split(',')] except: blockDistance, blockAngle = 0, 0 return blockDistance, blockAngle else: return None def getDistanceTraveled(self): """returns Distance traveled in inches. Call resetEncoders, drive forward until you've reached your destination""" print 'Left: ' + str(self.encoderL.val) print 'Right: ' + str(self.encoderR.val) avg = (self.encoderL.val + self.encoderR.val)/2 return avg/360.
class KiteControl(object): def __init__(self): self.port = 'COM22' # change this to the com port for your Arduino self.baud = 115200 # baudrate don't change try: self.ser = serial.Serial(self.port, self.baud) print "Successfully opened Serial" print except serial.SerialException: self.ser = None print "Could not open Serial!" print # Initialize Joystick object joystick.init() pygame.display.init() if joystick.get_count(): self.joy = joystick.Joystick(0) self.joy.init() else: self.joy = None self.x_axis = 0 # this is usually 0 self.throttle_axis = 1 # change this to the correct axis for the joystick self.last_turn_val = 0 self.last_sheet_val = 47 self.auto_enabled = False # change to true to use PID self.kite_tracker = KiteTracker(path='cam') self.pos_list = [] self.pid = PID(3.0, 0.4, 1.2) self.pid.setSetPoint(0.0) # Set-point corresponds to heading vector which has angle = 0.0 in its frame. # create log file self.logfile = datetime.datetime.strftime(datetime.datetime.now(), '%Y-%m-%d_%H_%M_%S') + '_log.txt' with open(logfile, 'w') as f: f.write('time,command,turn_val,power_val\n') def write_to_serial(self, msg): if self.ser: if not '/' in msg: msg += '/' self.ser.write(msg) # Terminating char '/' must always be appended to the end of the msg with open('log.txt', 'a') as f: date = datetime.datetime.strftime(datetime.datetime.now(), '%Y-%m-%d %H:%M:%S.%f') f.write(date + ',' + msg + ',') if 'p' in msg: f.write(',' + msg.strip('t/p')) elif 't' in msg: f.write(msg.strip('t/p') + ',') f.write('\n') return True else: if not '/' in msg: msg += '/' print 'Serial not available: ' + msg return False def turn(self, val): val *= 30 val = int(val) if not val == self.last_turn_val: self.last_turn_val = val return self.write_to_serial('t' + str(val)) def sheet(self, val): val = (val + 1)/2 * 96 # maps the values (-1,1) to (0,96) This might need to be reversed? val = int(val) if not val == self.last_sheet_val: self.last_sheet_val = int(val) return self.write_to_serial('p' + str(val)) def run(self): while True: pygame.event.get() pygame.event.pump() if not self.auto_enabled: if self.joy: self.turn(self.joy.get_axis(self.x_axis)) self.sheet(self.joy.get_axis(self.throttle_axis)) self.kite_tracker.update() else: self.kite_tracker.update() # must be called once per loop error = self.update_error() #output = self.pid.update(error) #self.turn(output) time.sleep(0.05) def update_error(self): """ Input: tuple of x,y coordinates of the kite. Computes angle error between current heading and desired heading. Returns: error angle in degrees. """ # get updated position from kite tracker and store in array self.pos_list.append(self.kite_tracker.get_pos()) if len(self.pos_list) > 3: last_pos = self.pos_list[-3] pos = self.pos_list[-1] target = self.kite_tracker.get_current_target() if pos and last_pos and last_pos != pos: # only keep the last 5 positions if len(self.pos_list) > 5: self.pos_list = self.pos_list[1:] # compute the heading based on the last average vector over the last 2 positions vecA = np.array([pos[0]-last_pos[0], pos[1] - last_pos[1]]) vecA = vecA/np.linalg.norm(vecA) vecB = np.array([target[0] - last_pos[0], target[1] - last_pos[1]]) vecB = vecB/np.linalg.norm(vecB) ctheta = vecA.dot(vecB)/(np.linalg.norm(vecA) * np.linalg.norm(vecB)) # = cos(theta) if ctheta > 1: ctheta = 1.0 theta = np.rad2deg(np.arccos(ctheta)) # original unsigned angle angle = 1 rot = np.mat([[np.cos(np.deg2rad(angle)), -np.sin(np.deg2rad(angle))], [np.sin(np.deg2rad(angle)), np.cos(np.deg2rad(angle))]]) # rotate vecA by 1 degree ccw rot_vecA = rot * np.mat(vecA).T rot_vecA = rot_vecA.T # recalcluate angle ctheta = rot_vecA.dot(vecB)/(np.linalg.norm(rot_vecA) * np.linalg.norm(vecB)) if ctheta > 1: ctheta = 1.0 theta_new = np.rad2deg(np.arccos(ctheta)) # original unsigned angle if theta_new < theta: theta = -theta print theta return theta else: print 0 return 0 else: print 0 return 0
class Quadcopter(): def __init__(self): self.bus = smbus.SMBus(1) self.pwm = PwmBoard(self.bus, 0x40) self.motor1 = Motor(self.pwm, 0) self.motor2 = Motor(self.pwm, 1) self.motor3 = Motor(self.pwm, 2) self.motor4 = Motor(self.pwm, 3) self.mpu6050 = MPU6050(self.bus, 0x68) self.mpu6050.start() self.networkhandler = NetworkHandler(self) self.running = True self.pgain = 5.0 self.igain = 0.01 self.dgain = 1.5 self.roll_pid = PID(self.pgain, self.igain, self.dgain) self.pitch_pid = PID(self.pgain, self.igain, self.dgain) self.yaw_pid = PID(self.pgain, self.igain, self.dgain) self.wantedroll = 0.0 self.wantedpitch = 0.0 self.throttle = 0.0 time.sleep(1) def start(self): lasttime = time.time() time_diff = 0.01 while self.running: self.mpu6050.read() time.sleep(time_diff - 0.005) x, y, z = self.mpu6050.getlastvalues() delta_time = time.time() - lasttime lasttime = time.time() roll_pid_result = self.roll_pid.Compute(x, self.wantedroll, delta_time) pitch_pid_result = self.pitch_pid.Compute(y, self.wantedpitch, delta_time) #yaw_pid_result = self.yaw_pid.Compute(z, 0, delta_time) yaw_pid_result = 0 if self.throttle != 0: self.motor1.update((self.throttle + roll_pid_result + pitch_pid_result + yaw_pid_result)) self.motor2.update((self.throttle + roll_pid_result - pitch_pid_result - yaw_pid_result)) self.motor3.update((self.throttle - roll_pid_result - pitch_pid_result + yaw_pid_result)) self.motor4.update((self.throttle - roll_pid_result + pitch_pid_result - yaw_pid_result)) def changepidgain(self, pgain, igain, dgain): self.roll_pid.changegain(pgain, igain, dgain) self.pitch_pid.changegain(pgain, igain, dgain) def setroll(self, roll): if (roll > -50) & (roll < 50): self.wantedroll = roll def setpitch(self, pitch): if (pitch > -50) & (pitch < 50): self.wantedpitch = pitch def setthrottle(self, throttle): if throttle == 0: self.throttle = 0 time.sleep(0.1) self.motor1.stop() self.motor2.stop() self.motor3.stop() self.motor4.stop() else: throttle = throttle * (((self.motor1.max - 180) - (self.motor1.min + 180)) / 100) + (self.motor1.min + 180) if (throttle > (self.motor1.min + 180)) & (throttle < (self.motor1.max - 180)): self.throttle = throttle def getdata(self): gx, gy, ax, ay, x, y, z = self.mpu6050.getextendedvalues() return gx, gy, ax, ay, x, y, z, self.motor1.getpercent(), self.motor2.getpercent(), self.motor3.getpercent(), self.motor4.getpercent() def stop(self): self.running = False self.mpu6050.stop() time.sleep(1) self.motor1.stop() self.motor2.stop() self.motor3.stop() self.motor4.stop() self.networkhandler.stop()
def Lock(con, cur): setPoints = getSetpoints() LaserLock_369 = PID(P=-50, I=-500, D=0) LaserLock_399 = PID(P=20, I=20, D=0) LaserLock_935 = PID(P=40, I=60, D=0) LaserLock_369.setPoint(setPoints[0]) LaserLock_399.setPoint(setPoints[1]) LaserLock_935.setPoint(setPoints[2]) ADDA1.setVoltage(0,0) ADDA1.setVoltage(1,0) ADDA1.setVoltage(2,0) timeFlag_1 = False overTime = time.mktime(datetime.datetime.now().timetuple()) errorCount=0 while True: freq = getFreqs() t = getSetpoints() if(t != None): setPoints = getSetpoints() if(LaserLock_369.set_point != setPoints[0]): LaserLock_369.setPoint(setPoints[0]) if(LaserLock_399.set_point != setPoints[1]): LaserLock_399.setPoint(setPoints[1]) if(LaserLock_935.set_point != setPoints[2]): LaserLock_935.setPoint(setPoints[2]) for i in range(len(freq)): if freq[i]<0: freq[i] = setPoints[i] if abs(freq[i]-setPoints[i]) > 0.01: # > 10 Ghz or 0.01nm? probably a mode hop freq[i] = setPoints[i] # wait to try and recover #freq[0] = setPoints[0] error_369 = LaserLock_369.update(freq[0]) error_399 = LaserLock_399.update(freq[1]) error_935 = LaserLock_935.update(freq[2]) #print freq #print setPoints print round(error_369,4), round(error_399,4), round(error_935,4) if (error_369>=5) or(error_399>=5) or(error_935>=5) or(error_369<=-5) or(error_399<=-5) or(error_935<=-5): errorCount += 1 else: errorCount = 0 if errorCount > 3: # winsound.PlaySound("SystemQuestion", winsound.SND_ALIAS) # winsound.PlaySound("SystemQuestion", winsound.SND_ALIAS) # winsound.PlaySound("SystemQuestion", winsound.SND_ALIAS) # winsound.PlaySound("SystemQuestion", winsound.SND_ALIAS) ADDA1.setVoltage(0,0) ADDA1.setVoltage(1,0) ADDA1.setVoltage(2,0) print "Lock Broken!" break ADDA1.setVoltage(0, error_369) ADDA1.setVoltage(1, error_399) ADDA1.setVoltage(2, error_935) cTime = time.mktime(datetime.datetime.now().timetuple())*1e3 + datetime.datetime.now().microsecond/1e3 #cur.execute("INSERT INTO `wavemeter`.`error`( `index`, `time`, `739`, `935`, `739w`, `935w`) VALUES (NULL, \'%s\',\'%s\',\'%s\',\'%s\',\'%s\');",(cTime,error_2,error_1, freq[1], freq[0])) #con.commit() cur.execute("INSERT INTO `wavemeter`.`error` (time, `369`, `399`, `935`, 369w, 399w, 935w) VALUES (\'%s\',\'%s\',\'%s\',\'%s\',\'%s\',\'%s\',\'%s\');",(cTime,round(error_369,4), round(error_399,4), round(error_935,4), freq[0], freq[1], freq[2])) con.commit() time.sleep(.2)
class PIDDrive(SyncedSketch): ss_pin = 10 #gyro select pin image_pipe = './image' if not os.path.exists(image_pipe): os.mkfifo(image_pipe) image_fd = os.open(image_pipe, os.O_RDONLY) def setup(self): #Pygame stuff pygame.init() self.screen = pygame.display.set_mode((300, 300)) self.TicpIn = 4480/2.875 self.color = Color(self.tamp, integrationTime=Color.INTEGRATION_TIME_101MS, gain=Color.GAIN_1X) self.uIR = DigitalInput(self.tamp, 17) self.uIR.val = 1 self.servo = Servo(self.tamp, 9) self.servo.bottom = 0 self.servo.top = 200 self.servo.speed = 30 self.servo.write(self.servo.bottom) self.servoval = self.servo.bottom self.delta = 0 self.sorter = Servo(self.tamp, 5) self.sorter.center = 90 self.sorter.right = 25 self.sorter.left = 165 self.sorter.speed = 15 self.sorter.write(self.sorter.center) self.sorterval = self.sorter.center self.dsorter = 0 self.encoderL = Encoder(self.tamp, 22, 23) self.encoderR = Encoder(self.tamp, 21, 20) self.init_time = time.time() # self.encoderL = Encoder(self.tamp, 23, 22) # self.encoderR = Encoder(self.tamp, 21, 20) #motor left self.motorL = Motor(self.tamp, 2, 4) self.motorLdrive = 0 self.motorL.write(1,0) #self.deltaL = 1 #self.motorvalL = 0 #motor right self.motorR = Motor(self.tamp,1, 3) self.motorRdrive = 0 self.motorR.write(1,0) #self.deltaR = 1 #self.motorvalR = 0 #gyro self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True) self.initAngle = self.gyro.val self.newAngle = 0 self.blockAngle = 0 self.blockDistance = 0 print "initial angle:"+str(self.initAngle) self.prevGyro = 0 self.drift = -.02875 self.totalDrift = 0 self.drifts = [] self.PID=PID(.5, 1, 0.15) self.fwdVel = 0 self.turnVel = 0 self.turn = 0 self.MoveArm, self.Top, self.Bottom = False, 0, 0 self.Sort = 0 self.SortPause = 0 self.State = 1 self.timer = Timer() def loop(self): #state 0 - wait for first no block, do nothing #transition: no block -> state 1 #state 1 - look for block #transition: found block -> state 2 #sate 2 - drive over block #transition: ir triggered -> state 3 #sate 3 - pick up block #transition: color sensor done -> sate 1 message = os.read(self.image_fd, 20) if message: # print("Recieved: '%s'" % message) if message[:2] == 'no': self.blockAngle = 0 if self.State == 0: self.State = 1 if self.State == 2: self.State = 3 else: if self.State != 0: self.State = 2 try: self.blockDistance, self.blockAngle = [number[:6] for number in message.split(',')] except: self.blockAngle = 0 self.blockAngle = float(self.blockAngle) if self.timer.millis() > 100: for event in pygame.event.get(): if event.type == pygame.QUIT: pygame.display.quit() if event.type == pygame.KEYDOWN: if event.key == pygame.K_LEFT: self.turnVel = -10 if event.key == pygame.K_RIGHT: self.turnVel = 10 if event.key == pygame.K_UP: self.fwdVel = 100 if event.key == pygame.K_DOWN: self.fwdVel = -100 if event.key == pygame.K_1: self.Sort = 1 if event.key == pygame.K_2: self.Sort = 2 if event.key == pygame.K_SPACE: self.MoveArm = True if event.type == pygame.KEYUP: if event.key == pygame.K_LEFT or event.key == pygame.K_RIGHT: self.turnVel = 0 self.turn = 0 if event.key == pygame.K_UP or event.key == pygame.K_DOWN: self.fwdVel = 0 print 'Color' print self.color.c print self.color.r, self.color.g print if self.color.c > 800 and self.Sort == 0: if self.color.r > self.color.g: self.Sort = 1 else: self.Sort = 2 # print self.uIR.val if self.uIR.val == 0: self.MoveArm = True self.State = 0 if self.MoveArm: if self.Bottom == 2 and self.Top == 1: self.delta = 0 self.servoval = self.servo.bottom self.servo.write(self.servo.bottom) self.Bottom, self.Top = 0, 0 self.MoveArm = False elif self.servoval >= self.servo.top: time.sleep(1) self.delta = -self.servo.speed self.Top = 1 elif self.servoval <= self.servo.bottom: self.delta = self.servo.speed self.Bottom = 1 if self.Top == 1: self.Bottom = 2 if self.Sort == 1: if self.sorterval < self.sorter.left: self.dsorter = self.sorter.speed elif self.sorterval >= self.sorter.left: self.dsorter = 0 self.Sort = 3 self.SortPause = time.time() if self.Sort == 2: if self.sorterval > self.sorter.right: self.dsorter = -self.sorter.speed elif self.sorterval <= self.sorter.right: self.dsorter = 0 self.Sort = 3 self.SortPause = time.time() if self.Sort == 3: if time.time() - self.SortPause > 1: self.sorterval = self.sorter.center self.Sort = 0 self.sorterval += self.dsorter self.sorter.write(abs(self.sorterval)) self.servoval += self.delta self.servo.write(abs(self.servoval)) self.turn += self.turnVel self.timer.reset() #response = self.rp.read() #print "Got response %s" % response # Valid gyro status is [0,1], see datasheet on ST1:ST0 bits cAngle=self.gyro.val - self.totalDrift #corrected angle # print (self.gyro.val-self.prevGyro), self.gyro.val, self.gyro.status, cAngle, self.totalDrift self.prevGyro=self.gyro.val self.totalDrift+=self.drift # print 'State: ' + str(self.Searching) self.newAngle = cAngle + self.blockAngle print self.blockAngle pidResult=self.PID.valuePID(cAngle, cAngle+self.blockAngle+self.turn) # print 'Angle Dif: ' + str(cAngle-self.initAngle) + '\tPID RESULT: '+ str(pidResult) # print 'Encoders:\tR: ' + str(self.encoderR.val) + '\tL: ' + str(self.encoderL.val) # print 'AVG: ' + str((self.encoderR.val + self.encoderL.val)/2.) self.motorLdrive = self.fwdVel - pidResult self.motorRdrive = self.fwdVel + pidResult self.motorL.write(self.motorLdrive < 0,abs(self.motorLdrive)) self.motorR.write(self.motorRdrive < 0,abs(self.motorRdrive)) '''
# Preset options. requiredTemp = 37.5 saveToLogFile = False minTempAlarm = 0.0 maxTempAlarm = 100.0 maxTempDiff = 100.0 # Pin asignments sensorPins = [] alarmOutputPin = 0 tempOutputPin = 0 tempPID = PID(1.0, 0.0, 0.0) # Retrieve a sensor reading def getSensorValues(sensorPin): # Use the (modified) Adafruit DHT library proc = subprocess.Popen(' '.join(["./Adafruit_DHT 22",str(sensorPin)]), stdout=subprocess.PIPE, shell=True) (out, err) = proc.communicate() if out == "": raise ValueError else: # Output format: TT.t,HH.h vals = out.split(",") return float(vals[0]), float(vals[1])
def setup(self): #Pygame stuff pygame.init() self.screen = pygame.display.set_mode((300, 300)) self.TicpIn = 4480/2.875 self.color = Color(self.tamp, integrationTime=Color.INTEGRATION_TIME_101MS, gain=Color.GAIN_1X) self.uIR = DigitalInput(self.tamp, 17) self.uIR.val = 1 self.servo = Servo(self.tamp, 9) self.servo.bottom = 0 self.servo.top = 200 self.servo.speed = 30 self.servo.write(self.servo.bottom) self.servoval = self.servo.bottom self.delta = 0 self.sorter = Servo(self.tamp, 5) self.sorter.center = 90 self.sorter.right = 25 self.sorter.left = 165 self.sorter.speed = 15 self.sorter.write(self.sorter.center) self.sorterval = self.sorter.center self.dsorter = 0 self.encoderL = Encoder(self.tamp, 22, 23) self.encoderR = Encoder(self.tamp, 21, 20) self.init_time = time.time() # self.encoderL = Encoder(self.tamp, 23, 22) # self.encoderR = Encoder(self.tamp, 21, 20) #motor left self.motorL = Motor(self.tamp, 2, 4) self.motorLdrive = 0 self.motorL.write(1,0) #self.deltaL = 1 #self.motorvalL = 0 #motor right self.motorR = Motor(self.tamp,1, 3) self.motorRdrive = 0 self.motorR.write(1,0) #self.deltaR = 1 #self.motorvalR = 0 #gyro self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True) self.initAngle = self.gyro.val self.newAngle = 0 self.blockAngle = 0 self.blockDistance = 0 print "initial angle:"+str(self.initAngle) self.prevGyro = 0 self.drift = -.02875 self.totalDrift = 0 self.drifts = [] self.PID=PID(.5, 1, 0.15) self.fwdVel = 0 self.turnVel = 0 self.turn = 0 self.MoveArm, self.Top, self.Bottom = False, 0, 0 self.Sort = 0 self.SortPause = 0 self.State = 1 self.timer = Timer()
numReadings = 0 sumReadings= 0 while time.time() < (startTimeForCalibration+10.0): numReadings += 1 sumReadings += accelorometer.getReadingX() meanReading = 0 - (sumReadings / numReadings) start_time = time.time() # Set up the PID control system pid = PID(P=1, I=1, D=1, Derivator=0, Integrator=0, Integrator_max=50, Integrator_min=-50) for i in range (0,1000): r= accelorometer.getReadingX() + meanReading print r, pid.update(r) end_time = time.time() print("Elapsed time was %g seconds" % (end_time - start_time))
g=numpy.matrix(numpy.diag([l/Ixx,l/Iyy,1/Izz])) ginv=g.I ## Vectores de x deseada y su derivada xd=numpy.array([[0],[0],[0]]) xpd=numpy.array([[0],[0],[0]]) Altd=260 ## Objeto diferenciador numerico por modos deslizantes difN = 4 ## Orden del diferenciador dif = diffOrdN.DiffOrdN(difN,[12,8,4,3.5,2.1]) ## Objeto que calcula la integral de la funcion signo intsgn=Integral(lambda x:numpy.sign(x)) ## Controlador de altitud ctrlalt=PID(.7,.2,.1) ### Se establece configuracion con el ardrone, se apagan los motores y se cambia el modo de camara stdout.write("Estableciendo comunicacion con el ARDrone\n") stdout.flush() drone=libardrone.ARDrone() sleep(1) print "Listo!" stdout.write("Estableciendo configuracion inicial\n") stdout.flush() drone.reset() sleep(0.1) drone.trim() sleep(1.5) drone.reset() print "Encendiendo motores"
class PIDDrive(SyncedSketch): ss_pin = 10 # gyro select pin def setup(self): self.TicpIn = 4480 / 2.875 self.encoderL = Encoder(self.tamp, 22, 23) self.encoderR = Encoder(self.tamp, 21, 20) self.init_time = time.time() # self.encoderL = Encoder(self.tamp, 23, 22) # self.encoderR = Encoder(self.tamp, 21, 20) # motor left self.motorL = Motor(self.tamp, 2, 4) self.motorLdrive = 0 self.motorL.write(1, 0) # self.deltaL = 1 # self.motorvalL = 0 # motor right self.motorR = Motor(self.tamp, 1, 3) self.motorRdrive = 0 self.motorR.write(1, 0) # self.deltaR = 1 # self.motorvalR = 0 # gyro self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True) self.initAngle = self.gyro.val print "initial angle:" + str(self.initAngle) self.prevGyro = 0 self.drift = -0.02875 self.totalDrift = 0 self.drifts = [] self.PID = PID(5, 4, 0.2) self.fwdVel = 0 self.timer = Timer() """ self.pipePath = "./vision" print "ok1" time.sleep(1) try: os.mkfifo(self.pipePath) except OSError: print "error" print "ok" self.rp = open(self.pipePath, 'r') """ def loop(self): if self.timer.millis() > 100: self.timer.reset() # response = self.rp.read() # print "Got response %s" % response # Valid gyro status is [0,1], see datasheet on ST1:ST0 bits cAngle = self.gyro.val - self.totalDrift # corrected angle # print (self.gyro.val-self.prevGyro), self.gyro.val, self.gyro.status, cAngle, self.totalDrift self.prevGyro = self.gyro.val self.totalDrift += self.drift pidResult = self.PID.valuePID(cAngle, self.initAngle) print "Angle Dif: " + str(cAngle - self.initAngle) + "\tPID RESULT: " + str(pidResult) print "Encoders:\tR: " + str(self.encoderR.val) + "\tL: " + str(self.encoderL.val) print "AVG: " + str((self.encoderR.val + self.encoderL.val) / 2.0) self.motorLdrive = self.fwdVel - pidResult self.motorRdrive = self.fwdVel + pidResult if self.motorLdrive >= 0: dirL = 0 else: dirL = 1 if self.motorRdrive >= 0: dirR = 0 else: dirR = 1 self.motorL.write(dirL, abs(self.motorLdrive)) self.motorR.write(dirR, abs(self.motorRdrive)) """
class Stablizer (threading.Thread): def __init__(self, imu, m1, m2, m3): threading.Thread.__init__(self) self.imu = imu self.m1 = m1 self.m2 = m2 self.m3 = m3 self.pidX = PID() self.pidY = PID() self.pidZ = PID(P=0.0, I=0.0, D=0.0) self.pidX.setPoint(0.02) # in radians self.pidY.setPoint(0.03) self.pidZ.setPoint(0.0) self.start() def run(self): t = 0 while not config.exitStatus: try: if config.is_PID_new == True: self.pidX.setKp(config.PID[0]) self.pidX.setKi(config.PID[1]) self.pidX.setKd(config.PID[2]) self.pidX.setIntegrator(0) self.pidY.setKp(config.PID[0]) self.pidY.setKi(config.PID[1]) self.pidY.setKd(config.PID[2]) self.pidY.setIntegrator(0) except AttributeError: pass data = self.imu.getData() roll = data['complementaryX'] + 5.5 pitch = data['complementaryY'] - 3.4 yaw = data['complementaryZ'] x = self.pidX.update(math.sin(roll*math.pi/180.0)) y = self.pidY.update(math.sin(pitch*math.pi/180.0)) output = self.vector2Motor(x, y) self.m1.setSpeed(1.00*output[0]) self.m2.setSpeed(1.00*output[1]) self.m3.setSpeed(1.00*output[2]) sleep(0.02) print "Stablizer Thread: Shutting down." def vector2Motor(self, x, y): phase_offset = 4.107860468 # Motor to IMU angle offset (786) magnitude = math.sqrt(x*x + y*y) angle = math.atan(x/y) if y != 0 else math.pi/2.0 if y < 0: angle = angle + math.pi angle = angle + phase_offset return (magnitude*math.sin(angle + math.pi/3.0), magnitude*math.sin(angle + 5.0*math.pi/3.0), magnitude*math.sin(angle + math.pi))
class RoboClawState: """ test """ def __init__(self, imuConnectString): # go to default connect string if no string was specified if imuConnectString == None: imuConnectString ='tcp://localhost:10001/arduinoIMU/arduinoIMUData' self.m1Duty = 0 self.m2Duty = 0 #largest absolute value that the motors can be set to self.dutyMax = 5000 # control mode will be either Duty or Velocity depending on last command sent self.controlMode = 'Duty' self.canMeasureWheelVelocities = True self.pidControllerR = PID(20,0,0,0,0,0,0) self.pidControllerL = PID(-20,0,0,0,0,0,0) # try to find and connect to NRF IMU server to get wheel velocities try: self.imuGateway = RRN.ConnectService(imuConnectString) except: print "Couldn't find NRF IMU server, unable to accept velocity commands!" self.canMeasureWheelVelocities = False self._lock = threading.RLock() def setDutyMax(self, newMax): self.dutyMax = newMax # exposed to user through RR (implicitly sets to Duty mode) def setMDuties(self, leftDuty, rightDuty): print "received command" self.controlMode = 'Duty' global lastMessageTime lastMessageTime = time.time() if abs(leftDuty) > self.dutyMax: self.m1Duty = self.dutyMax * cmp(leftDuty, 0) else: self.m1Duty = leftDuty if abs(rightDuty) > self.dutyMax: self.m2Duty = self.dutyMax * cmp(rightDuty, 0) else: self.m2Duty = rightDuty # used by velocity controller to write motor duties without setting to Duty mode or updating time def internalSetDuties(self, leftDuty, rightDuty): if abs(leftDuty) > self.dutyMax: self.m1Duty = self.dutyMax * cmp(leftDuty, 0) else: self.m1Duty = leftDuty if abs(rightDuty) > self.dutyMax: self.m2Duty = self.dutyMax * cmp(rightDuty, 0) else: self.m2Duty = rightDuty def setMVelocities(self, leftV,rightV): self.controlMode = 'Velocity' global lastMessageTime lastMessageTime = time.time() #print leftV,rightV self.pidControllerL.setPoint(-leftV) self.pidControllerR.setPoint(rightV)