class MotorInterface:
#Instanciates the object
def __init__(self):
self.motor = Motor()
self.motor.enable()
'''
Goes forward x seconds
'''
def forward(self, tsec):
self.motor.forward()
time.sleep(tsec)
self.motor.stop()
'''
Goes left x seconds
'''
def left(self, tsec):
self.motor.left()
time.sleep(tsec)
self.motor.stop()
'''
Goes right x seconds
'''
def right(self, tsec):
self.motor.right()
time.sleep(tsec)
self.motor.stop()
'''
Goes back x seconds
'''
def back(self, tsec):
self.motor.backward()
time.sleep(tsec)
self.motor.stop()
'''
Stops and disables the motor
'''
def disable(self):
self.motor.stop()
self.motor.disable()
'''
Enables the motor
'''
def enable(self):
self.motor.enable()
class App(object):
date = datetime.datetime.now()
def __init__(self):
self.longitude = 13.01053
self.latitude = 56.69815
# Pins and settings for altitude motor
self.altitudeMotorStepPin = 21
self.altitudeMotorDirPin = 20
self.altitudeMotorEnablePin = 16
self.altitudeMotorPosAdress = 0 # Adress of i2c AD converter
self.altitudeMotorSPR = 400 # Steps per revolution
self.altitudeMotorMinPos = 2
self.altitudeMotorMaxPos = 70
# create motor object
self.altitudeMotor = Motor(self.altitudeMotorStepPin,
self.altitudeMotorDirPin,
self.altitudeMotorEnablePin,
self.altitudeMotorPosAdress,
self.altitudeMotorSPR,
self.altitudeMotorMinPos,
self.altitudeMotorMaxPos)
# Pins and settings for azimuth motor
self.azimuthMotorStepPin = 26
self.azimuthMotorDirPin = 19
self.azimuthMotorEnablePin = 13
self.azimuthMotorPosAdress = 1 # Adress of i2c AD converter
self.azimuthMotorSPR = 400 # Steps per revolution
self.azimuthMotorMinPos = 20
self.azimuthMotorMaxPos = 280
# create motor object
self.azimuthMotor = Motor(self.azimuthMotorStepPin,
self.azimuthMotorDirPin,
self.azimuthMotorEnablePin,
self.azimuthMotorPosAdress,
self.azimuthMotorSPR,
self.azimuthMotorMinPos,
self.azimuthMotorMaxPos)
# settings
self.updateTime = 10.0
self.sunAltitude = 0.0
self.sunAzimuth = 0.0
self.jogCCW = 23
self.jogCW = 24
self.jogMode = 25
self.jogAltMotor = 18
self.wentToSunrise = False
self.running = False
self.isRunning = 6
self.setup_pins()
print('Init done')
def check_altitude(self):
print("Altitude motor pos:{} Sun altitude:{}".format(self.altitudeMotor.get_pos(), self.sunAltitude))
if(self.sunAltitude > self.altitudeMotor.get_pos() and (self.sun_going_up(self.sunAltitude)) or self.sunAltitude - self.altitudeMotor.get_pos() > 2.0):
self.altitudeMotor.move(self.sunAltitude, False)
elif(self.sunAltitude < self.altitudeMotor.get_pos() and not self.sun_going_up(self.sunAltitude)):
self.altitudeMotor.move(self.sunAltitude, True)
else:
pass
def sun_going_up(self, current_alt):
"""Check if sun is still rising"""
new_time = self.date + datetime.timedelta(minutes=1)
future_alt = solar.get_altitude(self.latitude, self.longitude, new_time)
return current_alt < future_alt
def check_azimuth(self):
print("Azimuth motor pos:{} Sun azimuth:{}".format(self.azimuthMotor.get_pos(), self.sunAzimuth))
if(self.sunAzimuth > self.azimuthMotor.get_pos()):
self.azimuthMotor.move(self.sunAzimuth, False)
def setup_pins(self):
"""Ställ in GPIO utgångar"""
GPIO.setmode(GPIO.BCM)
GPIO.setup(self.altitudeMotorStepPin, GPIO.OUT)
GPIO.setup(self.altitudeMotorDirPin, GPIO.OUT)
GPIO.setup(self.altitudeMotorEnablePin, GPIO.OUT)
GPIO.setup(self.azimuthMotorStepPin, GPIO.OUT)
GPIO.setup(self.azimuthMotorDirPin, GPIO.OUT)
GPIO.setup(self.azimuthMotorEnablePin, GPIO.OUT)
GPIO.setup(self.isRunning, GPIO.OUT)
GPIO.setup(self.jogCCW, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.setup(self.jogCW, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.setup(self.jogMode, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.setup(self.jogAltMotor, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
def get_sun_pos(self):
"""Get sun altitude and azimuth using pysolar"""
self.date = datetime.datetime.now()
self.sunAltitude = solar.get_altitude(self.latitude, self.longitude, self.date)
self.sunAzimuth = self.offset(solar.get_azimuth(self.latitude, self.longitude, self.date))
def offset(self, sun_value):
"""offset between pot and pysolar"""
offset = 180.0
if abs(sun_value) >= 0 and abs(sun_value) < 180:
return abs(sun_value) + offset
else:
return abs(sun_value + offset)
def run(self):
"""Start app and main loop"""
startTime = time.time()
self.running = True
while self.running:
GPIO.output(self.isRunning, GPIO.HIGH)
if(time.time() > (startTime + self.updateTime) and not self.jog_mode()):
self.update()
startTime = time.time()
elif self.jog_mode():
self.jog()
def jog(self):
if GPIO.input(self.jogCW) and not GPIO.input(self.jogCCW):
if not GPIO.input(self.jogAltMotor):
self.azimuthMotor.jog(self.jogCW, False)
else:
self.altitudeMotor.jog(self.jogCW, False)
elif GPIO.input(self.jogCCW) and not GPIO.input(self.jogCW):
if not GPIO.input(self.jogAltMotor):
self.azimuthMotor.jog(self.jogCCW, True)
else:
self.altitudeMotor.jog(self.jogCCW, True)
def update(self):
"""Update App ,general things to do every cycle"""
self.get_sun_pos()
if(self.sunAltitude > 15.0):
self.check_altitude()
self.check_azimuth()
self.wentToSunrise = False
elif(self.sunAltitude < 15.0 and not self.wentToSunrise):
print('Sun to low\n')
next_pos = self.check_next_sunrise()
print('Going to {} pos.\n'.format(next_pos))
self.azimuthMotor.move(next_pos, True)
self.wentToSunrise = True
else:
pass
def check_next_sunrise(self):
"""Check azimuth of next sunrise"""
new_date = self.date
while(solar.get_altitude(self.latitude, self.longitude, new_date) < 15.0):
print(new_date)
new_date += datetime.timedelta(minutes=10)
return self.offset(solar.get_azimuth(self.latitude, self.longitude, new_date))
def jog_mode(self):
return GPIO.input(self.jogMode)
def on_exit(self):
"""cleanup when app closes"""
self.altitudeMotor.disable()
self.azimuthMotor.disable()
GPIO.output(self.isRunning, GPIO.LOW)
class Driver:
def __init__(self):
motors = parse_config()['motors']
self.motor_left = Motor(en_pin=motors['en_pin'],
slew_pin=motors['slew_pin'],
pwm_pin=motors['left']['pwm_pin'],
forward_pin=motors['left']['forward_pin'],
reverse_pin=motors['left']['reverse_pin'],
sf_pin=motors['left']['state_flag_pin'],
name='left')
self.motor_right = Motor(en_pin=motors['en_pin'],
slew_pin=motors['slew_pin'],
pwm_pin=motors['right']['pwm_pin'],
forward_pin=motors['right']['forward_pin'],
reverse_pin=motors['right']['reverse_pin'],
sf_pin=motors['right']['state_flag_pin'],
name='right')
self.motor_left.disable()
self.motor_right.disable()
# todo: Implement a 2d matrix to represent direction and duration of previous movement.
self.travel_memory = list()
self.turn_a = 22.5
self.turn_limit = 180 / self.turn_a
self.last_turn = None
self.turns = 0
self.turn_speed = 440
self.opposites = {"left": "right", "right": "left"}
def start(self):
self.motor_left.disable()
self.motor_right.disable()
while not rd.current_heading or not rd.current_distances:
logging.debug("Waiting for sensor data")
logging.debug(rd.current_heading)
logging.debug(rd.current_distances)
sleep(1)
logging.info('Sensor data populated')
self.navigate()
def navigate(self):
while True:
# If its clear ahead left and right continue forwards.
if rd.c_clear and rd.l_clear and rd.r_clear:
logging.warning('I think its clear... driving')
self.drive()
# todo: Use travel_memory to map previous movement
self.travel_memory.append(
[rd.current_heading, int(time.time())])
self.turns = 0
else:
# Evaluate turn options
decision = self.evaluate_turn()
logging.info(f'Decided to turn {decision}')
self.turn(decision)
def evaluate_turn(self):
logging.warning(
f"c_clear: {rd.c_clear} l_clear: {rd.l_clear} r_clear: {rd.r_clear}"
)
# If we just drove here, Use USS to determine turn.
if (self.last_turn is None and rd.l_clear) or (rd.c_clear
and rd.l_clear):
turn_choice = 'left'
elif (self.last_turn is None and rd.r_clear) or (rd.c_clear
and rd.r_clear):
turn_choice = 'right'
# Turn in the same direction until we find a gap
elif self.turns <= self.turn_limit and self.last_turn is not None:
turn_choice = self.last_turn
# If we turn 180 degrees we go back the other way. (prevent erroneous physical navigation loop)
elif self.turns >= self.turn_limit:
turn_choice = self.opposites[self.last_turn]
# Double the turn limit to turn back the other way.
logging.info(f'Negating turn count: {self.turns}')
self.turns = (0 - self.turn_limit)
else:
# Its the first turn at this position
turn_choice = choice(['left', 'right'])
if turn_choice == 'left':
self.last_turn = turn_choice
return -self.turn_a
elif turn_choice == 'right':
self.last_turn = turn_choice
return self.turn_a
else:
self.last_turn = self.opposites[self.last_turn]
return -180
# Drive in a straight(ish) line
def drive(self, distance_cm: int = 10, direction: int = 0):
self.last_turn = None
self.turns = 0
if direction is 1:
direction_str = 'Forward'
else:
direction_str = 'Reverse'
logging.debug(f'Driving {distance_cm} centimeters {direction_str}')
initial_distance = rd.current_distances[1]
initial_time = time.time()
motors_on = False
while True:
if not (rd.c_clear and rd.l_clear and rd.r_clear):
# Something got in the way
return False
# We only drive for 1000ms or 10cm whichever is first.
if time.time() > initial_time + 1:
return True
if not rd.current_heading or not rd.current_distances:
self.motor_left.disable()
self.motor_right.disable()
logging.warning('Lost navigation data')
continue
# todo: discard outliers in ultrasonic thread.
# If we have driven the distance_cm we return to parent loop
if rd.current_distances[1] <= (initial_distance - distance_cm):
self.motor_left.disable()
self.motor_right.disable()
return True
# We were not currently driving so we drive.
if not motors_on:
self.motor_left.move(1)
self.motor_right.move(1)
motors_on = True
@staticmethod
def heading_calc(angle, initial_heading):
if initial_heading + angle > 360:
desired_heading = (initial_heading + angle) - 360
elif initial_heading + angle < 0:
desired_heading = (initial_heading + angle) + 360
else:
desired_heading = rd.current_heading + angle
return desired_heading
def turn(self, angle: int = 0, hard: bool = False):
if angle is 0:
logging.error('I cant turn 0 degrees!')
return False
self.turns += 1
initial_heading = rd.current_heading
# If we lose magnetometer heading we wait
while initial_heading is None:
logging.warning('Waiting for heading')
initial_heading = rd.current_heading
desired_heading = self.heading_calc(angle, initial_heading)
logging.debug(f'Desired Heading: {desired_heading}')
# Right turn
if angle > 0:
driving = False
logging.debug(f'Initial heading: {initial_heading}')
while True:
# If we lose magnetometer heading mid-turn we return to navigation loop.
if not rd.current_heading:
logging.warning(
f'Current_heading is {type(rd.current_heading)}')
return False
# If a path clears mid turn return to navigation loop
if rd.c_path and rd.l_clear and rd.r_clear:
self.motor_left.disable()
self.motor_right.disable()
return True
if self.approximate_angle(desired_heading, rd.current_heading):
logging.debug(f'Current Heading: {rd.current_heading}')
logging.debug(
f"{angle} Turn complete attempting to disable motors")
self.motor_left.disable()
self.motor_right.disable()
return True
if driving is not True:
self.motor_left.move(1, speed=self.turn_speed)
self.motor_right.move(0, speed=self.turn_speed)
driving = True
# Left turn
if angle < 0:
driving = False
logging.debug(f'Initial heading: {initial_heading}')
while True:
if not rd.current_heading:
logging.warning(
f'Current_heading is {type(rd.current_heading)}')
return False
# If a path clears mid turn return
if rd.c_path and rd.l_clear and rd.r_clear:
self.motor_left.disable()
self.motor_right.disable()
return True
if self.approximate_angle(desired_heading, rd.current_heading):
logging.debug(f'Current Heading: {rd.current_heading}')
logging.debug(f"{angle} Turn complete disabling motors")
self.motor_left.disable()
self.motor_right.disable()
return True
if driving is not True:
self.motor_left.move(0, speed=self.turn_speed)
self.motor_right.move(1, speed=self.turn_speed)
driving = True
logging.debug('Finished turn loop')
def abort(self):
logging.debug('Aborting')
self.motor_left.disable()
self.motor_right.disable()
@staticmethod
def approximate_angle(desired, current):
try:
fuzzy = 5
if current >= (desired - fuzzy) and current <= (desired + fuzzy):
return True
else:
return False
except TypeError as e:
logging.debug(f'Exception caught attempting None comparison')
