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)