def __init__(self):
#setup gpio
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(12, GPIO.OUT)
self.mcp = analog.Analog()
self.motor2 = motor.Motor(19, 20)
self.motor1 = motor.Motor(26, 21)
self.sonic1 = sonic.Sonic(18, 23)
self.sonic4 = sonic.Sonic(25, 24)
self.sonic2 = sonic.Sonic(16, 12)
self.sonic3 = sonic.Sonic(6, 13)
self.sonic5 = sonic.Sonic(22, 5)
self.sense = heading.Heading()
self.data_status = False
self.wpath = os.getcwd() + "/data/" + time.strftime("%H%M%S") + ".csv"
self.file = open(self.wpath, "w")
self.file.write("time, motor, pwm\n")
with open('data/saved_net.pickle', 'rb') as handle:
self.net = pickle.load(handle)
# self.sensor_collect = open("data/sensors.txt","w")
# self.sensor_collect.write("time, sonic1, sonic2, sonic3, sonic4, sonic5\n")
self.data = list()
self.collecting = False
#setup controller values
self.state = 0
#create xbox controller class
self.xboxCont = XboxController.XboxController(joystickNo=0,
deadzone=0.1,
scale=1,
invertYAxis=False)
#setup call backs
self.xboxCont.setupControlCallback(self.xboxCont.XboxControls.A,
self.aButtonCallBack)
self.xboxCont.setupControlCallback(self.xboxCont.XboxControls.BACK,
self.backButton)
self.xboxCont.setupControlCallback(self.xboxCont.XboxControls.LTHUMBY,
self.LthumbY)
self.xboxCont.setupControlCallback(self.xboxCont.XboxControls.RTHUMBY,
self.RthumbY)
#start the controller
self.xboxCont.start()
self.running = True
def learn_location(signatures, _dir):
ls = LocationSignature()
_dir, readings = characterize_location(sonic.Sonic(), _dir)
ls.sig[0:len(readings)] = readings
idx = signatures.get_free_index()
if idx == -1: # run out of signature files
print "\nWARNING:"
print "No signature file is available. NOTHING NEW will be learned and stored."
print "Please remove some loc_%%.dat files.\n"
return _dir, readings
signatures.save(ls, idx)
print "STATUS: Location " + str(idx) + " learned and saved."
return _dir
def mcl(oldParticles):
sn = sonic.Sonic()
z = sn.getSonar() + sonarToCenter
if z == -1:
print "Skipping MCL as sonar distance is unreliable"
return tuple(oldParticles)
newParticles = []
for particle in oldParticles:
likelihood = calculate_likelihood(particle[0], particle[1], particle[2], z)
newWeight = likelihood * particle[3]
newParticle = (particle[0], particle[1], particle[2], newWeight)
newParticles.append(newParticle)
normalisedParticles = normalisation(newParticles)
resampledParticles = resampling(normalisedParticles)
newParticles = resampledParticles
return newParticles
waypointd, waypointde, waypointe, waypointef, waypointf
]
# canvas = pds.Canvas()
# mymap = pds.Map()
# pds.drawWall(mymap, canvas)
particles = [initialPosition for i in range(numberOfParticles)]
# canvas.drawParticles(particles)
signatures = prb.SignatureContainer()
# bumper = bumper.Bumper()
# bumperThread = threading.Thread(name='bumper', target=bumper.getTouch)
# bumperThread.start()
_sonnic = sonic.Sonic()
_gostraight = gostraight.go()
_dir = 1
def ConvertToHisto(readings):
step = 10
s = [0] * (200 / step)
for currenttuple in readings:
currentAngle = currenttuple[0]
currentDistance = currenttuple[1]
for idx, freq in enumerate(s):
# print "Comparing current Distance " + str(currentDistance) + " against " + str(step*(idx+1))
if (step * (idx + 1) > currentDistance):
def __init__(self, sonic_trig=SONIC_TRIG, sonic_echo=SONIC_ECHO):
self.car = buggy.Buggy()
self.snc= sonic.Sonic(sonic_trig, sonic_echo)
def load_img(name):
return pygame.image.load("./assets/" + name + ".png").convert_alpha()
window = pygame.display.set_mode((640, 480))
pygame.display.set_caption("Le reve de Robotnik")
background = load_img("background")
ground = load_img("ground")
tree1 = load_img("tree1")
tree2 = load_img("tree2")
sunflower = load_img("sunflower")
totem = load_img("totem")
sonic = sonic.Sonic(load_img("sonic"))
i = 0
running = True
while running:
for event in pygame.event.get():
if event.type == QUIT:
running = False
if event.type == KEYDOWN:
i += 1
window.blit(background, (0, 0))
window.blit(tree1, (32, 140))
window.blit(tree1, (149, 125))
def __init__(self, max_dist_len=25, check_interval=0.1):
self.enabled = True
self.max_dist_len = max_dist_len
self.check_interval = check_interval
self.car = buggy.Buggy()
self.snc = sonic.Sonic(SONIC_TRIG, SONIC_ECHO)
