def move_thread(kill, pin, steps=-1, default_ramp_step = 2000, min_sleep = 100-15, stop_condition = None):
""" run this biatch """
STOP_CONDITION_INTERVAL = 50
stop_condition = stop_condition or (lambda: False); #Default: No stop conditions
bbio.pinMode(pin, bbio.OUTPUT)
step = 0
ramp_step = float(default_ramp_step if steps == -1 else min(default_ramp_step, steps))
ramp_sleep = 100.0
#ramp_sleep_decrement = ramp_sleep / (ramp_step*ramp_step)
half_ramp_step = ramp_step/2
dec = 0
while (step < ramp_step) and not kill.isSet() and \
(step%STOP_CONDITION_INTERVAL != 0 or not stop_condition()):
bbio.digitalWrite(pin,bbio.LOW)
bbio.digitalWrite(pin,bbio.HIGH)
step +=1
x = (step/ramp_step)
dec = x*x
if step > half_ramp_step:
dec = -(x-1)*(x-1) + 0.5
bbio.delayMicroseconds( min_sleep + ramp_sleep - 2*dec*ramp_sleep)
while (step < steps or steps == -1) and not kill.isSet() and \
(step%STOP_CONDITION_INTERVAL != 0 or not stop_condition()):
bbio.digitalWrite(pin,bbio.LOW)
bbio.digitalWrite(pin,bbio.HIGH)
step +=1
bbio.delayMicroseconds(min_sleep)
def begin(self, cols, rows):
""" Initializes the LCD driver and sets it to the given number of rows
and columns. """
assert cols == 8 or cols == 16 or cols == 20, \
"LiquidCrystal only supports 8, 16 and 20 row displays"
assert rows == 1 or rows == 2 or rows == 4, \
"LiquidCrystal only supports 1, 2 and 4 column displays"
self.cols = cols
self.rows = rows
for pin in (self.rs, self.enable) + self.data_pins:
bbio.pinMode(pin, bbio.OUTPUT)
if self.rw: bbio.pinMode(self.rw, bbio.OUTPUT)
bbio.digitalWrite(self.enable, bbio.LOW) # idle state
# This is the initialization procedure as defined in the HD44780 datasheet:
if self.mode_bits == 8:
self.writeCommand(self.COMMAND_FUNCTIONSET | self.FUNCTIONSET_8BIT)
else:
self._write4bits((self.COMMAND_FUNCTIONSET
| self.FUNCTIONSET_8BIT) >> 4)
bbio.delay(5)
if self.mode_bits == 8:
self.writeCommand(self.COMMAND_FUNCTIONSET | self.FUNCTIONSET_8BIT)
else:
self._write4bits((self.COMMAND_FUNCTIONSET
| self.FUNCTIONSET_8BIT) >> 4)
bbio.delay(5)
if self.mode_bits == 8:
self.writeCommand(self.COMMAND_FUNCTIONSET | self.FUNCTIONSET_8BIT)
else:
self._write4bits((self.COMMAND_FUNCTIONSET
| self.FUNCTIONSET_8BIT) >> 4)
function_set = 0
if self.mode_bits == 8: function_set = self.FUNCTIONSET_8BIT
else:
# Must put in 4 bit mode before we can set the rows and cols
self._write4bits(self.COMMAND_FUNCTIONSET >> 4)
if self.rows > 1: function_set |= self.FUNCTIONSET_2LINE
self.writeCommand(self.COMMAND_FUNCTIONSET | function_set)
self.setDisplay(self.OFF)
self.clear()
self._entry_mode = self.ENTRYMODE_SHIFTRIGHT
self.writeCommand(self.COMMAND_ENTRYMODE | self._entry_mode)
self.setDisplay(self.ON)
def begin(self, cols, rows):
""" Initializes the LCD driver and sets it to the given number of rows
and columns. """
assert cols == 8 or cols == 16 or cols == 20, \
"LiquidCrystal only supports 8, 16 and 20 row displays"
assert rows == 1 or rows == 2 or rows == 4, \
"LiquidCrystal only supports 1, 2 and 4 column displays"
self.cols = cols
self.rows = rows
for pin in (self.rs, self.enable) + self.data_pins:
bbio.pinMode(pin, bbio.OUTPUT)
if self.rw: bbio.pinMode(self.rw, bbio.OUTPUT)
bbio.digitalWrite(self.enable, bbio.LOW) # idle state
# This is the initialization procedure as defined in the HD44780 datasheet:
if self.mode_bits == 8:
self.writeCommand(self.COMMAND_FUNCTIONSET | self.FUNCTIONSET_8BIT)
else:
self._write4bits((self.COMMAND_FUNCTIONSET | self.FUNCTIONSET_8BIT) >> 4)
bbio.delay(5)
if self.mode_bits == 8:
self.writeCommand(self.COMMAND_FUNCTIONSET | self.FUNCTIONSET_8BIT)
else:
self._write4bits((self.COMMAND_FUNCTIONSET | self.FUNCTIONSET_8BIT) >> 4)
bbio.delay(5)
if self.mode_bits == 8:
self.writeCommand(self.COMMAND_FUNCTIONSET | self.FUNCTIONSET_8BIT)
else:
self._write4bits((self.COMMAND_FUNCTIONSET | self.FUNCTIONSET_8BIT) >> 4)
function_set = 0
if self.mode_bits == 8:
function_set = self.FUNCTIONSET_8BIT
else:
# Must put in 4 bit mode before we can set the rows and cols
self._write4bits(self.COMMAND_FUNCTIONSET >> 4)
if self.rows > 1: function_set |= self.FUNCTIONSET_2LINE
self.writeCommand(self.COMMAND_FUNCTIONSET | function_set)
self.setDisplay(self.OFF)
self.clear()
self._entry_mode = self.ENTRYMODE_SHIFTRIGHT
self.writeCommand(self.COMMAND_ENTRYMODE | self._entry_mode)
self.setDisplay(self.ON)
def setup():
print "setup..."
for trigger, echo in ULTRAS:
bbio.pinMode(echo, bbio.INPUT)
bbio.pinMode(trigger, bbio.OUTPUT)
bbio.digitalWrite(trigger, bbio.LOW)
def setup():
print "setup..."
for trigger, echo in ULTRAS:
bbio.pinMode(echo, bbio.INPUT)
bbio.pinMode(trigger, bbio.OUTPUT)
bbio.digitalWrite(trigger, bbio.LOW)
right_wheel['pi_controller'] = right_pid
right_wheel['encoder'] = right_encoder
left_motor = Motor(cst.LEFT_MOTOR)
left_pid = PI_controller(cst.KP, cst.KI, cst.PAUSE_S)
left_encoder = Encoder(cst.LEFT_ENCODER, cst.ENCODER_FREQ,
cst.ENCODER_OUTPUT_GAIN)
left_wheel = {}
left_wheel['motor'] = left_motor
left_wheel['pi_controller'] = left_pid
left_wheel['encoder'] = left_encoder
ir_sensors = IR_sensor(cst.IR_SENSOR_SPI, cst.IR_SENSOR_CS,
cst.IR_SENSOR_FREQ)
robot = Robot(right_wheel, left_wheel, ir_sensors, cst.MAX_SPEED,
potentiometer, start_stop_button)
robot.start()
if __name__ == '__main__':
io.pinMode(cst.STATUS_LED, io.OUTPUT)
io.pinMode(cst.START_LED, io.OUTPUT)
io.pinMode(cst.START_STOP_BUTTON, io.INPUT)
potentiometer = Potentiometer(cst.POTENTIOMETER, cst.POTENTIOMETER_GAIN)
start_stop_button = Button(cst.START_STOP_BUTTON)
initial_start(start_stop_button)
main(start_stop_button)
import DMCC
import bbio
import pyDMCC
import time
tp = bbio.GPIO1_16
bbio.pinMode(tp, bbio.OUTPUT)
def motor_ex():
DMCC.setPIDConstants(0, 2, 1, -3000, -32768, 0)
while 1:
try:
bbio.digitalWrite(tp, bbio.HIGH)
DMCC.setTargetVel(0, 2, 150)
bbio.digitalWrite(tp, bbio.LOW)
except KeyboardInterrupt:
DMCC.setTargetVel(0, 2, 0)
exit()
def motor_club():
dmccs = pyDMCC.autodetect()
motor = dmccs[0].motors[1]
constants = (-15000, -30000, 0)
motor.velocity_pid = constants
velocity = 0
while 1:
