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 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()