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
