def __init__(self, m1, m2, jib=None, dist=None, verbose=False):
self.m1 = m1
self.m2 = m2
self.acc = Accel()
self.dist = dist
self.jib = jib
self.motorlog = []
# CURRENT STATE
self.power = 0
self.steerpower = 0
self.minpower = 30
self.minsteerpower = 20
# DEBUG
self.verbose = verbose
print "Hi, I'm Rover... hopefully I won't roll over!"
def __init__(self, m1, m2, jib=None, dist=None, verbose=False):
self.m1 = m1
self.m2 = m2
self.acc = Accel()
self.dist = dist
self.jib = jib
self.motorlog = []
# CURRENT STATE
self.power = 0
self.steerpower = 0
self.minpower = 30
self.minsteerpower = 20
# DEBUG
self.verbose = verbose
print "Hi, I'm Rover... hopefully I won't roll over!"
class Rover():
def __init__(self, m1, m2, jib=None, dist=None, verbose=False):
self.m1 = m1
self.m2 = m2
self.acc = Accel()
self.dist = dist
self.jib = jib
self.motorlog = []
# CURRENT STATE
self.power = 0
self.steerpower = 0
self.minpower = 30
self.minsteerpower = 20
# DEBUG
self.verbose = verbose
print "Hi, I'm Rover... hopefully I won't roll over!"
def keycontrol(self):
mapping = {
# KEY BINDINGS: key to function (with default value)
# ROVER KEYS
'w': 'fw',
's': 'rw',
'a': 'left',
'd': 'right',
' ': 'stop',
'r': 'init_log',
't': 'save_log',
# JIB KEYS
'i': 'jib.moveup',
'j': 'jib.moveleft',
'k': 'jib.movedw',
'l': 'jib.moveright',
# MEASURE DISTANCE
'm': 'print_distance'
}
print "ROVER KEYPAD: initialized"
keypad.keypad(self, mapping)
print "ROVER KEYPAD: terminated"
# DIRECTION
def stop(self):
self.m1.stop()
self.m2.stop()
self.power = 0
self.motorlog.append([time(), 'stop', 0, 0])
if self.verbose: print "stop | m1: %i | m2: %i" % (0, 0)
def fw(self, seconds=None):
#TODO review
if m1.use_pwm and m2.use_pwm:
if self.state == 'rw': self.power = 0
self.power = min(100, max(self.minpower, self.power + 10))
self.steerpower = 0
self._fw(seconds, power=self.power)
else:
self._fw(seconds)
def rw(self, seconds=None):
#TODO review
if m1.use_pwm and m2.use_pwm:
if self.state == 'fw': self.power = 0
self.power = min(100, max(self.minpower, self.power + 10))
self.steerpower = 0
self._rw(seconds, power=self.power)
else:
self._fw(seconds)
def left(self, seconds=None):
#TODO review
if m1.use_pwm and m2.use_pwm:
self.steerpower = min(
100, max(self.minsteerpower, self.steerpower + 10))
self._left(seconds, steerpower=self.steerpower)
else:
self._left(seconds)
def right(self, seconds=None):
#TODO review
if m1.use_pwm and m2.use_pwm:
self.steerpower = min(
100, max(self.minsteerpower, self.steerpower + 10))
self._right(seconds, steerpower=self.steerpower)
else:
self._right(seconds)
def _fw(self, seconds=None, power=100):
#TODO review
self.m1.power(power=power)
self.m2.power(power=power)
self.motorlog.append([time(), 'fw', power, power])
if self.verbose:
print "fw: %i%% | m1.fw: %i | m2.fw: %i" % (power, power, power)
if seconds is not None:
sleep(seconds)
self.stop()
def _rw(self, seconds=None, power=100):
#TODO review
self.m1.power(power=-power)
self.m2.power(power=-power)
self.motorlog.append([time(), 'rw', power, power])
if self.verbose:
print "rw: %i%% | m1.rw: %i | m2.rw %i" % (power, power, power)
if seconds is not None:
sleep(seconds)
self.stop()
def _left(self, seconds=None, steerpower=100):
#TODO review
if m1.use_pwm and m2.use_pwm:
# if the rover is not moving, rotating power is forced to 100
if self.power == 0:
steerpower = 100
self.power = 100
m2power = self.power
if steerpower >= 50:
# STRONG TURN (inverted direction on motors)
m1power = ((steerpower - 50) * 2 / 100.) * self.power
self.m1.power(m1power * np.sign(power))
m1dir = 'rw' if self.state == 'fw' else 'fw'
else:
# LIGHT TURN (same direction, but slower motors)
m1power = ((50 - steerpower) * 2 / 100.) * self.power
self.m1.fw(
power=m1power) if self.state == 'fw' else self.m1.rw(
power=m1power)
m1dir = 'fw' if self.state == 'fw' else 'rw'
# adjust m2 power to be at least twice the minimum combined power
m2power += max(0, self.minpower * 2 - (m1power + m2power))
self.m2.rw(power=m2power) if self.state == 'rw' else self.m2.fw(
power=m2power)
m2dir = 'rw' if self.state == 'rw' else 'fw'
if self.verbose:
print "left: %i%% | m1.%s: %i | m2.%s %i" % (
steerpower, m1dir, m1power, m2dir, m2power)
else:
self.m1.rw(power=self.power)
self.m2.fw(power=self.power)
m1power, m2power = self.power, self.power
# log motor
self.motorlog.append([time(), 'left', m1power, m2power])
if seconds is not None:
sleep(seconds)
self.stop()
def _right(self, seconds=None, steerpower=100):
#TODO review
if m1.use_pwm and m2.use_pwm:
# if the rover is not moving, rotating power is forced to 100
if self.state == 'stop':
steerpower = 100
self.power = 100
self.state = 'fw'
m1power = self.power
if steerpower >= 50:
# STRONG TURN (inverted direction on motors)
m2power = ((steerpower - 50) * 2 / 100.) * self.power
self.m2.fw(
power=m2power) if self.state == 'rw' else self.m2.rw(
power=m2power)
m2dir = 'fw' if self.state == 'rw' else 'rw'
else:
# LIGHT TURN (same direction, but slower motors)
m2power = ((50 - steerpower) * 2 / 100.) * self.power
self.m2.fw(
power=m2power) if self.state == 'fw' else self.m2.rw(
power=m2power)
m2dir = 'fw' if self.state == 'fw' else 'rw'
# adjust m2 power to be at least twice the minimum combined power
m1power += max(0, self.minpower * 2 - (m2power + m1power))
self.m1.fw(power=m1power) if self.state == 'fw' else self.m1.rw(
power=m1power)
m1dir = 'fw' if self.state == 'fw' else 'rw'
if self.verbose:
print "left: %i%% | m1.%s: %i | m2.%s %i" % (
steerpower, m1dir, m1power, m2dir, m2power)
else:
self.m1.fw(power=self.power)
self.m2.rw(power=self.power)
m1power, m2power = self.power, self.power
# log motor
self.motorlog.append([time(), 'right', m1power, m2power])
if seconds is not None:
sleep(seconds)
self.stop()
def distance(self):
if self.dist is not None:
return self.dist.measure()
else:
return None
def print_distance(self):
dist = self.distance()[1]
if dist is None: print "Distance Measurement not initialized"
else: print "Distance: %.2fcm" % dist
# LOGGING
def init_log(self, seconds=3600, interval=0.010):
self.motorlog = []
self.acc.samples = [] # reset motor logger
self.stop() # stop car
self.acc.sampler(seconds, interval) # start reading accelerometer
def save_log(self, savepath=None):
self.stop()
acclog = self.acc.get_samples()
import pandas as pd
acclog = pd.DataFrame(acclog, columns=['time', 'x', 'y',
'z']).set_index('time')
carlog = pd.DataFrame(self.motorlog,
columns=['time', 'action']).set_index('time')
carlog['seq'] = range(len(carlog))
data = pd.merge(acclog,
carlog,
how='outer',
left_index=True,
right_index=True)
# reset data with star date
data.index = data.index.values - data.index.values.min()
if savepath is not None:
if savepath == True: savepath = ''
data.to_csv(datetime.now().strftime(savepath +
'carlog_%Y%m%d_%H%M%S.csv'))
return data
from shortrangemethod import shortrange_Class from minisumo_motorcontrol2 import Motors_Class from accelerometer import Accel lineSensors = lineSensor_Class() motors = Motors_Class() longrange = longrange_Class() shortrange = shortrange_Class() short = shortrange.rngsens longone = longrange.rangesens # l1 = lineSensors.check1() # l2 = lineSensors.check2() # l3 = lineSensors.check3() l4 = lineSensors.check4() results = Accel.mag_accel() # test variables # l1 = 0 # l2 = 1 # l3 = 1 # l4 = 1 # short = 12 # long = 60 # ax = -4 axtotal = results[0] aytotal = results[1] vx = results[2] vy = results[3]
class Rover:
def __init__(self, m1, m2, jib=None, dist=None, verbose=False):
self.m1 = m1
self.m2 = m2
self.acc = Accel()
self.dist = dist
self.jib = jib
self.motorlog = []
# CURRENT STATE
self.power = 0
self.steerpower = 0
self.minpower = 30
self.minsteerpower = 20
# DEBUG
self.verbose = verbose
print "Hi, I'm Rover... hopefully I won't roll over!"
def keycontrol(self):
mapping = {
# KEY BINDINGS: key to function (with default value)
# ROVER KEYS
"w": "fw",
"s": "rw",
"a": "left",
"d": "right",
" ": "stop",
"r": "init_log",
"t": "save_log",
# JIB KEYS
"i": "jib.moveup",
"j": "jib.moveleft",
"k": "jib.movedw",
"l": "jib.moveright",
# MEASURE DISTANCE
"m": "print_distance",
}
print "ROVER KEYPAD: initialized"
keypad.keypad(self, mapping)
print "ROVER KEYPAD: terminated"
# DIRECTION
def stop(self):
self.m1.stop()
self.m2.stop()
self.power = 0
self.motorlog.append([time(), "stop", 0, 0])
if self.verbose:
print "stop | m1: %i | m2: %i" % (0, 0)
def fw(self, seconds=None):
# TODO review
if m1.use_pwm and m2.use_pwm:
if self.state == "rw":
self.power = 0
self.power = min(100, max(self.minpower, self.power + 10))
self.steerpower = 0
self._fw(seconds, power=self.power)
else:
self._fw(seconds)
def rw(self, seconds=None):
# TODO review
if m1.use_pwm and m2.use_pwm:
if self.state == "fw":
self.power = 0
self.power = min(100, max(self.minpower, self.power + 10))
self.steerpower = 0
self._rw(seconds, power=self.power)
else:
self._fw(seconds)
def left(self, seconds=None):
# TODO review
if m1.use_pwm and m2.use_pwm:
self.steerpower = min(100, max(self.minsteerpower, self.steerpower + 10))
self._left(seconds, steerpower=self.steerpower)
else:
self._left(seconds)
def right(self, seconds=None):
# TODO review
if m1.use_pwm and m2.use_pwm:
self.steerpower = min(100, max(self.minsteerpower, self.steerpower + 10))
self._right(seconds, steerpower=self.steerpower)
else:
self._right(seconds)
def _fw(self, seconds=None, power=100):
# TODO review
self.m1.power(power=power)
self.m2.power(power=power)
self.motorlog.append([time(), "fw", power, power])
if self.verbose:
print "fw: %i%% | m1.fw: %i | m2.fw: %i" % (power, power, power)
if seconds is not None:
sleep(seconds)
self.stop()
def _rw(self, seconds=None, power=100):
# TODO review
self.m1.power(power=-power)
self.m2.power(power=-power)
self.motorlog.append([time(), "rw", power, power])
if self.verbose:
print "rw: %i%% | m1.rw: %i | m2.rw %i" % (power, power, power)
if seconds is not None:
sleep(seconds)
self.stop()
def _left(self, seconds=None, steerpower=100):
# TODO review
if m1.use_pwm and m2.use_pwm:
# if the rover is not moving, rotating power is forced to 100
if self.power == 0:
steerpower = 100
self.power = 100
m2power = self.power
if steerpower >= 50:
# STRONG TURN (inverted direction on motors)
m1power = ((steerpower - 50) * 2 / 100.0) * self.power
self.m1.power(m1power * np.sign(power))
m1dir = "rw" if self.state == "fw" else "fw"
else:
# LIGHT TURN (same direction, but slower motors)
m1power = ((50 - steerpower) * 2 / 100.0) * self.power
self.m1.fw(power=m1power) if self.state == "fw" else self.m1.rw(power=m1power)
m1dir = "fw" if self.state == "fw" else "rw"
# adjust m2 power to be at least twice the minimum combined power
m2power += max(0, self.minpower * 2 - (m1power + m2power))
self.m2.rw(power=m2power) if self.state == "rw" else self.m2.fw(power=m2power)
m2dir = "rw" if self.state == "rw" else "fw"
if self.verbose:
print "left: %i%% | m1.%s: %i | m2.%s %i" % (steerpower, m1dir, m1power, m2dir, m2power)
else:
self.m1.rw(power=self.power)
self.m2.fw(power=self.power)
m1power, m2power = self.power, self.power
# log motor
self.motorlog.append([time(), "left", m1power, m2power])
if seconds is not None:
sleep(seconds)
self.stop()
def _right(self, seconds=None, steerpower=100):
# TODO review
if m1.use_pwm and m2.use_pwm:
# if the rover is not moving, rotating power is forced to 100
if self.state == "stop":
steerpower = 100
self.power = 100
self.state = "fw"
m1power = self.power
if steerpower >= 50:
# STRONG TURN (inverted direction on motors)
m2power = ((steerpower - 50) * 2 / 100.0) * self.power
self.m2.fw(power=m2power) if self.state == "rw" else self.m2.rw(power=m2power)
m2dir = "fw" if self.state == "rw" else "rw"
else:
# LIGHT TURN (same direction, but slower motors)
m2power = ((50 - steerpower) * 2 / 100.0) * self.power
self.m2.fw(power=m2power) if self.state == "fw" else self.m2.rw(power=m2power)
m2dir = "fw" if self.state == "fw" else "rw"
# adjust m2 power to be at least twice the minimum combined power
m1power += max(0, self.minpower * 2 - (m2power + m1power))
self.m1.fw(power=m1power) if self.state == "fw" else self.m1.rw(power=m1power)
m1dir = "fw" if self.state == "fw" else "rw"
if self.verbose:
print "left: %i%% | m1.%s: %i | m2.%s %i" % (steerpower, m1dir, m1power, m2dir, m2power)
else:
self.m1.fw(power=self.power)
self.m2.rw(power=self.power)
m1power, m2power = self.power, self.power
# log motor
self.motorlog.append([time(), "right", m1power, m2power])
if seconds is not None:
sleep(seconds)
self.stop()
def distance(self):
if self.dist is not None:
return self.dist.measure()
else:
return None
def print_distance(self):
dist = self.distance()[1]
if dist is None:
print "Distance Measurement not initialized"
else:
print "Distance: %.2fcm" % dist
# LOGGING
def init_log(self, seconds=3600, interval=0.010):
self.motorlog = []
self.acc.samples = [] # reset motor logger
self.stop() # stop car
self.acc.sampler(seconds, interval) # start reading accelerometer
def save_log(self, savepath=None):
self.stop()
acclog = self.acc.get_samples()
import pandas as pd
acclog = pd.DataFrame(acclog, columns=["time", "x", "y", "z"]).set_index("time")
carlog = pd.DataFrame(self.motorlog, columns=["time", "action"]).set_index("time")
carlog["seq"] = range(len(carlog))
data = pd.merge(acclog, carlog, how="outer", left_index=True, right_index=True)
# reset data with star date
data.index = data.index.values - data.index.values.min()
if savepath is not None:
if savepath == True:
savepath = ""
data.to_csv(datetime.now().strftime(savepath + "carlog_%Y%m%d_%H%M%S.csv"))
return data
from shortrangemethod import shortrange_Class from minisumo_motorcontrol2 import Motors_Class from accelerometer import Accel lineSensors = lineSensor_Class() motors = Motors_Class() longrange = longrange_Class() shortrange = shortrange_Class() short = shortrange.rngsens longone = longrange.rangesens # l1 = lineSensors.check1() # l2 = lineSensors.check2() # l3 = lineSensors.check3() l4 = lineSensors.check4() results = Accel.mag_accel() # test variables # l1 = 0 # l2 = 1 # l3 = 1 # l4 = 1 # short = 12 # long = 60 # ax = -4 axtotal = results[0] aytotal = results[1] vx = results[2] vy = results[3]
from lineSense2 import lineSensor_Class
from longrangemethod import longrange_Class
from shortrangemethod import shortrange_Class
from minisumo_motorcontrol2 import Motors_Class
from minisumo_motorcontrol3 import Motors_Class2
from accelerometer import Accel
import time
lineSensors = lineSensor_Class()
Accel1 = Accel()
motor1 = Motors_Class()
motor2 = Motors_Class2()
longrange = longrange_Class()
shortrange = shortrange_Class()
try:
while True:
motor1.motor_move('a', 3)
motor2.motor_move('a', 3)
shortrange.rngsens()
lineSensors.check1()
lineSensors.check2()
lineSensors.check3()
lineSensors.check4()
longrange.rangesens()
except KeyboardInterrupt:
GPIO.cleanup()
