class WallFollower(sm.SM):
startState = 0.5
def getNextValues(self, state, inp):
currentError = desiredRight - inp
lastError = desiredRight - state
k1 = 100
k2 = -97.36
velocity = k1 * currentError + k2 * lastError
output = io.Action(fvel=forwardVelocity, rvel=velocity)
return inp, output
sensorMachine = Sensor()
sensorMachine.name = 'sensor'
mySM = sm.Cascade(sensorMachine, WallFollower())
######################################################################
#
# Running the robot
#
######################################################################
def setup():
robot.gfx = gfx.RobotGraphics(drawSlimeTrail=False)
robot.gfx.addStaticPlotSMProbe(y=('rightDistance', 'sensor', 'output',
lambda x: x))
robot.behavior = mySM
robot.behavior.start(traceTasks=robot.gfx.tasks())
def getNextValues(self, state, inp):
#################
k = -25
dDesired = 0.5
nextState = 0
diff = inp - dDesired
turn = (k * diff)
return (nextState, io.Action(fvel=0.1, rvel=turn))
################
pass
mySM = sm.Cascade(Sensor(), WallFollower())
######################################################################
#
# Running the robot
#
######################################################################
def plotDist():
func = lambda: sonarDist.getDistanceRight(io.SensorInput().sonars)
robot.gfx.addStaticPlotFunction(y=('d_o', func))
def setup():
robot.gfx = gfx.RobotGraphics(drawSlimeTrail=False)
class Controller(sm.SM):
def getNextValues(self, state, inp):
#Your code here
pass
# Input is SensorInput instance; output is a delayed front sonar reading
class Sensor(sm.SM):
def __init__(self, initDist, numDelays):
self.startState = [initDist]*numDelays
def getNextValues(self, state, inp):
print inp.sonars[2]
output = state[-1]
state = [inp.sonars[2]] + state[:-1]
return (state, output)
mySM = sm.Cascade(Sensor(1.5, 1), Controller())
mySM.name = 'brainSM'
######################################################################
###
### Brain methods
###
######################################################################
def plotSonar(sonarNum):
robot.gfx.addStaticPlotFunction(y=('sonar'+str(sonarNum),
lambda: io.SensorInput().sonars[sonarNum]))
def setup():
robot.gfx = gfx.RobotGraphics(drawSlimeTrail=False)
plotSonar(2)
