def __init__(self):
self.button = OneShotButton(BUTTON_PIN, False, 0)
if self.button.isPressed():
self.initialRun = "motionDemo"
print("Running Motion Demo")
else:
self.initialRun = "walking"
print("Running Walking")
self.logger = Logger('logfile.txt')
self.controller = BioloidController()
self.setupServos()
self.ikEngine = IKEngine()
self.ikEngine.setController(self.controller)
self.ikEngine.setLogger(self.logger)
self.setupHeadPosition()
self.ikEngine.setTransitionTime(TRANSITION_TIME)
self.ikEngine.setupForWalk()
self.debug = False
self.frontRangeFinder = RangeFinder(FRONT_RANGE_PIN, RANGE_MAX)
self.leftRangeFinder = RangeFinder(LEFT_RANGE_PIN, RANGE_MAX)
self.rightRangeFinder = RangeFinder(RIGHT_RANGE_PIN, RANGE_MAX)
self.readSensors()
self.shutdown = False
self.watchdogServoId = 1
self.heartbeat = HeartbeatLED(RED_LED)
self.waitingForButtonState = State("waitingForButton",
self.enterWaitingForButtonState,
self.handleWaitingForButtonState,
None)
self.waitingForNoButtonState = State(
"waitingForNoButton", self.enterWaitingForNoButtonState,
self.handleWaitingForNoButtonState, None)
self.walkingState = State("walking", self.enterWalkingState,
self.handleWalkingState, None)
self.obstacleAvoidanceState = State("obstacleAvoidance",
self.enterObstacleAvoidanceState,
self.handleObstacleAvoidanceState,
None)
self.obstacleAvoidanceScanState = State(
"obstacleAvoidanceScan", self.enterObstacleAvoidanceScanState,
self.handleObstacleAvoidanceScanState, None)
self.obstacleAvoidanceContinueState = State(
"obstacleAvoidanceContinue",
self.enterObstacleAvoidanceContinueState,
self.handleObstacleAvoidanceContinueState, None)
self.motionDemoState = State("motionDemo", self.enterMotionDemoState,
self.handleMotionDemoState, None)
self.motionDemoXYState = State("motionXYDemo",
self.enterMotionDemoXYState,
self.handleMotionDemoXYState, None)
self.shutdownState = State("shutdown", self.enterShutdownState, None,
None)
self.mainStateMachine = FiniteStateMachine(self.waitingForButtonState)
self.watchdogState = State("watchdog", None, self.handleWatchdogState,
None)
self.watchdogWaitState = State("watchdog-wait", None,
self.handleWatchdogWaitState, None)
self.watchdogStateMachine = FiniteStateMachine(self.watchdogState)
import pyb
from BioloidController import BioloidController
from Support import RangeFinder
def arduino_map(x, in_min, in_max, out_min, out_max):
return (x - in_min) * (out_max - out_min) // (in_max - in_min) + out_min
blue = pyb.LED(4)
ranger = RangeFinder('C3')
controller = BioloidController()
controller.rampServoTo(13, 306)
values = []
pyb.delay(250)
blue.on()
totalStart = pyb.millis()
for position in range(30600, 71700, 1705):
start = pyb.millis()
end = start + 40
i_pos = int(position / 100)
controller.setPosition(13, i_pos)
values.append(ranger.getDistance())
pyb.delay(max(0, end - pyb.millis()))
blue.off()
print("Total time: ", (pyb.millis() - totalStart))
# controller.rampServoTo(13, 511)
print(values)
