Foraging

# Steve Deiss, Terry Stewart 7/14
# Detect blinking light at 3 frequencies.
# Orient towards the one at 250Hz (good),
# and away from 50Hz (bad).  Approach 250.
# Retreat from 50 and reorient to look for 250.
# Height of laser (400Hz) in camera FOV provides range.
# Also upload compass to Hippocampus.  Does not yet include
# Power level monitoring - that's next.

import nengo
import nengo_pushbot
import numpy as np

model = nengo.Network()
with model:
    laser_freq=400  #blue  (image dot color)
    good_freq=250   #green
    bad_freq=50     #red
    
    #GOOD_bot = nengo_pushbot.PushBotNetwork('10.162.177.43')
    #GOOD_bot.led(good_freq)
    
    #BAD_bot = nengo_pushbot.PushBotNetwork('10.162.177.47')
    #BAD_bot.led(bad_freq)

    TANK_bot = nengo_pushbot.PushBotNetwork('10.162.177.55')
    TANK_bot.laser(laser_freq)
    TANK_bot.track_freqs([good_freq, bad_freq, laser_freq], certainty_scale=10000.0) #good-green, bad-red, laser-blue
    #TANK_bot.show_image()  # <----

    GOOD = nengo.Ensemble(100, 3, label='GOOD')
    nengo.Connection(TANK_bot.tracker_0, GOOD)

    BAD = nengo.Ensemble(100, 3, label='BAD')
    nengo.Connection(TANK_bot.tracker_1, BAD)

    TANK = nengo.Ensemble(100, 3, label='TANK')
    nengo.Connection(TANK_bot.tracker_2, TANK)

    # Perhaps the Y coordinate would work better for nearby than the laser which is low and offset?
    TARGETS = nengo.Ensemble(2000, 6, label='TARGETS',neuron_type=nengo.Direct()) # <----
    nengo.Connection(TANK_bot.tracker_0[1],TARGETS[0])  # GOOD X
    nengo.Connection(TANK_bot.tracker_1[1],TARGETS[1])  # BAD X
    nengo.Connection(TANK_bot.tracker_2[0],TARGETS[2])  # Laser Y
    nengo.Connection(TANK_bot.tracker_0[2],TARGETS[3], synapse=0.1)# GOOD certainty
    nengo.Connection(TANK_bot.tracker_1[2],TARGETS[4])  # BAD certainty
    nengo.Connection(TANK_bot.tracker_2[2],TARGETS[5])  # Laser certainty

    def victory(x):  # Supposed to buzz when it gets close to GOOD
        nearby=0.6
        if (x[3]>0.7) and (-0.1<x[0]<0.1):
            if x[2]>nearby:
                return [500]
        return [0]

    nengo.Connection(TARGETS, TANK_bot.beep, synapse=0.2, function=victory)

    def next_step(x):
        advance=0.5
        goforit=1
        retreat=2
        scram=3
        dance=10
        detect=0.7
        nearby=0.75
       #print 'good_c', x[3]
        if (x[3]<detect) and (x[4]>=detect) and (x[1]<0):
            #print 'no good present, bad on left'
        # no good present, and bad on left
            if x[2]>=-0.5: return [-1-retreat,-0.5-retreat]  # hurry away to the right, close
            elif x[2]<-0.5: return [-1-scram,-0.5-scram] # scram to the right , too close
            # turn right and go away
        elif (x[3]<detect) and (x[4]>=detect) and (x[1]>0):
            #print 'no good, bad on right'
        # no good, and bad right
            if x[2]>=-0.5: return [-0.5-retreat,-1-retreat]  # hurry away to the left, close
            elif x[2]<-0.5: return [-0.5-scram,-1-scram] # scram to the left, too close
            # turn left and go away
        elif ((x[3]>=detect) and (x[0]<-0)) and ((x[4]<detect) or ((x[4]>=detect) and (x[1]>=0))):
            #print 'good left, no bad or bad on right'
        # good left, and no bad or bad right
            if x[2]<-0.5: return [-0.75+goforit,1+goforit]
            elif x[2]>=-0.5: return [-0.4+advance,0.5+advance]
            # turn left and go forward
        elif ((x[3]>=detect) and (x[0]>5)) and ((x[4]<detect) or ((x[4]>=detect) and (x[1]<0))):
            #print 'good right, no bad or bad on left'
        # good right, and no bad or bad left
            if x[2]>=-0.5: return [1+goforit,-0.75+goforit]
            elif x[2]<-0.5: return [0.5+advance,-0.4+advance]
            # turn right and go forward
        elif (x[3]>detect) and (-0.2<x[0]<0.2): # directly ahead
            #print 'charge directly ahead', x[2]
            if x[2]>nearby: # and really close
                return [-dance,dance]
            return [3,3]
        elif (x[3]>=detect) and (x[4]>detect) and (x[0]<x[1]):
            #print 'good left, bad right'
        # good left, bad right
            if x[2]>=-0.5: return [-0.75,0]
            elif x[2]<-0.5: return [-0.25,0]
            #turn left and inch forward
        elif (x[3]>=detect) and (x[4]>detect) and (x[0]>x[1]):
            #print 'good right, bad left'
        # good right, bad left
            if x[2]>=-0.5: return [0.75,0]
            elif x[2]<-0.5: return [0.25,0]
            # turn right and inch forward
        elif (x[3]<detect) and (x[4]<detect):
            #print 'no good and no bad'
        # no good and no bad, check for and avoid obstacles
            if x[2]<=0: return [0.5+advance,-advance] # spiral forward and right
            elif x[2]<0.5: return [0.5,-0.5] # turn in place to avoid near object
            elif x[2]>=0.5: return [0.25,-0.25] # turn in place to avoid very near object
            # spiral out and right
        elif x[2]>.7: return [-3,-3]
        # Too close to something, backup
        #print 'nothing to do'
        return [0.25,-0.25]
            # fell through above ifs

    nengo.Connection(TARGETS, TANK_bot.motor, function=next_step, transform=0.05,synapse=0.002)
   
    compass = nengo.Ensemble(100, 2, label='compass')
    nengo.Connection(bot.compass[1:], compass)
    udpsend = nengo_pushbot.UDPSender('10.162.177.248', 3333, size_in=3)
    nengo.Connection(bot.compass[1:], udpsend[1:])

    #nengo.Probe(GOOD)
    #nengo.Probe(BAD)
    #nengo.Probe(TANK)

if __name__ == '__main__':
    sim = nengo.Simulator(model)
    sim.run(1000)
'''
import nengo_gui
gui = nengo_gui.Config()
gui[model].scale = 0.6091107921824119
gui[model].offset = 66.63766671857468,6.827243817716749
gui[pos0].pos = 500.000, 400.000
gui[pos0].scale = 1.000
gui[pos1].pos = 325.000, 50.000
gui[pos1].scale = 1.000
gui[bot0].pos = 0.000, 0.000
gui[bot0].scale = 1.000
gui[bot0].size = 80.000, 80.000
gui[bot0.accel].pos = 0.000, 0.000
gui[bot0.accel].scale = 1.000
gui[bot0.beep].pos = 0.000, 0.000
gui[bot0.beep].scale = 1.000
gui[bot0.compass].pos = 0.000, 0.000
gui[bot0.compass].scale = 1.000
gui[bot0.gyro].pos = 0.000, 0.000
gui[bot0.gyro].scale = 1.000
gui[bot0.motor].pos = 0.000, 0.000
gui[bot0.motor].scale = 1.000
gui[bot0.touch].pos = 0.000, 0.000
gui[bot0.touch].scale = 1.000
gui[bot].pos = 325.000, 437.500
gui[bot].scale = 1.000
ui[bot].size = 80.000, 605.000
gui[bot.tracker_0].pos = 325.000, 175.000
gui[bot.tracker_0].scale = 1.000
gui[bot.tracker_1].pos = 325.000, 250.000
gui[bot.tracker_1].scale = 1.000
gui[bot.motor].pos = 325.000, 325.000
gui[bot.motor].scale = 1.000
gui[bot.accel].pos = 325.000, 400.000
gui[bot.accel].scale = 1.000
gui[bot.beep].pos = 325.000, 475.000
gui[bot.beep].scale = 1.000
gui[bot.compass].pos = 325.000, 550.000
gui[bot.compass].scale = 1.000
gui[bot.gyro].pos = 325.000, 625.000
gui[bot.gyro].scale = 1.000
gui[bot.touch].pos = 325.000, 700.000
gui[bot.touch].scale = 1.000
'''