def get(self, desired_value, actual_value): """Calculates output signal with PID Arguments: desired_value {float} -- Desired value of system state actual_value {float} -- Actual current value of system state """ error = desired_value - actual_value self.integral = self.integral + error if error == 0: self.integral = 0 if self.first_call: self.first_call = False self.last_error = error self.derivative = ( error - self.last_error + self.derivative * self.d_fading ) self.last_error = error log('p: {}\ti: {} d: {}'.format( error * self.pk, self.integral * self.ik, self.derivative * self.dk)) return ( self.pk * error + self.ik * self.integral + self.dk * self.derivative )
def stop(self, breaks=True): moving = self.is_moving() if moving: self.move_thread.exit() if self.move_thread.is_alive(): self.move_thread.join() if breaks and moving: log('Breaking...') # active break self.lmotor.rotate(False) self.rmotor.rotate(False) sleep(self.brake_time) self.lmotor.stop() self.lmotor.stop() log('Breakin done.') if self.lmotor is not None: self.lmotor.stop() if self.rmotor is not None: self.rmotor.stop() # reset pid error self.sensor_pid.reset()
def rotate(self, degrees, stop_function=None): self.stop() log('Turning...') if degrees == 180: self.lmotor.rotate(False, self.left_motor_pow[self.TURN]) self.rmotor.rotate(True, self.right_motor_pow[self.TURN]) else: self.lmotor.rotate(degrees == 90, self.left_motor_pow[self.TURN]) self.rmotor.rotate(degrees == -90, self.right_motor_pow[self.TURN]) if stop_function is None: sleep(self.turn_time * float(abs(degrees)) / 90.0) else: start = time() # first have a sleep equal to half turn to get car started to turn sleep(1.5 * self.turn_time * float(abs(degrees)) / 180.0) enough_time = 3 * self.turn_time * float(abs(degrees)) / 90.0 while not stop_function() and time() - start < enough_time: pass # breaking... if degrees == 180: self.lmotor.rotate(True, self.left_motor_pow[self.TURN]) self.rmotor.rotate(False, self.right_motor_pow[self.TURN]) sleep(self.brake_time * 1.3) else: self.lmotor.rotate(degrees == -90, self.left_motor_pow[self.TURN]) self.rmotor.rotate(degrees == 90, self.right_motor_pow[self.TURN]) sleep(self.brake_time * 1.3) self.stop(breaks=False) super().rotate(degrees, stop_function) log('Turning finished.')
def turn_around(self): log('Brain says: turn around') self.car.stop() self.car.turn_around(stop_function=self.stop_function) self.update_turn_time() # disable brake at turns self.car.set_brake_status(False) log('Brain says: forward') self.car.move()
def __del__(self): if self.lmotor is not None: self.lmotor.stop() self.lmotor.cleanup() if self.rmotor is not None: self.rmotor.stop() self.rmotor.cleanup() log('GPIO PWMs de-initialized.')
def put(self, item): if self.__current_size >= self.max_size: for i in range(0, self.max_size - 1): self.__queue[i] = self.__queue[i + 1] self.__current_size = self.max_size - 1 try: self.__queue[self.__current_size] = item self.__current_size += 1 except: log('current_size: {}'.format(self.__current_size))
def _move(self): # get PID value based on sensor values # this is to get robot rolling straight pid, is_turning = self.sensor_pid.get_pid() l, r = self._pid_to_power(pid, is_turning) log('Power {} {}'.format(l, r)) self.lmotor.rotate(True, l) self.rmotor.rotate(True, r)
def turn(self, cw): log('Brain says to turn clockwise: {}'.format(cw)) self.car.stop() if cw: self.car.rotate_cw(stop_function=self.stop_function) else: self.car.rotate_ccw(stop_function=self.stop_function) self.update_turn_time() # disable brake at turns self.car.set_brake_status(False) log('Brain says: forward') self.car.move()
def move(self): """ Move car one step in Direction. In case it is FORWARD, moves where current Orientation is heading. If LEFT or RIGHT, turns first (changes Orientation) and quits. :param direction: Direction where to move """ if self.is_moving(): return self.chassis.move() log('Reseting sensors---------') for sensor in self.sensors: sensor.reset()
def handle(self): cipher = AESCipher() peer = self.request.getpeername() log.log(peer) print(peer) while True: req = self.request.recv(16) if len(req) > 0: req = cipher.decrypt(req) locale = req[0:4] site_id = req[4:11] # version = req[11:16] forecast = Forecast(locale, site_id) size_list, weather_forecast = forecast.get_forecast() msg = cipher.encrypt(size_list) self.request.send(msg) msg = cipher.encrypt(weather_forecast) self.request.send(msg) else: self.request.close() break
def hand_search(self, hand_direction): # get options where we can go dirs = self.car.sensors[0].get_directions() # None may mean end of the maze. Nothing to do here if dirs is None: log('Brain says: out!') return False # how long time ago we made a turn? # if too soon - don't do any checks now and just go fwd if not self.check_turn_bounce(): return True # enable brake at turns self.car.set_brake_status(True) # is this a crossing? if so - remember it if self.maze_map is not None and self.check_crossing(dirs): self.maze_map.on_crossing(self.car) if len(dirs) == 0: dirs = [Direction.FORWARD] if len(dirs) == 1 and Direction.BACK in dirs: self.turn_around() return True if hand_direction in dirs: self.turn(hand_direction == Direction.RIGHT) else: if Direction.FORWARD in dirs: self.car.move() else: self.turn(hand_direction != Direction.RIGHT) return True