@tigerpython/robotics-libraries 1.4.0

package/microbit/mbled.py

@@ -0,0 +1,56 @@
+from neopixel import *
+import gc
+from microbit import pin2
+
+
+_nbLeds = 24
+_np = NeoPixel(pin2, _nbLeds)
+
+# Fill all pixels with the same color.
+# The color is specified by giving values between 0 and 255 for red, green and blue
+
+
+def fill(red, green, blue):
+    for i in range(_nbLeds):
+        _np[i] = (red, green, blue)
+    _np.show()
+
+# Set the specified pixel to the specified color.
+# The pixel is specified by a number between 0 and 23.
+# The color is specified by giving values between 0 and 255 for red, green and blue
+
+
+def set_led(pos, red, green, blue):
+    _np[pos] = (red, green, blue)
+    _np.show()
+
+# clears all LEDS by turning them off
+
+
+def clear():
+    _np.clear()
+
+# shift the displayed image by as many led-positions as specified in parameter "amount".
+# LED strip behaves like a cycle and wraps around when shifting across borders.
+
+
+def shift_by(amount):
+    shifted_copy = [None]*_nbLeds
+    for n in range(0, _nbLeds):
+        next_i = (n + amount) % _nbLeds
+        shifted_copy[n] = _np[next_i]
+    for n in range(0, _nbLeds):
+        _np[n] = shifted_copy[n]
+    _np.show()
+
+# get linearly interpolated colors in RGB space.
+# "percent" is the blending distance between the two colors. 0 is first color only, and 100 is second color only.
+
+
+def lerp_RGB(r1, g1, b1, r2, g2, b2, percent):
+    if percent < 0.0 or percent > 100.0:
+        raise RuntimeError("Argument 'percent' must be between 0 and 100.")
+    red = max(0, min(255, round(r1 + (r2 - r1) * (percent/100.0))))
+    green = max(0, min(255, round(g1 + (g2 - g1) * (percent/100.0))))
+    blue = max(0, min(255, round(b1 + (b2 - b1) * (percent/100.0))))
+    return red, green, blue

package/microbit/mbmarsrover.py

@@ -0,0 +1,216 @@
+from microbit import *
+from neopixel import *
+import utime
+
+_servoArray = [0, 9, 11, 13, 15]
+_servoOffset = [0, 0, 0, 0, 0]
+_initI2C = False
+
+_nbLeds = 4
+_np = NeoPixel(pin2, _nbLeds)
+
+_speedPercent = 50
+_powerByteL = 40
+_powerByteR = 40
+
+_powerOffset = 0
+_powerDifferential = 0
+_arcScaling = 0
+
+_MAX_CM_DISTANCE = 500
+_CM_PER_MICROSECOND = 29.1
+
+
+def _setMotors(dirL, powerL, dirR, powerR):
+    pinsL = (pin1, pin12)
+    pinsR = (pin8, pin0)
+
+    pinsL[dirL].write_analog(powerL)
+    pinsL[1 - dirL].write_analog(0)
+
+    pinsR[dirR].write_analog(powerR)
+    pinsR[1 - dirR].write_analog(0)
+
+
+def _setSingleMotor(side, direction, power):
+    if side == 0:
+        pins = (pin1, pin12)
+    elif side == 1:
+        pins = (pin8, pin0)
+
+    pins[direction].write_analog(power)
+    pins[1 - direction].write_analog(0)
+
+
+def _getArcBytes(r):
+    outerSpeed = _speedPercent
+    rCm = int(r * 100)
+    threshold = outerSpeed - max(rCm + 20, 40)
+    if (threshold <= 0):
+        outerSpeed = min(max(rCm + 40, 40), 100)
+    reducedSpeed = 0
+    if rCm >= 4:
+        flattening = (100 - outerSpeed) // 2
+        reducedSpeed = (rCm * 10 - 35) / \
+            (rCm * (11 + (_arcScaling - 4) / 10) + 90 + flattening)
+        reducedSpeed = reducedSpeed * outerSpeed
+    innerByte = _convertSpeedToAnalogValue(int(reducedSpeed), 0)
+    outerByte = _convertSpeedToAnalogValue(int(outerSpeed), 0)
+    return (innerByte, outerByte)
+
+
+def _initPCA():
+    global _initI2C
+    _initI2C = True
+    i2cData = bytearray(2)
+    i2cData[0] = 0
+    i2cData[1] = 0x10
+    i2c.write(0x40, i2cData)
+
+    i2cData[0] = 0xFE
+    i2cData[1] = 101
+    i2c.write(0x40, i2cData)
+
+    i2cData[0] = 0
+    i2cData[1] = 0x81
+    i2c.write(0x40, i2cData)
+
+
+def _pulse_in(pin, value, timeout):
+    start_time = utime.ticks_us()
+
+    while pin.read_digital() != value:
+        if utime.ticks_diff(utime.ticks_us(), start_time) > timeout:
+            return 0
+
+    start_time = utime.ticks_us()
+    while pin.read_digital() == value:
+        if utime.ticks_diff(utime.ticks_us(), start_time) > timeout:
+            return 0
+
+    return utime.ticks_diff(utime.ticks_us(), start_time)
+
+
+def calibrate(offset, differential=0, arcScaling=0):
+    global _powerDifferential
+    global _powerOffset
+    global _arcScaling
+
+    _powerOffset = max(min(int(offset), 500), -50)
+    _powerDifferential = max(min(int(differential), 150), -150)
+    _arcScaling = max(min(arcScaling, 50), -15)
+    setSpeed(_speedPercent)
+
+
+def setSpeed(speed):
+    global _speedPercent
+    global _powerByteL
+    global _powerByteR
+
+    _speedPercent = int(min(max(speed, 0), 100))
+    powerValue = _convertSpeedToAnalogValue(_speedPercent, _powerOffset)
+    boost = round((1 - _speedPercent / 100) *
+                  abs(_powerDifferential)) if _speedPercent > 0 else 0
+    reduction = round((_speedPercent / 100) * abs(_powerDifferential))
+    if _powerDifferential > 0:
+        _powerByteL = powerValue - reduction
+        _powerByteR = powerValue + boost
+    else:
+        _powerByteL = powerValue + boost
+        _powerByteR = powerValue - reduction
+
+
+def _convertSpeedToAnalogValue(speed, offset):
+    analogValue = int(speed * 1023 / 100) + offset
+    return min(max(analogValue, 0), 1023)
+
+
+def stop():
+    _setMotors(0, 0, 0, 0)
+
+
+def forward():
+    _setMotors(0, _powerByteL, 0, _powerByteR)
+
+
+def backward():
+    _setMotors(1, _powerByteL, 1, _powerByteR)
+
+
+def setServo(servoNum, angle):
+    global _servoArray, _initI2C
+    if _initI2C == False:
+        _initPCA()
+    offsetNum = servoNum
+    servoNum = _servoArray[servoNum]
+    i2cData = bytearray(2)
+    start = 0
+    angle = max(min(angle, 90), -90)
+    stop = 396 + (angle + _servoOffset[offsetNum]) * 223 / 90
+    i2cData[0] = 0x06 + servoNum * 4 + 2
+    i2cData[1] = int(stop) + 0xff
+    i2c.write(0x40, i2cData)
+
+    i2cData[0] = 0x06 + servoNum*4 + 3
+    i2cData[1] = int(stop) >> 8
+    i2c.write(0x40, i2cData)
+
+
+def clearServos():
+    setServo(0, 0)
+    setServo(1, 0)
+    setServo(2, 0)
+    setServo(3, 0)
+    setServo(4, 0)
+    sleep(500)
+
+
+def steer(angle):
+    angle = max(min(angle, 45), -45)
+    setServo(1, angle)
+    setServo(2, -angle)
+    setServo(3, -angle)
+    setServo(4, angle)
+    sleep(500)
+
+
+def setLED(pos, red, green, blue):
+    _np[pos] = (red, green, blue)
+    _np.show()
+
+
+def fill(red, green, blue):
+    for i in range(_nbLeds):
+        _np[i] = (red, green, blue)
+    _np.show()
+
+
+def getDistance():
+    trig = pin13
+    echo = pin13
+
+    d = 10
+
+    trig.set_pull(trig.NO_PULL)
+
+    for _ in range(10):
+        trig.write_digital(0)
+        utime.sleep_us(2)
+        trig.write_digital(1)
+        utime.sleep_us(10)
+        trig.write_digital(0)
+
+        duration = _pulse_in(echo, 1, _MAX_CM_DISTANCE * _CM_PER_MICROSECOND)
+        if duration > 0:
+            d = duration
+            break
+
+    return round(d / _CM_PER_MICROSECOND)
+
+
+def calibrateServo(pos, angleDifferential):
+    global _servoOffset
+
+    angleDifferential = max(min(angleDifferential, 90), -90)
+    _servoOffset[pos] = angleDifferential
+    clearServos()

package/microbit/mbminibit.py

@@ -0,0 +1,139 @@
+from microbit import *
+from neopixel import *
+import utime
+
+_nbLeds = 4
+_np = NeoPixel(pin13, _nbLeds)
+
+_speedPercent = 50
+_powerLeft = 90
+_powerRight = 90
+
+_powerOffset = 0
+_powerDifferential = 0
+_arcScaling = 0
+
+_MAX_CM_DISTANCE = 500
+_CM_PER_MICROSECOND = 29.1
+
+
+def _setMotors(dirL, powerL, dirR, powerR):
+    pin8.write_analog(powerL if dirL == 1 else 0)
+    pin12.write_analog(0 if dirL == 1 else powerL)
+    pin16.write_analog(0 if dirR == 1 else powerR)
+    pin14.write_analog(powerR if dirR == 1 else 0)
+
+
+def _pulse_in(pin, value, timeout):
+    start_time = utime.ticks_us()
+    while pin.read_digital() != value:
+        if utime.ticks_diff(utime.ticks_us(), start_time) > timeout:
+            return 0
+    start_time = utime.ticks_us()
+    while pin.read_digital() == value:
+        if utime.ticks_diff(utime.ticks_us(), start_time) > timeout:
+            return 0
+    return utime.ticks_diff(utime.ticks_us(), start_time)
+
+
+def _getArcBytes(r):
+    outerSpeed = _speedPercent
+    if r > 0:
+        innerSpeed = outerSpeed * max(0.2, min(1, 1 - r / 100))
+    else:
+        innerSpeed = 0
+
+    innerByte = _convertSpeedToAnalogValue(int(innerSpeed), 0)
+    outerByte = _convertSpeedToAnalogValue(int(outerSpeed), 0)
+    return (innerByte, outerByte)
+
+
+def _convertSpeedToAnalogValue(speed, offset):
+    analogValue = int(speed * 255 / 100) + offset
+    return min(max(analogValue, 0), 255)
+
+
+def calibrate(offset, differential=0, arcScaling=0):
+    global _powerDifferential
+    global _powerOffset
+    global _arcScaling
+    _powerOffset = max(min(int(offset), 100), -10)
+    _powerDifferential = max(min(int(differential), 150), -150)
+    _arcScaling = max(min(arcScaling, 50), -15)
+    setSpeed(_speedPercent)
+
+
+def setSpeed(speed):
+    global _speedPercent
+    global _powerLeft
+    global _powerRight
+    _speedPercent = int(min(max(speed, 0), 100))
+    powerByte = _convertSpeedToAnalogValue(_speedPercent, _powerOffset)
+    boost = round((1 - _speedPercent / 100) *
+                  abs(_powerDifferential)) if _speedPercent > 0 else 0
+    reduction = round((_speedPercent / 100) * abs(_powerDifferential))
+    if _powerDifferential > 0:
+        _powerLeft = powerByte - reduction
+        _powerRight = powerByte + boost
+    else:
+        _powerLeft = powerByte + boost
+        _powerRight = powerByte - reduction
+
+
+def stop():
+    _setMotors(0, 0, 0, 0)
+
+
+def forward():
+    _setMotors(0, _powerLeft, 0, _powerRight)
+
+
+def backward():
+    _setMotors(1, _powerLeft, 1, _powerRight)
+
+
+def left():
+    _setMotors(1, _powerLeft, 0, _powerRight)
+
+
+def right():
+    _setMotors(0, _powerLeft, 1, _powerRight)
+
+
+def rightArc(radius):
+    inner, outer = _getArcBytes(radius)
+    _setMotors(0, outer, 0, inner)
+
+
+def leftArc(radius):
+    inner, outer = _getArcBytes(radius)
+    _setMotors(0, inner, 0, outer)
+
+
+def setLED(pos, red, green, blue):
+    _np[pos] = (red, green, blue)
+    _np.show()
+
+
+def fill(red, green, blue):
+    for i in range(_nbLeds):
+        _np[i] = (red, green, blue)
+    _np.show()
+
+
+def getDistance():
+    trig = pin15
+    echo = pin15
+    d = 10
+    trig.set_pull(trig.NO_PULL)
+    for _ in range(10):
+        trig.write_digital(0)
+        utime.sleep_us(2)
+        trig.write_digital(1)
+        utime.sleep_us(10)
+        trig.write_digital(0)
+        duration = _pulse_in(echo, 1, _MAX_CM_DISTANCE * _CM_PER_MICROSECOND)
+        if duration > 0:
+            d = duration
+            break
+    return round(d / _CM_PER_MICROSECOND)