@tigerpython/robotics-libraries 1.4.0
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- package/CHANGELOG +68 -0
- package/LICENSE +373 -0
- package/README.md +81 -0
- package/calliope/README.md +15 -0
- package/calliope/callibot.py +176 -0
- package/calliope/callibotmot.py +46 -0
- package/calliope/callimk.py +143 -0
- package/calliope/cbalarm.py +14 -0
- package/calliope/cpglow.py +76 -0
- package/calliope/cpmike.py +16 -0
- package/calliope/cprover.py +33 -0
- package/calliope/cputils.py +23 -0
- package/calliope/libraries.json +10 -0
- package/calliope/libraries.raw.json +10 -0
- package/calliope/min/callibot.py +97 -0
- package/calliope/min/callibotmot.py +23 -0
- package/calliope/min/callimk.py +47 -0
- package/calliope/min/cbalarm.py +6 -0
- package/calliope/min/cpglow.py +29 -0
- package/calliope/min/cpmike.py +8 -0
- package/calliope/min/cprover.py +9 -0
- package/calliope/min/cputils.py +7 -0
- package/dist/index.d.mts +124 -0
- package/dist/index.d.ts +124 -0
- package/dist/index.js +2896 -0
- package/dist/index.js.map +1 -0
- package/dist/index.mjs +2862 -0
- package/dist/index.mjs.map +1 -0
- package/microbit/README.md +48 -0
- package/microbit/controller.py +212 -0
- package/microbit/huskylens.py +479 -0
- package/microbit/libraries.json +19 -0
- package/microbit/libraries.raw.json +19 -0
- package/microbit/mbalarm.py +15 -0
- package/microbit/mbbitbot.py +127 -0
- package/microbit/mbglow.py +77 -0
- package/microbit/mbled.py +56 -0
- package/microbit/mbmarsrover.py +216 -0
- package/microbit/mbminibit.py +139 -0
- package/microbit/mbrobot.py +401 -0
- package/microbit/mbrobot_legacy.py +90 -0
- package/microbit/mbrobot_plus.py +179 -0
- package/microbit/mbrobot_plusV2.py +490 -0
- package/microbit/mbrobot_plusV3.py +427 -0
- package/microbit/mbrobotmot.py +49 -0
- package/microbit/mbthetabot.py +167 -0
- package/microbit/mbwait.py +50 -0
- package/microbit/mbxgo.py +173 -0
- package/microbit/min/controller.py +42 -0
- package/microbit/min/huskylens.py +145 -0
- package/microbit/min/mbalarm.py +6 -0
- package/microbit/min/mbbitbot.py +40 -0
- package/microbit/min/mbglow.py +29 -0
- package/microbit/min/mbled.py +18 -0
- package/microbit/min/mbmarsrover.py +62 -0
- package/microbit/min/mbminibit.py +50 -0
- package/microbit/min/mbrobot.py +82 -0
- package/microbit/min/mbrobot_legacy.py +45 -0
- package/microbit/min/mbrobot_plus.py +75 -0
- package/microbit/min/mbrobot_plusV2.py +102 -0
- package/microbit/min/mbrobot_plusV3.py +194 -0
- package/microbit/min/mbrobotmot.py +25 -0
- package/microbit/min/mbthetabot.py +47 -0
- package/microbit/min/mbwait.py +23 -0
- package/microbit/min/mbxgo.py +37 -0
- package/package.json +54 -0
package/dist/index.js
ADDED
|
@@ -0,0 +1,2896 @@
|
|
|
1
|
+
"use strict";
|
|
2
|
+
var __defProp = Object.defineProperty;
|
|
3
|
+
var __getOwnPropDesc = Object.getOwnPropertyDescriptor;
|
|
4
|
+
var __getOwnPropNames = Object.getOwnPropertyNames;
|
|
5
|
+
var __hasOwnProp = Object.prototype.hasOwnProperty;
|
|
6
|
+
var __export = (target, all) => {
|
|
7
|
+
for (var name in all)
|
|
8
|
+
__defProp(target, name, { get: all[name], enumerable: true });
|
|
9
|
+
};
|
|
10
|
+
var __copyProps = (to, from, except, desc) => {
|
|
11
|
+
if (from && typeof from === "object" || typeof from === "function") {
|
|
12
|
+
for (let key of __getOwnPropNames(from))
|
|
13
|
+
if (!__hasOwnProp.call(to, key) && key !== except)
|
|
14
|
+
__defProp(to, key, { get: () => from[key], enumerable: !(desc = __getOwnPropDesc(from, key)) || desc.enumerable });
|
|
15
|
+
}
|
|
16
|
+
return to;
|
|
17
|
+
};
|
|
18
|
+
var __toCommonJS = (mod) => __copyProps(__defProp({}, "__esModule", { value: true }), mod);
|
|
19
|
+
|
|
20
|
+
// src/index.ts
|
|
21
|
+
var index_exports = {};
|
|
22
|
+
__export(index_exports, {
|
|
23
|
+
calliopeLibraries: () => libraries_default,
|
|
24
|
+
calliopeRawLibraries: () => libraries_raw_default,
|
|
25
|
+
getLibrary: () => getLibrary,
|
|
26
|
+
getRawLibrary: () => getRawLibrary,
|
|
27
|
+
listDevices: () => listDevices,
|
|
28
|
+
listLibraries: () => listLibraries,
|
|
29
|
+
microbitLibraries: () => libraries_default2,
|
|
30
|
+
microbitRawLibraries: () => libraries_raw_default2
|
|
31
|
+
});
|
|
32
|
+
module.exports = __toCommonJS(index_exports);
|
|
33
|
+
|
|
34
|
+
// calliope/libraries.json
|
|
35
|
+
var libraries_default = {
|
|
36
|
+
cbalarm: "import music\n_g1=['c6:1','r','c6,1','r','r','r']\ndef setAlarm(on):\n if on:music.play(_g1,wait=False,loop=True)\n else:music.stop()\ndef beep():music.pitch(2000,200,wait=False)",
|
|
37
|
+
cprover: "from calliope_mini import pin28,pin29,pin30\ndef forward():pin28.write_analog(v);pin29.write_digital(1);pin30.write_digital(1)\ndef left():pin28.write_analog(v);pin29.write_digital(0);pin30.write_digital(1)\ndef right():pin28.write_analog(v);pin29.write_digital(1);pin30.write_digital(0)\ndef stop():pin28.write_digital(0)\ndef move():left()\ndef rewind():right()\ndef setSpeed(speed):global v;v=int(speed/100*1023)\nsetSpeed(100)",
|
|
38
|
+
callimk: "import gc\nfrom calliope_mini import i2c,sleep\ni2c.init()\n_g1=30\n_g2=.09\ndef w(d1,d2,s1,s2):i2c.write(16,bytearray([0,d1,s1]));i2c.write(16,bytearray([2,d2,s2]))\ndef forward():w(0,0,_g1,_g1)\ndef backward():w(1,1,_g1,_g1)\ndef stop():w(0,0,0,0)\ndef right():w(0 if _g1>0 else 1,1 if _g1>0 else 0,_g1,_g1)\ndef left():w(1 if _g1>0 else 0,0 if _g1>0 else 1,_g1,_g1)\ndef rightArc(r):\n A=abs(_g1)\n if r<_g2:B=0\n else:C=(r-_g2)/(r+_g2)*(1-A*A/200000);B=int(C*A)\n if _g1>0:w(0,0,A,B)\n else:w(1,1,B,A)\ndef leftArc(r):\n A=abs(_g1)\n if r<_g2:B=0\n else:C=(r-_g2)/(r+_g2)*(1-A*A/200000);B=int(C*A)\n if _g1>0:w(0,0,B,A)\n else:w(1,1,A,B)\ndef setSpeed(speed):\n A=speed;global _g1\n if A<15:_g1=15\n else:_g1=A\ndef motorL(dir,speed):i2c.write(16,bytearray([0,dir,speed]))\ndef motorR(dir,speed):i2c.write(16,bytearray([2,dir,speed]))\ndef led(right_left,on_off):\n A=bytearray(2)\n if right_left==0:A[0]=11\n else:A[0]=12\n A[1]=on_off;i2c.write(16,A)\ndef setLEDLeft(on):led(1,on)\ndef setLEDRight(on):led(0,on)\ndef setLED(on):led(1,on);led(0,on)\ndef rgbLED(red,green,blue):A=bytearray(2);A[0]=24;A[1]=red;B=bytearray(2);B[0]=25;B[1]=green;C=bytearray(2);C[0]=26;C[1]=blue;i2c.write(16,A);i2c.write(16,B);i2c.write(16,C)\ndef getDistance():i2c.write(16,bytearray([40]));sleep(20);A=i2c.read(16,2);B=A[0]<<8|A[1];return B\ndef irLeftValue():i2c.write(16,bytearray([29]));A=i2c.read(16,1)[0];return 0 if A&1!=0 else 1\ndef irRightValue():i2c.write(16,bytearray([29]));A=i2c.read(16,1)[0];return 0 if A&2!=0 else 1\ndef setServo(S,Angle):\n if S=='S1':A=20\n if S=='S2':A=21\n i2c.write(16,bytearray([A,Angle]))\nexit=stop\ndelay=sleep",
|
|
39
|
+
cpglow: `from calliope_mini import*
|
|
40
|
+
_g1=0
|
|
41
|
+
_g2=0
|
|
42
|
+
_g3=0
|
|
43
|
+
_g4=50
|
|
44
|
+
_g5=True
|
|
45
|
+
_g6=False
|
|
46
|
+
def makeGlow():global _g6;display.set_pixel(2,2,9);_g6=True
|
|
47
|
+
def clear():display.clear()
|
|
48
|
+
def forward():_f1(1)
|
|
49
|
+
def back():_f1(-1)
|
|
50
|
+
def right(angle):global _g3;_g3=(_g3+angle)%360
|
|
51
|
+
def left(angle):right(-angle)
|
|
52
|
+
def setPos(x,y):global _g1,_g2;_g1=x;_g2=y;_f2()
|
|
53
|
+
def getPos():return _g1,_g2
|
|
54
|
+
def setSpeed(speed):global _g4;_g4=speed
|
|
55
|
+
def showTrace(enable):global _g5;_g5=enable
|
|
56
|
+
def isLit():return display.get_pixel(_g1+2,4-(_g2+2))==9
|
|
57
|
+
def _f1(s):
|
|
58
|
+
global _g1,_g2;sleep(2000-_g4*20);d=_g3//45
|
|
59
|
+
if d in[1,2,3]:_g1+=s
|
|
60
|
+
if d in[5,6,7]:_g1-=s
|
|
61
|
+
if d in[0,1,7]:_g2+=s
|
|
62
|
+
if d in[3,4,5]:_g2-=s
|
|
63
|
+
_f2()
|
|
64
|
+
def _f2():
|
|
65
|
+
if not _g6:print('Use "makeGlow()" to create a Glow.');raise Exception('Glow not initialized.')
|
|
66
|
+
if not _g5:display.clear()
|
|
67
|
+
if-2<=_g1<=2 and-2<=_g2<=2:display.set_pixel(_g1+2,4-(_g2+2),9)`,
|
|
68
|
+
callibotmot: "import gc\nfrom calliope_mini import i2c,sleep\nimport machine\nclass Motor:\n def __init__(A,id):A._id=id\n def _f2(B,d,s):\n if B._id==0:A=0\n else:A=2\n try:i2c.write(32,bytearray([A,d,s]))\n except:\n print('Please switch on mbRobot!')\n while True:0\n def rotate(B,s):\n A=abs(s);A=A+50\n if s>0:B._f2(0,A)\n elif s<0:B._f2(1,A)\n else:B._f2(0,0)\ndelay=sleep\ndef setLED(on):\n if on==1:i2c.write(33,bytearray([0,3]))\n else:i2c.write(33,bytearray([0,0]))\nmotL=Motor(0)\nmotR=Motor(2)",
|
|
69
|
+
callibot: "_B=True\n_A='Please switch on Robot!'\nimport gc\nfrom calliope_mini import i2c,sleep\n_g1=.06\ndef w(d1,d2,s1,s2):\n try:i2c.write(32,bytearray([0,d1,s1]));i2c.write(32,bytearray([2,d2,s2]))\n except:\n print(_A)\n while _B:0\ndef setSpeed(speed):global _g2;_g2=speed+40\ndef forward():w(0,0,_g2,_g2)\ndef backward():w(1,1,_g2,_g2)\ndef stop():w(0,0,0,0)\ndef right():A=int(_g2*1.1);w(0 if _g2>0 else 1,1 if _g2>0 else 0,A,A)\ndef left():A=int(_g2*1.1);w(1 if _g2>0 else 0,0 if _g2>0 else 1,A,A)\ndef rightArc(r):\n A=abs(_g2)\n if r<_g1:B=0\n else:C=(r-_g1)/(r+_g1)*(1-A*A/200000);B=int(C*A)\n if _g2>0:w(0,0,A,B)\n else:w(1,1,B,A)\ndef leftArc(r):\n A=abs(_g2)\n if r<_g1:B=0\n else:C=(r-_g1)/(r+_g1)*(1-A*A/200000);B=int(C*A)\n if _g2>0:w(0,0,B,A)\n else:w(1,1,A,B)\ndef setLEDLeft(on):\n try:i2c.write(33,bytearray([0,0]))\n except:\n print(_A)\n while _B:0\n if on==1:i2c.write(33,bytearray([0,1]))\n else:i2c.write(33,bytearray([0,0]))\ndef setLEDRight(on):\n try:i2c.write(33,bytearray([0,0]))\n except:\n print(_A)\n while _B:0\n if on==1:i2c.write(33,bytearray([0,2]))\n else:i2c.write(33,bytearray([0,0]))\ndef setLED(on):\n try:i2c.write(33,bytearray([0,0]))\n except:\n print(_A)\n while _B:0\n if on==1:i2c.write(33,bytearray([0,3]))\n else:i2c.write(33,bytearray([0,0]))\ndef irLeftValue():\n try:\n A=i2c.read(33,1)\n if A[0]==130 or A[0]==131:return 1\n else:return 0\n except:\n print(_A)\n while _B:0\ndef irRightValue():\n try:\n A=i2c.read(33,1)\n if A[0]==129 or A[0]==131:return 1\n else:return 0\n except:\n print(_A)\n while _B:0\ndef getDistance():\n try:A=i2c.read(33,3);B=(256*A[1]+A[2])/10;return B\n except:\n print(_A)\n while _B:0\ndef tsValue():\n try:\n A=i2c.read(33,1)\n if A[0]==140 or A[0]==143:return 1\n else:return 0\n except:\n print(_A)\n while _B:0\ndef tsLeftValue():\n try:\n A=i2c.read(33,1)\n if A[0]==136 or A[0]==139:return 1\n else:return 0\n except:\n print(_A)\n while _B:0\ndef tsRightValue():\n try:\n A=i2c.read(33,1)\n if A[0]==132 or A[0]==135:return 1\n else:return 0\n except:\n print(_A)\n while _B:0\nexit=stop\ndelay=sleep\n_g2=90",
|
|
70
|
+
cputils: "def cat(filename):\n with open(filename)as B:\n A=B.readline()\n while A:print(A[:-1]);A=B.readline()\nfrom math import asin,atan2,sqrt,degrees\ndef getPitch(a):A=atan2(a[1],a[2]);return int(degrees(A))\ndef getRoll(a):A=sqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);B=asin(a[0]/A);return int(degrees(B))",
|
|
71
|
+
cpmike: "from calliope_mini import pin3,running_time,sleep\n_g1=running_time()\ndef isClicked(level=10,rearm_time=500):\n A=False;global _g1\n if running_time()-_g1<rearm_time:sleep(10);return A\n B=pin3.read_analog()\n if B<518-level:_g1=running_time();return True\n return A"
|
|
72
|
+
};
|
|
73
|
+
|
|
74
|
+
// calliope/libraries.raw.json
|
|
75
|
+
var libraries_raw_default = {
|
|
76
|
+
cbalarm: "# cbalarm.py\nimport music\n\n_m = ['c6:1', 'r', 'c6,1', 'r', 'r', 'r']\n\ndef setAlarm(on):\n if on:\n music.play(_m, wait = False, loop = True) \n else:\n music.stop()\n \n \ndef beep():\n music.pitch(2000, 200, wait = False)",
|
|
77
|
+
cprover: "# cprover.py\n \nfrom calliope_mini import pin28, pin29, pin30\n\ndef forward():\n pin28.write_analog(v)\n pin29.write_digital(1)\n pin30.write_digital(1)\n\ndef left():\n pin28.write_analog(v)\n pin29.write_digital(0)\n pin30.write_digital(1)\n\ndef right():\n pin28.write_analog(v)\n pin29.write_digital(1)\n pin30.write_digital(0)\n\ndef stop():\n pin28.write_digital(0)\n\ndef move():\n left()\n\ndef rewind(): \n right()\n\ndef setSpeed(speed):\n global v\n v = int(speed / 100 * 1023)\n\nsetSpeed(100)\n",
|
|
78
|
+
callimk: '# callimk.py (motionkit2)\nimport gc\nfrom calliope_mini import i2c, sleep\ni2c.init()\n_v = 30\n_axe = 0.09\n\n\ndef w(d1, d2, s1, s2):\n i2c.write(0x10, bytearray([0x00, d1, s1]))\n i2c.write(0x10, bytearray([0x02, d2, s2]))\n\n\ndef forward():\n w(0, 0, _v, _v)\n\n\ndef backward():\n w(1, 1, _v, _v)\n\n\ndef stop():\n w(0, 0, 0, 0)\n\n\ndef right():\n w(0 if _v > 0 else 1, 1 if _v > 0 else 0, _v, _v)\n\n\ndef left():\n w(1 if _v > 0 else 0, 0 if _v > 0 else 1, _v, _v)\n\n\ndef rightArc(r):\n v = abs(_v)\n if r < _axe:\n v1 = 0\n else:\n f = (r - _axe) / (r + _axe) * (1 - v * v / 200000)\n v1 = int(f * v)\n if _v > 0:\n w(0, 0, v, v1)\n else:\n w(1, 1, v1, v)\n\n\ndef leftArc(r):\n v = abs(_v)\n if r < _axe:\n v1 = 0\n else:\n f = (r - _axe) / (r + _axe) * (1 - v * v / 200000)\n v1 = int(f * v)\n if _v > 0:\n w(0, 0, v1, v)\n else:\n w(1, 1, v, v1)\n\n\ndef setSpeed(speed):\n global _v\n if speed < 15:\n _v = 15\n else:\n _v = speed\n\n\ndef motorL(dir, speed):\n i2c.write(0x10, bytearray([0x00, dir, speed]))\n\n\ndef motorR(dir, speed):\n i2c.write(0x10, bytearray([0x02, dir, speed]))\n\n\ndef led(right_left, on_off):\n buf_led = bytearray(2)\n if right_left == 0:\n buf_led[0] = 0x0B\n else:\n buf_led[0] = 0x0C\n buf_led[1] = on_off\n i2c.write(0x10, buf_led)\n\n\ndef setLEDLeft(on):\n led(1, on)\n\n\ndef setLEDRight(on):\n led(0, on)\n\n\ndef setLED(on):\n led(1, on)\n led(0, on)\n\n\ndef rgbLED(red, green, blue):\n buf_rgbLed_red = bytearray(2)\n buf_rgbLed_red[0] = 0x18\n buf_rgbLed_red[1] = red\n buf_rgbLed_green = bytearray(2)\n buf_rgbLed_green[0] = 0x19\n buf_rgbLed_green[1] = green\n buf_rgbLed_blue = bytearray(2)\n buf_rgbLed_blue[0] = 0x1A\n buf_rgbLed_blue[1] = blue\n i2c.write(0x10, buf_rgbLed_red)\n i2c.write(0x10, buf_rgbLed_green)\n i2c.write(0x10, buf_rgbLed_blue)\n\n\ndef getDistance():\n i2c.write(0x10, bytearray([0x28]))\n sleep(20)\n data = i2c.read(0x10, 2)\n distance = (data[0] << 8) | data[1]\n return distance\n\n\ndef irLeftValue():\n i2c.write(0x10, bytearray([0x1D]))\n data = i2c.read(0x10, 1)[0]\n return 0 if (data & 0x01) != 0 else 1\n\n\ndef irRightValue():\n i2c.write(0x10, bytearray([0x1D]))\n data = i2c.read(0x10, 1)[0]\n return 0 if (data & 0x02) != 0 else 1\n\n\ndef setServo(S, Angle):\n if S == "S1":\n Servo = 0x14\n if S == "S2":\n Servo = 0x15\n i2c.write(0x10, bytearray([Servo, Angle]))\n\n\nexit = stop\ndelay = sleep\n',
|
|
79
|
+
cpglow: '# cpglow.py\n# Version 1.0, Dec. 5, 2018\n\nfrom calliope_mini import *\n\n_x = 0\n_y = 0\n_dir = 0\n_speed = 50\n_trace = True\n_visible = False\n\ndef makeGlow():\n global _visible\n display.set_pixel(2, 2, 9)\n _visible = True\n\ndef clear():\n display.clear()\n \ndef forward(): \n _forward(1)\n\ndef back(): \n _forward(-1)\n \ndef right(angle):\n global _dir\n _dir = (_dir + angle) % 360\n\ndef left(angle):\n right(-angle)\n\ndef setPos(x, y):\n global _x, _y\n _x = x\n _y = y\n _render()\n\ndef getPos():\n return _x, _y\n\ndef setSpeed(speed):\n global _speed\n _speed = speed\n \ndef showTrace(enable):\n global _trace\n _trace = enable \n\ndef isLit():\n return (display.get_pixel(_x + 2, 4 - (_y + 2)) == 9)\n \ndef _forward(s):\n global _x, _y\n sleep(2000 - _speed * 20)\n d = _dir // 45\n if d in [1, 2, 3]: \n _x += s\n if d in [5, 6, 7]: \n _x -= s\n if d in [0, 1, 7]: \n _y += s\n if d in [3, 4, 5]: \n _y -= s\n _render()\n\ndef _render():\n if not _visible:\n print("Use \\"makeGlow()\\" to create a Glow.")\n raise Exception("Glow not initialized.")\n if not _trace:\n display.clear()\n if -2 <= _x <= 2 and -2 <= _y <= 2: \n display.set_pixel(_x + 2, 4 - (_y + 2), 9)\n\n',
|
|
80
|
+
callibotmot: '# callibotmot.py\n# Version 1.0, Apr 24, 2021 / AR\n\nimport gc\nfrom calliope_mini import i2c, sleep\nimport machine\n\nclass Motor:\n def __init__(self, id):\n self._id = id\n\n def _w(self, d, s):\n if self._id == 0:\n _self = 0x00\n else:\n _self = 0x02 \n try: \n i2c.write(0x20, bytearray([_self, d, s]))\n except:\n print("Please switch on mbRobot!")\n while True:\n pass\n \n def rotate(self, s):\n v = abs(s)\n v = v + 50 \n if s > 0: \n self._w(0, v) \n elif s < 0: \n self._w(1, v) \n else: \n self._w(0, 0) \n\n \ndelay = sleep\ndef setLED(on):\n if on == 1:\n i2c.write(0x21, bytearray([0,0x03])) \n else:\n i2c.write(0x21, bytearray([0,0])) \n\nmotL = Motor(0)\nmotR = Motor(2)\n\n\n\n',
|
|
81
|
+
callibot: '# callibot.py\n# Version 1.3, 17-Sept-2021 /JA\n# new: Touchsensors\n\nimport gc\nfrom calliope_mini import i2c, sleep\n\n_axe = 0.06\n\ndef w(d1, d2, s1, s2):\n try:\n i2c.write(0x20, bytearray([0x00, d1, s1]))\n i2c.write(0x20, bytearray([0x02, d2, s2]))\n except:\n print("Please switch on Robot!")\n while True:\n pass\n \ndef setSpeed(speed):\n global _v\n _v = speed + 40\n #_v = int(speed * 1.1) + 35 \n\ndef forward():\n w(0, 0, _v, _v)\n\ndef backward():\n w(1, 1, _v, _v)\n \ndef stop():\n w(0, 0, 0, 0)\n \ndef right():\n v = int(_v * 1.1)\n w(0 if _v > 0 else 1, 1 if _v > 0 else 0, v , v) \n\ndef left():\n v = int(_v * 1.1) \n w(1 if _v > 0 else 0, 0 if _v > 0 else 1, v, v)\n\ndef rightArc(r): \n v = abs(_v) \n if r < _axe:\n v1 = 0\n else: \n f = (r - _axe) / (r + _axe) * (1 - v * v / 200000) \n v1 = int(f * v)\n if _v > 0:\n w(0, 0, v, v1)\n else:\n w(1, 1, v1, v)\n\ndef leftArc(r): \n v = abs(_v) \n if r < _axe:\n v1 = 0\n else:\n f = (r - _axe) / (r + _axe) * (1 - v * v / 200000) \n v1 = int(f * v)\n if _v > 0:\n w(0, 0, v1, v)\n else:\n w(1, 1, v, v1)\n\ndef setLEDLeft(on):\n try:\n i2c.write(0x21, bytearray([0,0])) \n except:\n print("Please switch on Robot!")\n while True:\n pass \n if on == 1:\n i2c.write(0x21, bytearray([0,0x01])) \n else:\n i2c.write(0x21, bytearray([0,0])) \n \ndef setLEDRight(on):\n try:\n i2c.write(0x21, bytearray([0,0])) \n except:\n print("Please switch on Robot!")\n while True:\n pass \n if on == 1:\n i2c.write(0x21, bytearray([0,0x02])) \n else:\n i2c.write(0x21, bytearray([0,0])) \n\ndef setLED(on):\n try:\n i2c.write(0x21, bytearray([0,0])) \n except:\n print("Please switch on Robot!")\n while True:\n pass \n if on == 1:\n i2c.write(0x21, bytearray([0,0x03])) \n else:\n i2c.write(0x21, bytearray([0,0])) \n \ndef irLeftValue():\n try:\n buffer = i2c.read(0x21,1) \n if (buffer[0] == 130 or buffer[0] == 131):\n return 1\n else:\n return 0 \n except:\n print("Please switch on Robot!")\n while True:\n pass \n\ndef irRightValue():\n try:\n buffer = i2c.read(0x21,1) \n if (buffer[0] == 129 or buffer[0] == 131):\n return 1\n else:\n return 0 \n except:\n print("Please switch on Robot!")\n while True:\n pass \n \ndef getDistance():\n try:\n buffer = i2c.read(0x21,3)\n dist = (256 * buffer[1] + buffer[2])/10\n return dist\n except:\n print("Please switch on Robot!")\n while True:\n pass \n \ndef tsValue():\n try:\n buffer = i2c.read(0x21,1) \n if (buffer[0] == 0x8C or buffer[0] == 0x8F):\n return 1\n else:\n return 0 \n except:\n print("Please switch on Robot!")\n while True:\n pass \n\ndef tsLeftValue():\n try:\n buffer = i2c.read(0x21,1) \n if (buffer[0] == 0x88 or buffer[0] == 0x8B):\n return 1\n else:\n return 0 \n except:\n print("Please switch on Robot!")\n while True:\n pass \n \ndef tsRightValue():\n try:\n buffer = i2c.read(0x21,1) \n if (buffer[0] == 0x84 or buffer[0] == 0x87):\n return 1\n else:\n return 0 \n except:\n print("Please switch on Robot!")\n while True:\n pass \n \nexit = stop\ndelay = sleep\n_v = 90 # entspricht default Speed 50\n\n\n\n',
|
|
82
|
+
cputils: "# cputils.py\n# V1.1, Dec 5, 2018\n# Additional classes / global functions for Calliope\n\n\ndef cat(filename):\n with open(filename) as f:\n line = f.readline()\n while line:\n print(line[:-1])\n line = f.readline()\n\n\nfrom math import asin, atan2, sqrt, degrees\n\ndef getPitch(a):\n pitch = atan2(a[1], a[2])\n return int(degrees(pitch))\n\ndef getRoll(a):\n anorm = sqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2])\n roll = asin(a[0] / anorm)\n return int(degrees(roll))",
|
|
83
|
+
cpmike: "# cpmike.py\n\nfrom calliope_mini import pin3, running_time, sleep\n\n_click_time = running_time()\n\ndef isClicked(level = 10, rearm_time = 500):\n global _click_time\n if running_time() - _click_time < rearm_time:\n sleep(10)\n return False\n v = pin3.read_analog()\n if v < 518 - level:\n _click_time = running_time()\n return True\n return False\n"
|
|
84
|
+
};
|
|
85
|
+
|
|
86
|
+
// microbit/libraries.json
|
|
87
|
+
var libraries_default2 = {
|
|
88
|
+
huskylens: `_E='Invalid model_id. Must be number in range [0,4]'
|
|
89
|
+
_D='Error: ID must be in range from 1 to 255.'
|
|
90
|
+
_C=None
|
|
91
|
+
_B=True
|
|
92
|
+
_A=False
|
|
93
|
+
from microbit import i2c,sleep,running_time
|
|
94
|
+
import math
|
|
95
|
+
_g1=['FaceRecognition','ObjectTracking','ObjectRecognition','LineTracking','ColorRecognition','TagRecognition','ObjectClassification','QRRecognition','BarcodeRecognition']
|
|
96
|
+
class Request_Command:KNOCK=44;ALGORITHM=45;ALL=32;BLOCKS=33;BLOCKS_LEARNED=36;BLOCKS_OF_ID=39;ARROWS=34;ARROWS_LEARNED=37;ARROWS_OF_ID=40;LEARNED=35;ALL_OF_ID=38;LEARN=54;FORGET=55;CUSTOM_LABEL=47;CUSTOM_TEXT=52;CLEAR_TEXT=53;SAVE_MODEL=50;LOAD_MODEL=51;SAVE_PHOTO=48;SAVE_SCREENSHOT=57;IS_PRO=59;VERSION=60
|
|
97
|
+
class Return_Code:ANY=1;OK=46;BUSY=61;INFO=41;BLOCK=42;ARROW=43;IS_PRO=59;NEED_PRO=62
|
|
98
|
+
class Algorithm:FACE_RECOGNITION=0;OBJECT_TRACKING=1;OBJECT_RECOGNITION=2;LINE_TRACKING=3;COLOR_RECOGNITION=4;TAG_RECOGNITION=5;OBJECT_CLASSIFICATION=6;QR_RECOGNITION=7;BARCODE_RECOGNITION=8
|
|
99
|
+
class Block:
|
|
100
|
+
def __init__(A,x,y,width,height,id):A.x=x;A.y=y;A.width=width;A.height=height;A.id=id
|
|
101
|
+
def _f2(A):return'Block: ID_'+str(A.id)+' Pos: ('+str(A.x)+' '+str(A.y)+') Size: ('+str(A.width)+' '+str(A.height)+')'
|
|
102
|
+
class Arrow:
|
|
103
|
+
def __init__(A,x_tail,y_tail,x_head,y_head,id):A.x_tail=x_tail;A.y_tail=y_tail;A.x_head=x_head;A.y_head=y_head;A.id=id
|
|
104
|
+
def get_direction(A):
|
|
105
|
+
C=A.x_head-A.x_tail;D=A.y_head-A.y_tail;B=90-math.degrees(math.atan2(D,C))
|
|
106
|
+
if B<0:B=B+360
|
|
107
|
+
return int(B)
|
|
108
|
+
def _f2(A):return'Arrow: ID_'+str(A.id)+' ('+str(A.x_tail)+' '+str(A.y_tail)+')->('+str(A.x_head)+' '+str(A.y_head)+')'
|
|
109
|
+
def byte_checksum(byte_list):return sum(byte_list)&255
|
|
110
|
+
def hexify(byte_array):
|
|
111
|
+
A=byte_array
|
|
112
|
+
if len(A)==0:return''
|
|
113
|
+
return'0x'+''.join('{:02x}'.format(A)for A in A)
|
|
114
|
+
class Huskylens:
|
|
115
|
+
I2C_ADDR=50
|
|
116
|
+
def __init__(A):A.learned_slot_count=0;A.id_slots={};A.id_names={};A.algorithm=Algorithm.OBJECT_TRACKING;A.clear_texts();A.pro_enabled=A.is_pro()
|
|
117
|
+
def initialize(A):
|
|
118
|
+
B=_A
|
|
119
|
+
for E in range(5):
|
|
120
|
+
A.knock();B,F=A.get_response(Return_Code.OK)
|
|
121
|
+
if B:break
|
|
122
|
+
if B>0:
|
|
123
|
+
C=A.clear_texts();D=A.set_algorithm(Algorithm.OBJECT_TRACKING)
|
|
124
|
+
if C and D:print('Initialization successful!');return _B
|
|
125
|
+
else:print("Initialization Failed. Couldn't change Algorithm");return _A
|
|
126
|
+
else:print('Initialization Failed. Please check connection to Huskylens.');return _A
|
|
127
|
+
def send_request(D,command,data=_C):
|
|
128
|
+
B=data;A=bytearray(b'U\\xaa\\x11\\x00\\x00');A[3]=0 if B is _C else len(B);A[4]=command
|
|
129
|
+
if B:
|
|
130
|
+
for C in B:A.append(C)
|
|
131
|
+
A.append(byte_checksum(A));i2c.write(Huskylens.I2C_ADDR,A);sleep(50)
|
|
132
|
+
def get_response(K,return_code=Return_Code.ANY,timeout=500):
|
|
133
|
+
C=return_code;A=bytearray(b'U\\x00\\x00\\x00\\x00');I=running_time()
|
|
134
|
+
while running_time()-I<timeout:
|
|
135
|
+
D=i2c.read(Huskylens.I2C_ADDR,1)[0]
|
|
136
|
+
if D==85:break
|
|
137
|
+
if D!=85:return-1,[]
|
|
138
|
+
for J in range(4):A[J+1]=i2c.read(Huskylens.I2C_ADDR,1)[0]
|
|
139
|
+
if A[0:3]!=b'U\\xaa\\x11':return-2,[]
|
|
140
|
+
E=A[3];F=A[4];B=[]
|
|
141
|
+
if E>0:G=i2c.read(Huskylens.I2C_ADDR,E+1);B=G[0:-1];H=G[-1]
|
|
142
|
+
else:H=ord(i2c.read(Huskylens.I2C_ADDR,1))
|
|
143
|
+
if H!=byte_checksum(list(A)+B):return-3,[]
|
|
144
|
+
if C==Return_Code.ANY or F==C:return F,B
|
|
145
|
+
else:return 0,B
|
|
146
|
+
def knock(A):A.send_request(Request_Command.KNOCK)
|
|
147
|
+
def set_algorithm(B,algorithm):
|
|
148
|
+
A=algorithm
|
|
149
|
+
if(A==Algorithm.QR_RECOGNITION or A==Algorithm.BARCODE_RECOGNITION)and not B.pro_enabled:raise RuntimeError('Error: Huskylens PRO version is required for algorithm ',_g1[A]);return _A
|
|
150
|
+
D=[A,0];B.send_request(Request_Command.ALGORITHM,D);C,E=B.get_response(Return_Code.OK)
|
|
151
|
+
if C>0:print('Current Algorithm:',_g1[A]);B.algorithm=A
|
|
152
|
+
return _B if C>0 else _A
|
|
153
|
+
def get_all(A):return A._f7(Request_Command.ALL)
|
|
154
|
+
def get_all_learned(A):return A._f7(Request_Command.LEARNED)
|
|
155
|
+
def get_all_with_id(A,id):
|
|
156
|
+
if id<=0 or id>255:raise RuntimeError(_D)
|
|
157
|
+
return A._f7(Request_Command.ALL_OF_ID,id)
|
|
158
|
+
def get_one(A):B=A._f7(Request_Command.ALL);return A._f8(B)
|
|
159
|
+
def get_one_learned(A):B=A._f7(Request_Command.LEARNED);return A._f8(B)
|
|
160
|
+
def get_one_with_id(A,id):
|
|
161
|
+
if id<=0 or id>255:raise RuntimeError(_D)
|
|
162
|
+
B=A._f7(Request_Command.ALL_OF_ID,id);return A._f8(B)
|
|
163
|
+
def attach_label(A,id,name):
|
|
164
|
+
B=name
|
|
165
|
+
if A.algorithm==Algorithm.OBJECT_TRACKING or A.algorithm==Algorithm.LINE_TRACKING:C=A._f6(1,B)
|
|
166
|
+
elif A.algorithm==Algorithm.FACE_RECOGNITION or A.algorithm==Algorithm.TAG_RECOGNITION or A.algorithm==Algorithm.OBJECT_CLASSIFICATION or A.algorithm==Algorithm.OBJECT_RECOGNITION:
|
|
167
|
+
D=A.id_slots.get(id)
|
|
168
|
+
if D==_C:raise RuntimeError("Can't attach a name to an unlearned ID number")
|
|
169
|
+
A.id_names[id]=B;C=_B
|
|
170
|
+
for E in D:F=A._f6(E,B);C=C and F>0
|
|
171
|
+
else:A.id_names[id]=B;C=A._f6(id,B)
|
|
172
|
+
return _B if C>0 else _A
|
|
173
|
+
def clear_labels(A):
|
|
174
|
+
A.id_names.clear()
|
|
175
|
+
for B in range(10):A._f6(B,'')
|
|
176
|
+
def add_text(E,text,position_x,position_y):
|
|
177
|
+
C=position_y;B=position_x;D=bytes(text,'utf-8')
|
|
178
|
+
if len(D)>19:raise RuntimeError('Custom Text must be less than 20 bytes long.')
|
|
179
|
+
if B>300 or B<0 or C<35 or C>240:raise RuntimeError("Custom Text can't be placed outside of screen pixel size.")
|
|
180
|
+
A=[len(D)];A.append(255 if B>255 else 0);A.append(B%255);A.append(240-C);A.extend(list(D));E.send_request(Request_Command.CUSTOM_TEXT,A);F,G=E.get_response(Return_Code.OK);return _B if F>0 else _A
|
|
181
|
+
def clear_texts(A):A.send_request(Request_Command.CLEAR_TEXT);B,C=A.get_response(Return_Code.OK);return _B if B>0 else _A
|
|
182
|
+
def learn(A,id,name=_C):
|
|
183
|
+
if id<=0 or id>255:raise RuntimeError('Parameter ID for learned item must be in range [0,255]')
|
|
184
|
+
if A.algorithm==Algorithm.OBJECT_TRACKING or A.algorithm==Algorithm.LINE_TRACKING:id=1
|
|
185
|
+
C=500
|
|
186
|
+
if A.algorithm==Algorithm.OBJECT_CLASSIFICATION:C=1000
|
|
187
|
+
A.send_request(Request_Command.LEARN,[id,0]);B,E=A.get_response(Return_Code.OK,C)
|
|
188
|
+
if B>0:
|
|
189
|
+
A.learned_slot_count+=1
|
|
190
|
+
if not id in A.id_slots:A.id_slots[id]=[A.learned_slot_count]
|
|
191
|
+
else:A.id_slots[id].append(A.learned_slot_count)
|
|
192
|
+
D=A.id_names.get(id)
|
|
193
|
+
if D!=_C:B=A.attach_label(A.learned_slot_count,D)
|
|
194
|
+
elif name!=_C:B=A.attach_label(id,name)
|
|
195
|
+
return _B if B>0 else _A
|
|
196
|
+
def forget(A):
|
|
197
|
+
A.send_request(Request_Command.FORGET);B,C=A.get_response(Return_Code.OK)
|
|
198
|
+
if B:A.learned_slot_count=0;A.id_slots.clear()
|
|
199
|
+
return _B if B else _A
|
|
200
|
+
def save_photo(A):A.send_request(Request_Command.SAVE_PHOTO);B,C=A.get_response(Return_Code.OK,1000);return _B if B>0 else _A
|
|
201
|
+
def save_screenshot(A):A.send_request(Request_Command.SAVE_SCREENSHOT);B,C=A.get_response(Return_Code.OK,1000);return _B if B>0 else _A
|
|
202
|
+
def save_model(B,model_id):
|
|
203
|
+
A=model_id
|
|
204
|
+
if A<0 or A>4:raise RuntimeError(_E)
|
|
205
|
+
B.send_request(Request_Command.SAVE_MODEL,[A,0]);C,D=B.get_response(Return_Code.OK,1000);print('Model saving: Check Huskylens screen for Result!\\n\\tModel name:',_g1[B.algorithm]+'_Backup_'+str(A)+'.conf');return _B if C>0 else _A
|
|
206
|
+
def load_model(B,model_id):
|
|
207
|
+
A=model_id
|
|
208
|
+
if A<0 or A>4:raise RuntimeError(_E)
|
|
209
|
+
B.send_request(Request_Command.LOAD_MODEL,[A,0]);C,D=B.get_response(Return_Code.OK,1000);print('Model Loading: Check Huskylens screen for Result!');return _B if C>0 else _A
|
|
210
|
+
def is_pro(A):A.send_request(Request_Command.IS_PRO);B,C=A.get_response(Return_Code.IS_PRO);return bool(C[0])if B>0 else _A
|
|
211
|
+
def _f6(C,id,name):
|
|
212
|
+
A=bytes(name,'utf-8')
|
|
213
|
+
if len(A)>19:raise RuntimeError('Custom Name must be less than 20 bytes long.')
|
|
214
|
+
B=[id,len(A)+1];B.extend(list(A));B.append(0);C.send_request(Request_Command.CUSTOM_LABEL,B);D,E=C.get_response(Return_Code.OK);return _B if D>0 else _A
|
|
215
|
+
def _f7(B,request_command,id=-1):
|
|
216
|
+
I=_C if id<0 else[id,0];B.send_request(request_command,I);G,H=B.get_response(Return_Code.INFO)
|
|
217
|
+
if not G:raise RuntimeError('Failed to request results. Got answer:'+str(G))
|
|
218
|
+
J=H[0]+H[1]*255;C=0;D=[]
|
|
219
|
+
while C<J:
|
|
220
|
+
E,A=B.get_response(Return_Code.ANY)
|
|
221
|
+
if E==Return_Code.BLOCK and B.algorithm!=3:K=A[0]+A[1]*255;L=240-A[2]+A[3]*255;M=A[4]+A[5]*255;N=A[6]+A[7]*255;F=A[8];O=Block(K,L,M,N,F);D.append(O);C+=1
|
|
222
|
+
elif E==Return_Code.ARROW:P=A[0]+A[1]*255;Q=240-A[2]+A[3]*255;R=A[4]+A[5]*255;S=240-A[6]+A[7]*255;F=A[8];T=Arrow(P,Q,R,S,F);D.append(T);C+=1
|
|
223
|
+
elif E==0:return[]
|
|
224
|
+
return D
|
|
225
|
+
def _f8(E,results):
|
|
226
|
+
C=_C;D=440
|
|
227
|
+
for A in results:
|
|
228
|
+
B=0
|
|
229
|
+
if E.algorithm==Algorithm.LINE_TRACKING:B=abs(A.x_tail+(A.x_tail-A.x_head)//2-160)+abs(A.y_tail+(A.y_tail-A.y_head)//2-120)
|
|
230
|
+
else:B=abs(A.x-160)+abs(A.y-120)
|
|
231
|
+
if B<D:C,D=A,B
|
|
232
|
+
return C`,
|
|
233
|
+
mbbitbot: "from microbit import*\nfrom neopixel import*\nfrom utime import ticks_us,sleep_us\n_g1=200\n_g2=12\n_g3=NeoPixel(pin13,_g2)\n_g4=0\n_g5=0\ndef w(leftforward,leftbackward,rightforward,rightbackward):\n if leftforward>0 or rightforward>0:pin16.write_analog(leftforward+_g4);pin8.write_analog(leftbackward);pin14.write_analog(rightforward-_g4);pin12.write_analog(rightbackward)\n elif leftbackward>0 or rightbackward>0:pin16.write_analog(leftforward);pin8.write_analog(leftbackward+_g4);pin14.write_analog(rightforward);pin12.write_analog(rightbackward-_g4)\n elif leftforward==0 and rightforward==0 and leftbackward==0 and rightbackward==0:pin16.write_analog(0);pin8.write_analog(0);pin14.write_analog(0);pin12.write_analog(0)\ndef forward():w(_g1,0,_g1,0)\ndef backward():w(0,_g1,0,_g1)\ndef stop():w(0,0,0,0)\ndef right():w(0,_g1,_g1,0)\ndef left():w(_g1,0,0,_g1)\ndef set_led(pos,red,green,blue):_g3[pos]=red,green,blue;_g3.show()\ndef fill(red,green,blue):\n for i in range(_g2):_g3[i]=red,green,blue\n _g3.show()\ndef getDistance():\n pin15.write_digital(1);sleep_us(10);pin15.write_digital(0);pin15.set_pull(pin15.NO_PULL)\n while pin15.read_digital()==0:0\n start=ticks_us()\n while pin15.read_digital()==1:0\n end=ticks_us();echo=end-start;distance=int(.01715*echo);return distance\ndef calibrate(offset,differential=0):global _g4;global _g5;global _g8;_g5=max(min(int(offset),50),-50);_g4=max(min(int(differential),50),-50);_g8=max(min(arcScaling,50),-50);setSpeed(_g1/1023*100)\ndef setSpeed(percent):global _g1;speed=percent/100*1023;speed+=_g5/100*1023;speed=min(max(speed,0),1023);_g1=speed\ndef getLine(bit):\n mask=1<<bit;value=0\n try:value=i2c.read(28,1)[0]\n except OSError:pass\n if value&mask>0:return 1\n else:return 0\ndef getLight(index):\n if index==0:return pin1.read_analog()\n elif index==1:return pin2.read_analog()\ndef leftArc(radius):speed=_g1;inner_wheel_speed=speed*(radius/(radius+1));w(inner_wheel_speed,0,speed,0)\ndef rightArc(radius):speed=_g1;inner_wheel_speed=speed*(radius/(radius+1));w(speed,0,inner_wheel_speed,0)",
|
|
234
|
+
mbwait: `from microbit import button_a,button_b,pin0,pin1,pin2,pin_logo,sleep
|
|
235
|
+
POLLING_DELAY=100
|
|
236
|
+
def wait_for_press(button='any'):
|
|
237
|
+
A=button;button_a.was_pressed();button_b.was_pressed()
|
|
238
|
+
if A=='a'or A=='A':
|
|
239
|
+
while not button_a.was_pressed():sleep(POLLING_DELAY)
|
|
240
|
+
elif A=='b'or A=='B':
|
|
241
|
+
while not button_b.was_pressed():sleep(POLLING_DELAY)
|
|
242
|
+
elif A=='any'or A=='ANY':
|
|
243
|
+
while not button_a.was_pressed()and not button_b.was_pressed():sleep(POLLING_DELAY)
|
|
244
|
+
elif A=='both'or A=='ab'or A=='AB':
|
|
245
|
+
while not(button_a.is_pressed()and button_b.is_pressed()):sleep(POLLING_DELAY)
|
|
246
|
+
while button_a.is_pressed()or button_b.is_pressed():sleep(POLLING_DELAY)
|
|
247
|
+
else:raise RuntimeError("Button to wait for must be either: 'a', 'b', 'any' or 'both'.")
|
|
248
|
+
def wait_for_touch(pin='logo'):
|
|
249
|
+
B=pin;A=pin_logo
|
|
250
|
+
if B=='pin0':A=pin0
|
|
251
|
+
elif B=='pin1':A=pin1
|
|
252
|
+
elif B=='pin2':A=pin2
|
|
253
|
+
elif B=='logo'or B=='pin_logo'or B=='pinLogo':A=pin_logo
|
|
254
|
+
else:raise RuntimeError("Argument 'pin' must be one of: 'pin_logo', 'pin0', 'pin1' or 'pin2'.")
|
|
255
|
+
A.set_touch_mode(A.CAPACITIVE)
|
|
256
|
+
while not A.is_touched():sleep(POLLING_DELAY)`,
|
|
257
|
+
mbled: `from neopixel import*
|
|
258
|
+
import gc
|
|
259
|
+
from microbit import pin2
|
|
260
|
+
_g1=24
|
|
261
|
+
_g2=NeoPixel(pin2,_g1)
|
|
262
|
+
def fill(red,green,blue):
|
|
263
|
+
for i in range(_g1):_g2[i]=red,green,blue
|
|
264
|
+
_g2.show()
|
|
265
|
+
def set_led(pos,red,green,blue):_g2[pos]=red,green,blue;_g2.show()
|
|
266
|
+
def clear():_g2.clear()
|
|
267
|
+
def shift_by(amount):
|
|
268
|
+
shifted_copy=[None]*_g1
|
|
269
|
+
for n in range(0,_g1):next_i=(n+amount)%_g1;shifted_copy[n]=_g2[next_i]
|
|
270
|
+
for n in range(0,_g1):_g2[n]=shifted_copy[n]
|
|
271
|
+
_g2.show()
|
|
272
|
+
def lerp_RGB(r1,g1,b1,r2,g2,b2,percent):
|
|
273
|
+
if percent<.0 or percent>1e2:raise RuntimeError("Argument 'percent' must be between 0 and 100.")
|
|
274
|
+
red=max(0,min(255,round(r1+(r2-r1)*(percent/1e2))));green=max(0,min(255,round(g1+(g2-g1)*(percent/1e2))));blue=max(0,min(255,round(b1+(b2-b1)*(percent/1e2))));return red,green,blue`,
|
|
275
|
+
mbrobot_plus: "_A='Please switch on mbRobot!'\nfrom microbit import i2c,pin0,pin1,pin2,sleep\nimport machine,gc,music\n_g1=50\n_g2=.082\ndef w(d1,d2,s1,s2):\n try:i2c.write(16,bytearray([0,d1,d2,s1,s2]))\n except:print(_A)\ndef setSpeed(speed):\n A=speed;global _g1\n if A<30 and A!=0:setPID(1);_g1=A+30\n elif A>=30 and A<32:setPID(0);_g1=A+2\n else:setPID(0);_g1=A\ndef setPID(pd):i2c.write(16,bytearray([10,pd]))\ndef stop():setPID(0);w(0,0,0,0)\ndef resetSpeed():setPID(0);A=50\ndef forward():w(1,_g1,1,_g1)\ndef backward():w(2,_g1,2,_g1)\ndef left():A=1.825-.0175*_g1;w(2,int(_g1*A),1,int(_g1*A))\ndef right():A=1.825-.0175*_g1;w(1,int(_g1*A),2,int(_g1*A))\ndef rightArc(r):\n A=abs(_g1)\n if r<_g2:B=0\n else:C=(r-_g2)/(r+_g2)*(1-A*A/200000);B=int(C*A)\n if _g1>0:w(1,A,1,B)\n else:w(2,B,2,A)\ndef leftArc(r):\n A=abs(_g1)\n if r<_g2:B=0\n else:C=(r-_g2)/(r+_g2)*(1-A*A/200000);B=int(C*A)\n if _g1>0:w(1,B,1,A)\n else:w(2,A,2,B)\ndef getDistance():pin1.write_digital(1);pin1.write_digital(0);B=machine.time_pulse_us(pin2,1,50000);A=int(B/58.2-.5);return A if A>0 else 255\nclass Motor:\n def __init__(A,id):A._id=2*id\n def _f2(A,d,s):\n try:i2c.write(16,bytearray([A._id,d,s]))\n except:\n print(_A)\n while True:0\n def rotate(A,s):\n B=abs(s)\n if s>0:A._f2(1,B)\n elif s<0:A._f2(2,B)\n else:A._f2(0,0)\nclass LEDState:OFF=0;RED=1;GREEN=2;YELLOW=3;BLUE=4;PINK=5;CYAN=6;WHITE=7\ndef setLED(state,stateR=None):B=state;A=stateR;A=A or B;i2c.write(16,bytearray([11,B,A]))\ndef setLEDLeft(state):i2c.write(16,bytearray([11,state]))\ndef setLEDRight(state):i2c.write(16,bytearray([12,state]))\ndef setAlarm(on):\n if on:music.play(_g3,wait=False,loop=True)\n else:music.stop()\ndef beep():music.pitch(2000,200,wait=False)\ndef ir_read_values_as_byte():i2c.write(16,bytearray([29]));A=i2c.read(16,1);return~A[0]\ndef setServo(S,Angle):\n if S=='S1':A=20\n if S=='S2':A=21\n B=A,Angle;i2c.write(16,bytes(B))\nclass IR:L3=0;L2=1;L1=2;R1=3;R2=4;R3=5;masks=[1,2,4,8,16,32]\nclass IRSensor:\n def __init__(A,index):A.index=index\n def read_digital(A):B=ir_read_values_as_byte();return(B&IR.masks[A.index])>>A.index\nirLeft=IRSensor(IR.L1)\nirRight=IRSensor(IR.R1)\nirL1=IRSensor(IR.L1)\nirR1=IRSensor(IR.R1)\nirL2=IRSensor(IR.L2)\nirR2=IRSensor(IR.R2)\nirL3=IRSensor(IR.L3)\nirR3=IRSensor(IR.R3)\npin2.set_pull(pin2.NO_PULL)\nmotL=Motor(0)\nmotR=Motor(1)\ndelay=sleep\n_g3=['c6:1','r','c6,1','r','r','r']",
|
|
276
|
+
mbrobot: `from microbit import i2c,pin1,pin2,pin8,pin12,pin13,pin14,pin15,sleep
|
|
277
|
+
import gc,machine,music,neopixel
|
|
278
|
+
_g1=50
|
|
279
|
+
_g2=40
|
|
280
|
+
_g3=40
|
|
281
|
+
_g4=bytearray(5)
|
|
282
|
+
_g5=bytearray(2)
|
|
283
|
+
_g6=bytes(b'\\x00\\x0b\\x0b\\x0c\\x0c\\r\\r\\r\\x0e\\x0e\\x0f\\x0f\\x0f\\x10\\x10\\x11\\x11\\x11\\x12\\x12\\x13\\x13\\x13\\x14\\x14\\x15\\x15\\x15\\x16\\x16\\x17\\x17\\x17\\x18\\x19\\x1a\\x1b\\x1b\\x1c\\x1d\\x1e\\x1f !"##$%&\\'()*++,-./0123468:<?ADFILOSVZ^bgkpu{\\x81\\x87\\x8d\\x94\\x9b\\xa3\\xab\\xb4\\xbd\\xc6\\xd0\\xdb\\xe6\\xf2\\xff')
|
|
284
|
+
_g7=0
|
|
285
|
+
_g8=0
|
|
286
|
+
_g9=0
|
|
287
|
+
_g10=neopixel.NeoPixel(pin15,4)
|
|
288
|
+
np_rgb_pixels=_g10
|
|
289
|
+
_g11=['c5:1','r','c5:1','r:3']
|
|
290
|
+
_g12='Please connect to Maqueen robot and switch it on.'
|
|
291
|
+
def _f1(dirL,powerL,dirR,powerR):
|
|
292
|
+
global _g4;_g4[1]=dirL;_g4[2]=powerL;_g4[3]=dirR;_g4[4]=powerR
|
|
293
|
+
try:i2c.write(16,_g4)
|
|
294
|
+
except:raise RuntimeError(_g12)
|
|
295
|
+
def _f2(side,dir,power):
|
|
296
|
+
global _g4;_g4[1+side]=dir;_g4[2+side]=power
|
|
297
|
+
try:i2c.write(16,_g4)
|
|
298
|
+
except:raise RuntimeError(_g12)
|
|
299
|
+
def _f3(speed,offset):return min(_g6[speed]+offset,255)
|
|
300
|
+
def _f4(r):
|
|
301
|
+
A=_g1;B=int(r*100);D=A-max(B+20,40)
|
|
302
|
+
if D<=0:A=min(max(B+40,40),100)
|
|
303
|
+
C=0
|
|
304
|
+
if B>=4:E=(100-A)//2;C=(B*10-35)/(B*(11+(_g9-4)/10)+90+E);C=C*A
|
|
305
|
+
F=_f3(int(C),0);G=_f3(int(A),0);return F,G
|
|
306
|
+
def calibrate(offset,differential=0,arcScaling=0):global _g8;global _g7;global _g9;_g7=max(min(int(offset),50),-10);_g8=max(min(int(differential),150),-150);_g9=max(min(arcScaling,50),-15);setSpeed(_g1)
|
|
307
|
+
def setSpeed(speed):
|
|
308
|
+
global _g1;global _g2;global _g3;_g1=int(min(max(speed,0),100));A=_f3(_g1,_g7);B=round((1-_g1/100)*abs(_g8))if _g1>0 else 0;C=round(_g1/100*abs(_g8))
|
|
309
|
+
if _g8>0:_g2=A-C;_g3=A+B
|
|
310
|
+
else:_g2=A+B;_g3=A-C
|
|
311
|
+
def resetSpeed():setSpeed(50)
|
|
312
|
+
def stop():_f1(0,0,0,0)
|
|
313
|
+
def forward():_f1(0,_g2,0,_g3)
|
|
314
|
+
def backward():_f1(1,_g2,1,_g3)
|
|
315
|
+
def left():_f1(1,_g2,0,_g3)
|
|
316
|
+
def right():_f1(0,_g2,1,_g3)
|
|
317
|
+
def rightArc(radius):A,B=_f4(radius);_f1(0,B,0,A)
|
|
318
|
+
def leftArc(radius):A,B=_f4(radius);_f1(0,A,0,B)
|
|
319
|
+
class Motor:
|
|
320
|
+
def __init__(A,side):A._side=side
|
|
321
|
+
def rotate(B,speed):A=speed;C=int(min(max(abs(A),0),100));D=_f3(C,_g7);E=0 if A>0 else 1;_f2(B._side,E,D)
|
|
322
|
+
def setServo(servo,angle):
|
|
323
|
+
B=angle;A=servo;global _g5
|
|
324
|
+
if A=='S1'or A=='P0':_g5[0]=20
|
|
325
|
+
elif A=='S2'or A=='P1':_g5[0]=21
|
|
326
|
+
else:raise ValueError("Unknown Servo. Please use 'S1' or 'S2'.")
|
|
327
|
+
if B<0 or B>180:raise ValueError('Invalid angle. Must be between 0 and 180')
|
|
328
|
+
_g5[1]=B
|
|
329
|
+
try:i2c.write(16,_g5)
|
|
330
|
+
except:raise RuntimeError(_g12)
|
|
331
|
+
class IRSensor:
|
|
332
|
+
def __init__(A,pin):A._pin=pin
|
|
333
|
+
def read_digital(A):return A._pin.read_digital()
|
|
334
|
+
def read_analog(A):raise NameError('Maqueen Lite does not support reading analog sensor values.')
|
|
335
|
+
def getDistance():pin1.write_digital(1);pin1.write_digital(0);A=machine.time_pulse_us(pin2,1,50000);B=(A>>6)+(A>>10)+(A>>11)+(A>>12)+1;return max(min(B,500),0)if B>0 else 255
|
|
336
|
+
def setLED(state,stateR=None):B=state;A=stateR;A=A if A!=None else B;pin8.write_digital(B);pin12.write_digital(A)
|
|
337
|
+
def setLEDLeft(state):pin8.write_digital(state)
|
|
338
|
+
def setLEDRight(state):pin12.write_digital(state)
|
|
339
|
+
def fillRGB(red,green,blue):
|
|
340
|
+
for A in range(4):_g10[A]=red,green,blue
|
|
341
|
+
_g10.show()
|
|
342
|
+
setRGB=fillRGB
|
|
343
|
+
def clearRGB():_g10.clear()
|
|
344
|
+
def posRGB(position,red,green,blue):
|
|
345
|
+
A=position
|
|
346
|
+
if A<0 or A>3:raise ValueError('invalid RGB-LED position. Must be 0,1,2 or 3.')
|
|
347
|
+
_g10[A]=red,green,blue;_g10.show()
|
|
348
|
+
def setAlarm(state):
|
|
349
|
+
if state:music.play(_g11,wait=False,loop=True)
|
|
350
|
+
else:music.stop()
|
|
351
|
+
def beep():music.pitch(440,200,wait=False)
|
|
352
|
+
pin2.set_pull(pin2.NO_PULL)
|
|
353
|
+
delay=sleep
|
|
354
|
+
irLeft=IRSensor(pin13)
|
|
355
|
+
irRight=IRSensor(pin14)
|
|
356
|
+
motL=Motor(0)
|
|
357
|
+
motR=Motor(2)`,
|
|
358
|
+
mbrobot_plusV3: `_B=True
|
|
359
|
+
_A=False
|
|
360
|
+
from microbit import i2c,sleep,running_time,pin0,pin1,pin2
|
|
361
|
+
import neopixel,music
|
|
362
|
+
_g1=bytearray(5)
|
|
363
|
+
_g2=50
|
|
364
|
+
_g3=50
|
|
365
|
+
_g4=50
|
|
366
|
+
_g5=bytes(b'\\x00\\x0f\\x10\\x10\\x11\\x12\\x12\\x13\\x13\\x14\\x15\\x15\\x16\\x16\\x17\\x18\\x18\\x19\\x19\\x1a\\x1b\\x1b\\x1c\\x1c\\x1d\\x1e\\x1e\\x1f\\x1f !!""#$$%%&\\')*+,-./0123456789:;;<>?ACEGIKMPRUX[^aeimquz\\x7f\\x84\\x89\\x8f\\x95\\x9b\\xa2\\xa9\\xb1\\xb9\\xc1\\xca\\xd3\\xdd\\xe8\\xf3\\xff')
|
|
367
|
+
_g6=0
|
|
368
|
+
_g7='Please connect to Maqueen robot and switch it on.'
|
|
369
|
+
_g8=neopixel.NeoPixel(pin1,4)
|
|
370
|
+
_g9=['c5:1','r','c5,1','r:3']
|
|
371
|
+
_g10=bytearray(1)
|
|
372
|
+
_g11=bytearray(2)
|
|
373
|
+
_g12=16
|
|
374
|
+
_g13=25
|
|
375
|
+
_g14=131
|
|
376
|
+
_g15=8
|
|
377
|
+
def _f1(reg):_g10[0]=reg;i2c.write(_g12,_g10)
|
|
378
|
+
def _f2(reg,val):_g11[0]=reg;_g11[1]=val;i2c.write(_g12,_g11)
|
|
379
|
+
def _f3(dirL,powerL,dirR,powerR):
|
|
380
|
+
global _g1;_g1[1]=dirL;_g1[2]=powerL;_g1[3]=dirR;_g1[4]=powerR
|
|
381
|
+
try:i2c.write(16,_g1)
|
|
382
|
+
except:raise RuntimeError(_g7)
|
|
383
|
+
def _f4(side,dir,power):
|
|
384
|
+
global _g1;_g1[1+side]=dir;_g1[2+side]=power
|
|
385
|
+
try:i2c.write(16,_g1)
|
|
386
|
+
except:raise RuntimeError(_g7)
|
|
387
|
+
def setSpeed(speed):A=speed;global _g2;global _g3;global _g4;_g2=int(min(max(A,0),100));_g3=int(round(2.4*A+14));_g4=int(round(2.4*A+14))
|
|
388
|
+
def resetSpeed():setSpeed(50)
|
|
389
|
+
def stop():_f3(0,0,0,0)
|
|
390
|
+
def forward():_f3(0,_g3,0,_g4)
|
|
391
|
+
def backward():_f3(1,_g3,1,_g4)
|
|
392
|
+
def left():_f3(1,_g3,0,_g4)
|
|
393
|
+
def right():_f3(0,_g3,1,_g4)
|
|
394
|
+
def _f5(speed,offset):return min(_g5[speed]+offset,255)
|
|
395
|
+
def _f6(r):
|
|
396
|
+
B=int(r*100);A=_g2
|
|
397
|
+
if A<25:A=25
|
|
398
|
+
C=0
|
|
399
|
+
if B>5:
|
|
400
|
+
D=A*(3*_g6-A-9*B+220);E=-14*_g6+A-200+3*A-10*B-290;C=int(D/E)
|
|
401
|
+
if C<2:C=2 if B>15 else 1
|
|
402
|
+
F=_f5(int(C),0);G=_f5(int(A),0);return F,G
|
|
403
|
+
def rightArc(radius):A,B=_f6(radius);_f3(0,B,0,A)
|
|
404
|
+
def leftArc(radius):A,B=_f6(radius);_f3(0,A,0,B)
|
|
405
|
+
class Motor:
|
|
406
|
+
def __init__(A,side):A._side=side
|
|
407
|
+
def rotate(B,speed):A=speed;C=int(min(max(abs(A),0),100));D=C;E=0 if A>0 else 1;_f4(B._side,E,D)
|
|
408
|
+
def setServo(servo,angle):
|
|
409
|
+
D=angle;C=servo
|
|
410
|
+
if D<0 or D>180:raise ValueError('Invalid angle. Must be between 0 and 180')
|
|
411
|
+
if C in['P0','S1']:A=pin0
|
|
412
|
+
elif C in['P1','S2']:A=pin1
|
|
413
|
+
elif C in['P2','S3']:A=pin2
|
|
414
|
+
else:raise ValueError('Valid servo names: S1, S2, S3 or P0, P1, P2')
|
|
415
|
+
B=(_g14-_g13)*int(D);E=(B>>8)+(B>>10)+(B>>11)+(B>>12);F=_g13+E;A.set_analog_period(20);A.write_analog(F)
|
|
416
|
+
class IRSensor:
|
|
417
|
+
_g19=bytes(b'\\x1d')
|
|
418
|
+
def __init__(A,index):A.index=index
|
|
419
|
+
def read_digital(A):
|
|
420
|
+
try:i2c.write(16,IRSensor._g19)
|
|
421
|
+
except:raise RuntimeError(_g7)
|
|
422
|
+
B=~i2c.read(16,1)[0];return(B&2**A.index)>>A.index
|
|
423
|
+
def read_analog(A):
|
|
424
|
+
try:_f1(29)
|
|
425
|
+
except:raise RuntimeError(_g7)
|
|
426
|
+
B=i2c.read(16,11);return B[2+2*A.index]<<8|B[1+2*A.index]
|
|
427
|
+
def setLEDs(rgbl,rgbr):_f2(11,rgbl);_f2(12,rgbr)
|
|
428
|
+
def setLED(state,stateR=None):B=state;A=stateR;A=A if A!=None else B;setLEDs(B,A)
|
|
429
|
+
def setLEDLeft(rgbl):_f2(11,rgbl)
|
|
430
|
+
def setLEDRight(rgbr):_f2(12,rgbr)
|
|
431
|
+
def fillRGB(red,green,blue):_g8.clear();_g8.fill((red,green,blue));_g8.show()
|
|
432
|
+
def setRGB(r,g,b):fillRGB(r,g,b)
|
|
433
|
+
def clearRGB():_g8.clear()
|
|
434
|
+
def posRGB(position,red,green,blue):
|
|
435
|
+
A=position
|
|
436
|
+
if A<0 or A>3:raise ValueError('invalid RGB-LED position. Must be 0,1,2 or 3.')
|
|
437
|
+
_g8[A]=red,green,blue;_g8.show()
|
|
438
|
+
def setAlarm(state):
|
|
439
|
+
if state:music.play(_g9,wait=_A,loop=_B)
|
|
440
|
+
else:music.stop()
|
|
441
|
+
def beep():music.pitch(440,200,wait=_A)
|
|
442
|
+
def readLightIntensity(side):
|
|
443
|
+
_f1(78);A=i2c.read(16,4,repeat=_A)
|
|
444
|
+
if side==1:return A[0]<<8|A[1]
|
|
445
|
+
else:return A[2]<<8|A[3]
|
|
446
|
+
def setPatrolSpeed(speed):_f2(63,speed)
|
|
447
|
+
def setIntersectionRunMode(mode):_f2(69,mode)
|
|
448
|
+
def setTRordRunMode(mode):_f2(70,mode)
|
|
449
|
+
def setLeftOrStraightRunMode(mode):_f2(71,mode)
|
|
450
|
+
def setRightOrStraightRunMode(mode):_f2(72,mode)
|
|
451
|
+
def patrolling(patrol):
|
|
452
|
+
if patrol==1:A=5
|
|
453
|
+
else:A=8
|
|
454
|
+
_f2(60,A)
|
|
455
|
+
def intersectionDetecting():_f1(61);A=i2c.read(16,1)[0];return A
|
|
456
|
+
def pidControlDistance(dir,distance,interruption):
|
|
457
|
+
A=distance;C=2
|
|
458
|
+
if A>=6000:A=60000
|
|
459
|
+
_f2(64,dir);_f2(85,C);_f2(65,A>>8);_f2(66,A);_f2(60,6)
|
|
460
|
+
if interruption==1:
|
|
461
|
+
_f1(87);B=i2c.read(16,1)
|
|
462
|
+
while B[0]==1:sleep(10);B=i2c.read(16,1)
|
|
463
|
+
def pidControlAngle(angle,interruption):
|
|
464
|
+
A=angle;D=2
|
|
465
|
+
if A>=0:B=1
|
|
466
|
+
else:B=2;A=-A
|
|
467
|
+
_f2(67,B);_f2(86,D);_f2(68,A);_f2(60,6)
|
|
468
|
+
if interruption==1:
|
|
469
|
+
_f1(87);C=i2c.read(16,1)
|
|
470
|
+
while C[0]==1:sleep(10);C=i2c.read(16,1)
|
|
471
|
+
def pidControlStop():_f2(60,16)
|
|
472
|
+
def readRealTimeSpeed(type):
|
|
473
|
+
_f2(76,1);A=i2c.read(16,2)
|
|
474
|
+
if type==1:return A[0]/5
|
|
475
|
+
else:return A[1]/5
|
|
476
|
+
def _f7(cmd,args=[]):
|
|
477
|
+
B=len(args);C=B+1;A=bytearray(4+B);A[0]=85;A[1]=C>>8&255;A[2]=C&255;A[3]=cmd
|
|
478
|
+
for(D,E)in enumerate(args):A[4+D]=E
|
|
479
|
+
i2c.write(51,A)
|
|
480
|
+
def _f8(expectedCommand):
|
|
481
|
+
F=1000;J=32;G=running_time();B=_A;A=None;C=_A
|
|
482
|
+
while running_time()-G<F and C==_A:
|
|
483
|
+
H=i2c.read(51,1)[0]
|
|
484
|
+
if H==83:C=_B
|
|
485
|
+
elif H==99:return B,A
|
|
486
|
+
sleep(16)
|
|
487
|
+
if C==_B:
|
|
488
|
+
D=i2c.read(51,3);K=D[0];L=D[1]|D[2]<<8
|
|
489
|
+
if K==expectedCommand:
|
|
490
|
+
B=_B;A=bytearray();E=L;M=min(E,J)
|
|
491
|
+
while running_time()-G<F and E>0:
|
|
492
|
+
try:I=i2c.read(51,M);A.extend(I);E-=len(I)
|
|
493
|
+
except:sleep(1)
|
|
494
|
+
return B,A
|
|
495
|
+
def setLidarMode(mode=8):
|
|
496
|
+
A=mode;global _g15
|
|
497
|
+
if A not in[4,8]:raise ValueError('Lidar mode must be 4 or 8')
|
|
498
|
+
C='4x4'if A==4 else'8x8';print('Switching Lidar Mode to '+C+'.\\nPlease wait up to 10 seconds.');B=_A
|
|
499
|
+
for D in range(10):
|
|
500
|
+
_f7(1,[0,0,0,A]);B,E=_f8(1)
|
|
501
|
+
if B==_B:break
|
|
502
|
+
sleep(17)
|
|
503
|
+
if B:_g15=A;sleep(5000)
|
|
504
|
+
else:raise RuntimeError('Failed to switch Lidar Mode')
|
|
505
|
+
def getDistanceAt(x_pos,y_pos):
|
|
506
|
+
_f7(3,[x_pos,y_pos]);B,A=_f8(3)
|
|
507
|
+
if B and len(A)>=2:C=(A[0]|A[1]<<8)//10;return C
|
|
508
|
+
else:return 1023
|
|
509
|
+
def getDistanceList():
|
|
510
|
+
_f7(2);D,A=_f8(2)
|
|
511
|
+
if D and len(A)>=32:
|
|
512
|
+
B=[]
|
|
513
|
+
for C in range(0,len(A),2):E=A[C]|A[C+1]<<8;B.append(E//10)
|
|
514
|
+
return B
|
|
515
|
+
else:return[]
|
|
516
|
+
def getDistance():global _g15;A=int(_g15/2);B=getDistanceAt(A-1,A-1);C=getDistanceAt(A,A-1);D=getDistanceAt(A-1,A);E=getDistanceAt(A,A);F=[B,C,D,E];return min(F)
|
|
517
|
+
def getDistanceGrid():
|
|
518
|
+
_f7(2);G,A=_f8(2)
|
|
519
|
+
if G and len(A)>=32:
|
|
520
|
+
B=[];C=16 if len(A)==128 else 8
|
|
521
|
+
for D in range(0,len(A),C):
|
|
522
|
+
E=[]
|
|
523
|
+
for F in range(0,C,2):H=A[D+F]|A[D+F+1]<<8;E.append(H//10)
|
|
524
|
+
B.append(E)
|
|
525
|
+
return B
|
|
526
|
+
else:return[]
|
|
527
|
+
def getDistanceColumn(index):
|
|
528
|
+
_f7(5,[index]);D,A=_f8(5)
|
|
529
|
+
if D and len(A)>=8:
|
|
530
|
+
B=[]
|
|
531
|
+
for C in range(0,len(A),2):E=A[C]|A[C+1]<<8;B.append(E//10)
|
|
532
|
+
return B
|
|
533
|
+
return[]
|
|
534
|
+
def getDistanceRow(index):
|
|
535
|
+
_f7(6,[index]);D,A=_f8(6)
|
|
536
|
+
if D and len(A)>=8:
|
|
537
|
+
B=[]
|
|
538
|
+
for C in range(0,len(A),2):E=A[C]|A[C+1]<<8;B.append(E//10)
|
|
539
|
+
return B
|
|
540
|
+
return[]
|
|
541
|
+
pin2.set_pull(pin2.NO_PULL)
|
|
542
|
+
delay=sleep
|
|
543
|
+
irR2=IRSensor(0)
|
|
544
|
+
irR1=IRSensor(1)
|
|
545
|
+
irRight=irR1
|
|
546
|
+
irM=IRSensor(2)
|
|
547
|
+
irL1=IRSensor(3)
|
|
548
|
+
irLeft=irL1
|
|
549
|
+
irL2=IRSensor(4)
|
|
550
|
+
motL=Motor(0)
|
|
551
|
+
motR=Motor(2)`,
|
|
552
|
+
mbmarsrover: "from microbit import*\nfrom neopixel import*\nimport utime\n_g1=[0,9,11,13,15]\n_g2=[0,0,0,0,0]\n_g3=False\n_g4=4\n_g5=NeoPixel(pin2,_g4)\n_g6=50\n_g7=40\n_g8=40\n_g9=0\n_g10=0\n_g11=0\n_g12=500\n_g13=29.1\ndef _f1(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)\ndef _f2(side,direction,power):\n if side==0:pins=pin1,pin12\n elif side==1:pins=pin8,pin0\n pins[direction].write_analog(power);pins[1-direction].write_analog(0)\ndef _f3(r):\n outerSpeed=_g6;rCm=int(r*100);threshold=outerSpeed-max(rCm+20,40)\n if threshold<=0:outerSpeed=min(max(rCm+40,40),100)\n reducedSpeed=0\n if rCm>=4:flattening=(100-outerSpeed)//2;reducedSpeed=(rCm*10-35)/(rCm*(11+(_g11-4)/10)+90+flattening);reducedSpeed=reducedSpeed*outerSpeed\n innerByte=_f6(int(reducedSpeed),0);outerByte=_f6(int(outerSpeed),0);return innerByte,outerByte\ndef _f4():global _g3;_g3=True;i2cData=bytearray(2);i2cData[0]=0;i2cData[1]=16;i2c.write(64,i2cData);i2cData[0]=254;i2cData[1]=101;i2c.write(64,i2cData);i2cData[0]=0;i2cData[1]=129;i2c.write(64,i2cData)\ndef _f5(pin,value,timeout):\n start_time=utime.ticks_us()\n while pin.read_digital()!=value:\n if utime.ticks_diff(utime.ticks_us(),start_time)>timeout:return 0\n start_time=utime.ticks_us()\n while pin.read_digital()==value:\n if utime.ticks_diff(utime.ticks_us(),start_time)>timeout:return 0\n return utime.ticks_diff(utime.ticks_us(),start_time)\ndef calibrate(offset,differential=0,arcScaling=0):global _g10;global _g9;global _g11;_g9=max(min(int(offset),500),-50);_g10=max(min(int(differential),150),-150);_g11=max(min(arcScaling,50),-15);setSpeed(_g6)\ndef setSpeed(speed):\n global _g6;global _g7;global _g8;_g6=int(min(max(speed,0),100));powerValue=_f6(_g6,_g9);boost=round((1-_g6/100)*abs(_g10))if _g6>0 else 0;reduction=round(_g6/100*abs(_g10))\n if _g10>0:_g7=powerValue-reduction;_g8=powerValue+boost\n else:_g7=powerValue+boost;_g8=powerValue-reduction\ndef _f6(speed,offset):analogValue=int(speed*1023/100)+offset;return min(max(analogValue,0),1023)\ndef stop():_f1(0,0,0,0)\ndef forward():_f1(0,_g7,0,_g8)\ndef backward():_f1(1,_g7,1,_g8)\ndef setServo(servoNum,angle):\n global _g1,_g3\n if _g3==False:_f4()\n offsetNum=servoNum;servoNum=_g1[servoNum];i2cData=bytearray(2);start=0;angle=max(min(angle,90),-90);stop=396+(angle+_g2[offsetNum])*223/90;i2cData[0]=6+servoNum*4+2;i2cData[1]=int(stop)+255;i2c.write(64,i2cData);i2cData[0]=6+servoNum*4+3;i2cData[1]=int(stop)>>8;i2c.write(64,i2cData)\ndef clearServos():setServo(0,0);setServo(1,0);setServo(2,0);setServo(3,0);setServo(4,0);sleep(500)\ndef steer(angle):angle=max(min(angle,45),-45);setServo(1,angle);setServo(2,-angle);setServo(3,-angle);setServo(4,angle);sleep(500)\ndef setLED(pos,red,green,blue):_g5[pos]=red,green,blue;_g5.show()\ndef fill(red,green,blue):\n for i in range(_g4):_g5[i]=red,green,blue\n _g5.show()\ndef getDistance():\n trig=pin13;echo=pin13;d=10;trig.set_pull(trig.NO_PULL)\n for _ in range(10):\n trig.write_digital(0);utime.sleep_us(2);trig.write_digital(1);utime.sleep_us(10);trig.write_digital(0);duration=_f5(echo,1,_g12*_g13)\n if duration>0:d=duration;break\n return round(d/_g13)\ndef calibrateServo(pos,angleDifferential):global _g2;angleDifferential=max(min(angleDifferential,90),-90);_g2[pos]=angleDifferential;clearServos()",
|
|
553
|
+
mbrobotmot: "import gc\nfrom microbit import i2c,pin1,pin2,pin8,pin12,pin13,pin14,sleep\nimport machine\nclass Motor:\n def __init__(A,id):A._id=2*id\n def rotate(A,s):\n B=abs(s)\n if s>0:A._f2(0,B)\n elif s<0:A._f2(1,B)\n else:A._f2(0,0)\n def _f2(A,d,s):\n try:i2c.write(16,bytearray([A._id,d,s]))\n except:\n print('Please switch on mbRobot!')\n while True:0\ndelay=sleep\ndef getDistance():pin1.write_digital(1);pin1.write_digital(0);B=machine.time_pulse_us(pin2,1,50000);A=int(B/58.2+.5);return A if A>0 else 255\ndef setLED(on):pin8.write_digital(on);pin12.write_digital(on)\npin2.set_pull(pin2.NO_PULL)\nirLeft=pin13\nirRight=pin14\nledLeft=pin8\nledRight=pin12\nmotL=Motor(0)\nmotR=Motor(1)",
|
|
554
|
+
controller: "_B=False\n_A=1.\nfrom microbit import run_every,pin13,pin14,pin15,pin16,pin8,pin1,pin2,pin12,button_a,button_b,sleep\nclass _Controller_Button:\n def __init__(A,pin):B=pin;A._pin=B;A.previous_state=0;A._press_count=0;A._pressed_before=_B;B.set_pull(B.PULL_UP)\n def _f3(A):\n B=1-A._pin.read_digital()\n if B==1 and A.previous_state==0:A._press_count+=1;A._pressed_before=True\n A.previous_state=B\n def is_pressed(A):return _B if A._pin.read_digital()else True\n def was_pressed(A):B=A._pressed_before;A._pressed_before=_B;return B\n def get_presses(A):B=A._press_count;A._press_count=0;return B\nclass _Controller_Analog_Stick:\n def __init__(A,pinX,pinY,pinZ):A.pin_x=pinX;A.pin_y=pinY;A.button_z=_Controller_Button(pinZ);A.dead_zone=.01;A.center_x=0;A.center_y=0;A.min_x=-1;A.max_x=1;A.min_y=-1;A.max_y=1\n def calibrate(A,dead_zone,center_x=.0,center_y=.0,x_min=-_A,x_max=_A,y_min=-_A,y_max=_A):A.dead_zone=min(.2,max(.01,dead_zone));A.center_x=min(.4,max(-.4,center_x));A.center_y=min(.5,max(-.5,center_y));A.min_x=min(-.5,max(-_A,x_min));A.max_x=min(_A,max(.5,x_max));A.min_y=min(-.5,max(-_A,y_min));A.max_y=min(_A,max(.5,y_max))\n def _f5(C,value,center,v_min,v_max):\n B=center;A=value-_A-B\n if abs(A)-C.dead_zone/2.>.0:\n if A<.0:A=A/abs(v_min-B)\n else:A=A/abs(v_max-B)\n A=min(_A,max(-_A,A));A=.0 if abs(A)<=C.dead_zone else A;return A\n def get_x(A):B=A.pin_x.read_analog()/512.;C=A._f5(B,A.center_x,A.min_x,A.max_x);return C\n def get_y(A):B=A.pin_y.read_analog()/512.;C=A._f5(B,A.center_y,A.min_y,A.max_y);return C\n def get_z(A):return 1 if A.button_z.is_pressed()else 0\n def is_pressed(A):return A.button_z.is_pressed()\n def was_pressed(A):return A.button_z.was_pressed()\n def get_presses(A):return A.button_z.get_presses()\ndef vibrate(state):pin12.write_digital(state)\njoystick=_Controller_Analog_Stick(pin1,pin2,pin8)\nbutton_z=joystick.button_z\nbutton_c=_Controller_Button(pin13)\nbutton_green=button_c\nbutton_d=_Controller_Button(pin14)\nbutton_yellow=button_d\nbutton_e=_Controller_Button(pin15)\nbutton_red=button_e\nbutton_f=_Controller_Button(pin16)\nbutton_blue=button_f\ntrigger_left=button_a\ntrigger_right=button_b\ndef _f1():global button_c;global button_d;global button_e;global button_f;global joystick;button_c._f3();button_d._f3();button_e._f3();button_f._f3();joystick.button_z._f3()\nrun_every(_f1,days=0,h=0,min=0,s=0,ms=33)",
|
|
555
|
+
mbxgo: "from microbit import*\n_g1=50\n_g2=False\n_g3=pin14\n_g4=pin13\ndef checkInit(func):\n def wrapper(*args,**kwargs):\n global _g2\n if not _g2:init_xgo_serial(_g3,_g4)\n return func(*args,**kwargs)\n return wrapper\ndef _f1(speed,in_min,in_max,out_min,out_max):return(speed-in_min)*(out_max-out_min)//(in_max-in_min)+out_min\n@checkInit\ndef _f2(direction,speed):\n move_buffer=bytearray(9);move_buffer[0]=85;move_buffer[1]=0;move_buffer[2]=9;move_buffer[3]=0;move_buffer[7]=0;move_buffer[8]=170;speed=max(0,min(100,speed))\n if direction==0:move_buffer[4]=48;move_buffer[5]=_f1(speed,0,100,128,255)\n elif direction==1:move_buffer[4]=48;move_buffer[5]=_f1(speed,0,100,128,0)\n elif direction==2:move_buffer[4]=49;move_buffer[5]=_f1(speed,0,100,128,0)\n elif direction==3:move_buffer[4]=49;move_buffer[5]=_f1(speed,0,100,128,255)\n move_buffer[6]=~(9+move_buffer[4]+move_buffer[5])&255;uart.write(move_buffer)\n@checkInit\ndef clampX(milimeters=50):clampBuffer=bytearray(9);clampBuffer[0]=85;clampBuffer[1]=0;clampBuffer[2]=9;clampBuffer[3]=0;clampBuffer[4]=115;clampBuffer[7]=0;clampBuffer[8]=170;clampBuffer[5]=milimeters;clampBuffer[6]=~(124+clampBuffer[5])&255;uart.write(clampBuffer);sleep(1000)\n@checkInit\ndef clampZ(milimeters=50):clampBuffer=bytearray(9);clampBuffer[0]=85;clampBuffer[1]=0;clampBuffer[2]=9;clampBuffer[3]=0;clampBuffer[4]=116;clampBuffer[7]=0;clampBuffer[8]=170;clampBuffer[5]=milimeters;clampBuffer[6]=~(125+clampBuffer[5])&255;uart.write(clampBuffer);sleep(1000)\n@checkInit\ndef clamp(force):clampBuffer=bytearray(9);clampBuffer[0]=85;clampBuffer[1]=0;clampBuffer[2]=9;clampBuffer[3]=0;clampBuffer[4]=113;clampBuffer[7]=0;clampBuffer[8]=170;clampBuffer[5]=force;clampBuffer[6]=~(122+clampBuffer[5])&255;uart.write(clampBuffer);sleep(1000)\ndef init_xgo_serial(tx_pin,rx_pin,baudrate=115200):global _g2;uart.init(baudrate=baudrate,tx=tx_pin,rx=rx_pin);init_action();_g2=True\ndef init_action():commands_buffer=bytearray(9);commands_buffer[0]=85;commands_buffer[1]=0;commands_buffer[2]=9;commands_buffer[3]=0;commands_buffer[4]=62;commands_buffer[5]=255;commands_buffer[6]=185;commands_buffer[7]=0;commands_buffer[8]=170;uart.write(commands_buffer);sleep(2000)\n@checkInit\ndef action(id):commands_buffer=bytearray(9);commands_buffer[0]=85;commands_buffer[1]=0;commands_buffer[2]=9;commands_buffer[3]=0;commands_buffer[4]=62;commands_buffer[5]=id;commands_buffer[6]=~(71+id)&255;commands_buffer[7]=0;commands_buffer[8]=170;uart.write(commands_buffer);sleep(2000)\ndef changeInit(tx,rx):global _g3;global _g4;_g3=tx;_g4=rx;init_xgo_serial(_g3,_g4)\n@checkInit\ndef setSpeed(speed):global _g1;_g1=speed\ndef forward():_f2(0,_g1)\ndef backward():_f2(1,_g1)\ndef left():_f2(2,_g1)\ndef right():_f2(3,_g1)",
|
|
556
|
+
mbglow: `from microbit import*
|
|
557
|
+
_g1=0
|
|
558
|
+
_g2=0
|
|
559
|
+
_g3=0
|
|
560
|
+
_g4=50
|
|
561
|
+
_g5=True
|
|
562
|
+
_g6=False
|
|
563
|
+
def makeGlow():global _g6;display.set_pixel(2,2,9);_g6=True
|
|
564
|
+
def clear():display.clear()
|
|
565
|
+
def forward():_f1(1)
|
|
566
|
+
def back():_f1(-1)
|
|
567
|
+
def right(angle):global _g3;_g3=(_g3+angle)%360
|
|
568
|
+
def left(angle):right(-angle)
|
|
569
|
+
def setPos(x,y):global _g1,_g2;_g1=x;_g2=y;_f2()
|
|
570
|
+
def getPos():return _g1,_g2
|
|
571
|
+
def setSpeed(speed):global _g4;_g4=speed
|
|
572
|
+
def showTrace(enable):global _g5;_g5=enable
|
|
573
|
+
def isLit():return display.get_pixel(_g1+2,4-(_g2+2))==9
|
|
574
|
+
def _f1(s):
|
|
575
|
+
global _g1,_g2;sleep(2000-_g4*20);d=_g3//45
|
|
576
|
+
if d in[1,2,3]:_g1+=s
|
|
577
|
+
if d in[5,6,7]:_g1-=s
|
|
578
|
+
if d in[0,1,7]:_g2+=s
|
|
579
|
+
if d in[3,4,5]:_g2-=s
|
|
580
|
+
_f2()
|
|
581
|
+
def _f2():
|
|
582
|
+
if not _g6:print('Use "makeGlow()" to create a Glow.');raise Exception('Glow not initialized.')
|
|
583
|
+
if not _g5:display.clear()
|
|
584
|
+
if-2<=_g1<=2 and-2<=_g2<=2:display.set_pixel(_g1+2,4-(_g2+2),9)`,
|
|
585
|
+
mbthetabot: `from microbit import*
|
|
586
|
+
from utime import ticks_us,sleep_us
|
|
587
|
+
from neopixel import*
|
|
588
|
+
_g1=50
|
|
589
|
+
_g2=40
|
|
590
|
+
_g3=40
|
|
591
|
+
_g4=bytes(b'\\x00\\x0b\\x0b\\x0c\\x0c\\r\\r\\r\\x0e\\x0e\\x0f\\x0f\\x0f\\x10\\x10\\x11\\x11\\x11\\x12\\x12\\x13\\x13\\x13\\x14\\x14\\x15\\x15\\x15\\x16\\x16\\x17\\x17\\x17\\x18\\x19\\x1a\\x1b\\x1b\\x1c\\x1d\\x1e\\x1f !"##$%&\\'()*++,-./0123468:<?ADFILOSVZ^bgkpu{\\x81\\x87\\x8d\\x94\\x9b\\xa3\\xab\\xb4\\xbd\\xc6\\xd0\\xdb\\xe6\\xf2\\xff')
|
|
592
|
+
_g5=0
|
|
593
|
+
_g6=0
|
|
594
|
+
_g7=0
|
|
595
|
+
_g8=34
|
|
596
|
+
_g9=14
|
|
597
|
+
def _f1(dirL,powerL,dirR,powerR):pinsL=pin14,pin13;pinsR=pin16,pin15;pinsL[dirL].write_analog(powerL);pinsL[1-dirL].write_analog(0);pinsR[dirR].write_analog(powerR);pinsR[1-dirR].write_analog(0)
|
|
598
|
+
def _f2(side,direction,power):
|
|
599
|
+
if side==0:pins=pin14,pin13
|
|
600
|
+
elif side==2:pins=pin16,pin15
|
|
601
|
+
pins[direction].write_analog(power);pins[1-direction].write_analog(0)
|
|
602
|
+
def _f3(r):
|
|
603
|
+
outerSpeed=_g1;rCm=int(r*100);threshold=outerSpeed-max(rCm+20,40)
|
|
604
|
+
if threshold<=0:outerSpeed=min(max(rCm+40,40),100)
|
|
605
|
+
reducedSpeed=0
|
|
606
|
+
if rCm>=4:flattening=(100-outerSpeed)//2;reducedSpeed=(rCm*10-35)/(rCm*(11+(_g7-4)/10)+90+flattening);reducedSpeed=reducedSpeed*outerSpeed
|
|
607
|
+
innerByte=_f4(int(reducedSpeed),0);outerByte=_f4(int(outerSpeed),0);return innerByte,outerByte
|
|
608
|
+
def calibrate(offset,differential=0,arcScaling=0):global _g6;global _g5;global _g7;_g5=max(min(int(offset),150),-10);_g6=max(min(int(differential),150),-150);_g7=max(min(arcScaling,50),-15);setSpeed(_g1)
|
|
609
|
+
def setSpeed(speed):
|
|
610
|
+
global _g1;global _g2;global _g3;_g1=int(min(max(speed,0),100));powerByte=_f4(_g1,_g5);boost=round((1-_g1/100)*abs(_g6))if _g1>0 else 0;reduction=round(_g1/100*abs(_g6))
|
|
611
|
+
if _g6>0:_g2=powerByte-reduction;_g3=powerByte+boost
|
|
612
|
+
else:_g2=powerByte+boost;_g3=powerByte-reduction
|
|
613
|
+
def _f4(speed,offset):speedIndex=int(speed*(len(_g4)-1)/100);return min(_g4[speedIndex]+offset,1023)
|
|
614
|
+
def stop():_f1(0,0,0,0)
|
|
615
|
+
def forward():_f1(0,_g2,0,_g3)
|
|
616
|
+
def backward():_f1(1,_g2,1,_g3)
|
|
617
|
+
def left():_f1(1,_g2,0,_g3)
|
|
618
|
+
def right():_f1(0,_g2,1,_g3)
|
|
619
|
+
def rightArc(radius):inner,outer=_f3(radius);_f1(0,outer,0,inner)
|
|
620
|
+
def leftArc(radius):inner,outer=_f3(radius);_f1(0,inner,0,outer)
|
|
621
|
+
def getDistance():
|
|
622
|
+
pin12.write_digital(1);sleep_us(10);pin12.write_digital(0);pin12.set_pull(pin15.NO_PULL)
|
|
623
|
+
while pin12.read_digital()==0:0
|
|
624
|
+
start=ticks_us()
|
|
625
|
+
while pin12.read_digital()==1:0
|
|
626
|
+
end=ticks_us();echo=end-start;distance=int(.01715*echo);return distance
|
|
627
|
+
def setLED(position,red,green,blue):global _g8;i2c_data=bytearray(5);i2c_data[0]=1;i2c_data[1]=position;i2c_data[2]=red;i2c_data[3]=green;i2c_data[4]=blue;i2c.write(_g8,i2c_data)
|
|
628
|
+
def fill(red,green,blue):
|
|
629
|
+
for position in range(_g9):setLED(position,red,green,blue)
|
|
630
|
+
def readLine(side):i2c.write(_g8,bytearray([side+1]),False);result=i2c.read(_g8,2);result=result[0]+(result[1]<<8);return result
|
|
631
|
+
def readLight(side):i2c.write(_g8,bytearray([side+3]),False);result=i2c.read(_g8,2);result=result[0]+(result[1]<<8);return result`,
|
|
632
|
+
mbrobot_legacy: "import gc\nfrom microbit import i2c,pin1,pin2,pin8,pin12,pin13,pin14,sleep\nimport machine\n_g1=.097\ndef w(d1,d2,s1,s2):\n try:i2c.write(16,bytearray([0,d1,s1]));i2c.write(16,bytearray([2,d2,s2]))\n except:\n print('Please switch on mbRobot!')\n while True:0\ndef setSpeed(speed):\n A=speed;global _g2\n if A<20:_g2=A+5\n else:_g2=A\ndef forward():w(0,0,_g2,_g2)\ndef backward():w(1,1,_g2,_g2)\ndef stop():w(0,0,0,0)\ndef right():w(0 if _g2>0 else 1,1 if _g2>0 else 0,int(_g2*.9),int(_g2*.9))\ndef left():w(1 if _g2>0 else 0,0 if _g2>0 else 1,int(_g2*.9),int(_g2*.9))\ndef rightArc(r):\n A=abs(_g2)\n if r<_g1:B=0\n else:C=(r-_g1)/(r+_g1)*(1-A*A/200000);B=int(C*A)\n if _g2>0:w(0,0,A,B)\n else:w(1,1,B,A)\ndef leftArc(r):\n A=abs(_g2)\n if r<_g1:B=0\n else:C=(r-_g1)/(r+_g1)*(1-A*A/200000);B=int(C*A)\n if _g2>0:w(0,0,B,A)\n else:w(1,1,A,B)\nexit=stop\ndelay=sleep\ndef getDistance():pin1.write_digital(1);pin1.write_digital(0);B=machine.time_pulse_us(pin2,1,50000);A=int(B/58.2+.5);return A if A>0 else 255\ndef setLED(on):pin8.write_digital(on);pin12.write_digital(on)\ndef setServo(S,Angle):\n if S=='S1':A=20\n if S=='S2':A=21\n B=A,Angle;i2c.write(16,bytes(B))\npin2.set_pull(pin2.NO_PULL)\n_g2=50\nirLeft=pin13\nirRight=pin14\nledLeft=pin8\nledRight=pin12\nforward()",
|
|
633
|
+
mbalarm: "import music\n_g1=['c6:1','r','c6,1','r','r','r']\ndef setAlarm(on):\n if on:music.play(_g1,wait=False,loop=True)\n else:music.stop()\ndef beep():music.pitch(2000,200,wait=False)",
|
|
634
|
+
mbminibit: "from microbit import*\nfrom neopixel import*\nimport utime\n_g1=4\n_g2=NeoPixel(pin13,_g1)\n_g3=50\n_g4=90\n_g5=90\n_g6=0\n_g7=0\n_g8=0\n_g9=500\n_g10=29.1\ndef _f1(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)\ndef _f2(pin,value,timeout):\n start_time=utime.ticks_us()\n while pin.read_digital()!=value:\n if utime.ticks_diff(utime.ticks_us(),start_time)>timeout:return 0\n start_time=utime.ticks_us()\n while pin.read_digital()==value:\n if utime.ticks_diff(utime.ticks_us(),start_time)>timeout:return 0\n return utime.ticks_diff(utime.ticks_us(),start_time)\ndef _f3(r):\n outerSpeed=_g3\n if r>0:innerSpeed=outerSpeed*max(.2,min(1,1-r/100))\n else:innerSpeed=0\n innerByte=_f4(int(innerSpeed),0);outerByte=_f4(int(outerSpeed),0);return innerByte,outerByte\ndef _f4(speed,offset):analogValue=int(speed*255/100)+offset;return min(max(analogValue,0),255)\ndef calibrate(offset,differential=0,arcScaling=0):global _g7;global _g6;global _g8;_g6=max(min(int(offset),100),-10);_g7=max(min(int(differential),150),-150);_g8=max(min(arcScaling,50),-15);setSpeed(_g3)\ndef setSpeed(speed):\n global _g3;global _g4;global _g5;_g3=int(min(max(speed,0),100));powerByte=_f4(_g3,_g6);boost=round((1-_g3/100)*abs(_g7))if _g3>0 else 0;reduction=round(_g3/100*abs(_g7))\n if _g7>0:_g4=powerByte-reduction;_g5=powerByte+boost\n else:_g4=powerByte+boost;_g5=powerByte-reduction\ndef stop():_f1(0,0,0,0)\ndef forward():_f1(0,_g4,0,_g5)\ndef backward():_f1(1,_g4,1,_g5)\ndef left():_f1(1,_g4,0,_g5)\ndef right():_f1(0,_g4,1,_g5)\ndef rightArc(radius):inner,outer=_f3(radius);_f1(0,outer,0,inner)\ndef leftArc(radius):inner,outer=_f3(radius);_f1(0,inner,0,outer)\ndef setLED(pos,red,green,blue):_g2[pos]=red,green,blue;_g2.show()\ndef fill(red,green,blue):\n for i in range(_g1):_g2[i]=red,green,blue\n _g2.show()\ndef getDistance():\n trig=pin15;echo=pin15;d=10;trig.set_pull(trig.NO_PULL)\n for _ in range(10):\n trig.write_digital(0);utime.sleep_us(2);trig.write_digital(1);utime.sleep_us(10);trig.write_digital(0);duration=_f2(echo,1,_g9*_g10)\n if duration>0:d=duration;break\n return round(d/_g10)",
|
|
635
|
+
mbrobot_plusV2: `from microbit import i2c,pin0,pin1,pin2,pin13,pin14,pin15,sleep
|
|
636
|
+
import gc,machine,music,neopixel
|
|
637
|
+
_g1=50
|
|
638
|
+
_g2=50
|
|
639
|
+
_g3=50
|
|
640
|
+
_g4=bytearray(5)
|
|
641
|
+
_g5=bytes(b'\\x00\\x0f\\x10\\x10\\x11\\x12\\x12\\x13\\x13\\x14\\x15\\x15\\x16\\x16\\x17\\x18\\x18\\x19\\x19\\x1a\\x1b\\x1b\\x1c\\x1c\\x1d\\x1e\\x1e\\x1f\\x1f !!""#$$%%&\\')*+,-./0123456789:;;<>?ACEGIKMPRUX[^aeimquz\\x7f\\x84\\x89\\x8f\\x95\\x9b\\xa2\\xa9\\xb1\\xb9\\xc1\\xca\\xd3\\xdd\\xe8\\xf3\\xff')
|
|
642
|
+
_g6=0
|
|
643
|
+
_g7=0
|
|
644
|
+
_g8=0
|
|
645
|
+
_g9=25
|
|
646
|
+
_g10=131
|
|
647
|
+
_g11=bytearray(b'\\x0b\\x00\\x00')
|
|
648
|
+
_g12=neopixel.NeoPixel(pin15,4)
|
|
649
|
+
np_rgb_pixels=_g12
|
|
650
|
+
_g13=['c5:1','r','c5,1','r:3']
|
|
651
|
+
_g14='Please connect to Maqueen robot and switch it on.'
|
|
652
|
+
def _f1(dirL,powerL,dirR,powerR):
|
|
653
|
+
global _g4;_g4[1]=dirL;_g4[2]=powerL;_g4[3]=dirR;_g4[4]=powerR
|
|
654
|
+
try:i2c.write(16,_g4)
|
|
655
|
+
except:raise RuntimeError(_g14)
|
|
656
|
+
def _f2(side,dir,power):
|
|
657
|
+
global _g4;_g4[1+side]=dir;_g4[2+side]=power
|
|
658
|
+
try:i2c.write(16,_g4)
|
|
659
|
+
except:raise RuntimeError(_g14)
|
|
660
|
+
def _f3(speed,offset):return min(_g5[speed]+offset,255)
|
|
661
|
+
def _f4(r):
|
|
662
|
+
B=int(r*100);A=_g1
|
|
663
|
+
if A<25:A=25
|
|
664
|
+
H=min(abs(A-70),20)/20;C=0
|
|
665
|
+
if B>5:
|
|
666
|
+
D=A*(3*_g8-A-9*B+220);E=-14*_g8+A-200+3*A-10*B-290;C=int(D/E)
|
|
667
|
+
if C<2:C=2 if B>15 else 1
|
|
668
|
+
F=_f3(int(C),0);G=_f3(int(A),0);return F,G
|
|
669
|
+
def calibrate(offset,differential=0,arcScaling=0):global _g7;global _g6;global _g8;_g6=max(min(int(offset),50),-14);_g7=max(min(int(differential),150),-150);_g8=max(min(arcScaling,50),-50);setSpeed(_g1)
|
|
670
|
+
def setSpeed(speed):
|
|
671
|
+
global _g1;global _g2;global _g3;_g1=int(min(max(speed,0),100));A=_f3(_g1,_g6);B=round((1-_g1/100)*abs(_g7))if _g1>0 else 0;C=round(_g1/100*abs(_g7))
|
|
672
|
+
if _g7>0:_g2=A-C;_g3=A+B
|
|
673
|
+
else:_g2=A+B;_g3=A-C
|
|
674
|
+
def resetSpeed():setSpeed(50)
|
|
675
|
+
def stop():_f1(0,0,0,0)
|
|
676
|
+
def forward():_f1(0,_g2,0,_g3)
|
|
677
|
+
def backward():_f1(1,_g2,1,_g3)
|
|
678
|
+
def left():_f1(1,_g2,0,_g3)
|
|
679
|
+
def right():_f1(0,_g2,1,_g3)
|
|
680
|
+
def rightArc(radius):A,B=_f4(radius);_f1(0,B,0,A)
|
|
681
|
+
def leftArc(radius):A,B=_f4(radius);_f1(0,A,0,B)
|
|
682
|
+
class Motor:
|
|
683
|
+
def __init__(A,side):A._side=side
|
|
684
|
+
def rotate(B,speed):A=speed;C=int(min(max(abs(A),0),100));D=_f3(C,_g6);E=0 if A>0 else 1;_f2(B._side,E,D)
|
|
685
|
+
def setServo(servo,angle):
|
|
686
|
+
D=angle;A=servo
|
|
687
|
+
if A=='P0'or A=='S1':B=pin0
|
|
688
|
+
elif A=='P1'or A=='S2':B=pin1
|
|
689
|
+
elif A=='P2':B=pin2
|
|
690
|
+
else:raise ValueError("Unknown Servo. Please use 'P0', 'P1' or 'P2'.")
|
|
691
|
+
if D<0 or D>180:raise ValueError('Invalid angle. Must be between 0 and 180')
|
|
692
|
+
C=(_g10-_g9)*int(D);E=(C>>8)+(C>>10)+(C>>11)+(C>>12);F=_g9+E;B.set_analog_period(20);B.write_analog(F)
|
|
693
|
+
def setMinAngleVal(duty):global _g9;_g9=int(duty)
|
|
694
|
+
def setMaxAngleVal(duty):global _g10;_g10=int(duty)
|
|
695
|
+
class IRSensor:
|
|
696
|
+
_g21=bytes(b'\\x1d')
|
|
697
|
+
def __init__(A,index):A._index=index
|
|
698
|
+
def read_digital(A):
|
|
699
|
+
try:i2c.write(16,IRSensor._g21)
|
|
700
|
+
except:raise RuntimeError(_g14)
|
|
701
|
+
B=~i2c.read(16,1)[0];return(B&2**A._index)>>A._index
|
|
702
|
+
def read_analog(A):
|
|
703
|
+
try:i2c.write(16,IRSensor._g21)
|
|
704
|
+
except:raise RuntimeError(_g14)
|
|
705
|
+
B=i2c.read(16,11);return B[1+A._index*2]
|
|
706
|
+
def ir_read_values_as_byte():i2c.write(16,bytearray([29]));A=i2c.read(16,1);return~A[0]
|
|
707
|
+
def getDistance():pin13.write_digital(1);pin13.write_digital(0);A=machine.time_pulse_us(pin14,1,50000);B=(A>>6)+(A>>10)+(A>>11)+(A>>12)+1;return max(min(B,500),0)if B>0 else 255
|
|
708
|
+
def setLED(state,stateR=None):B=state;A=stateR;global _g11;A=A if A!=None else B;_g11[1]=B;_g11[2]=A;i2c.write(16,_g11)
|
|
709
|
+
def setLEDLeft(state):global _g11;_g11[1]=state;i2c.write(16,_g11)
|
|
710
|
+
def setLEDRight(state):global _g11;_g11[2]=state;i2c.write(16,_g11)
|
|
711
|
+
def fillRGB(red,green,blue):
|
|
712
|
+
for A in range(4):_g12[A]=red,green,blue
|
|
713
|
+
_g12.show()
|
|
714
|
+
setRGB=fillRGB
|
|
715
|
+
def clearRGB():_g12.clear()
|
|
716
|
+
def posRGB(position,red,green,blue):
|
|
717
|
+
A=position
|
|
718
|
+
if A<0 or A>3:raise ValueError('invalid RGB-LED position. Must be 0,1,2 or 3.')
|
|
719
|
+
_g12[A]=red,green,blue;_g12.show()
|
|
720
|
+
def setAlarm(state):
|
|
721
|
+
if state:music.play(_g13,wait=False,loop=True)
|
|
722
|
+
else:music.stop()
|
|
723
|
+
def beep():music.pitch(440,200,wait=False)
|
|
724
|
+
class LEDState:ON=1;OFF=0;RED=1
|
|
725
|
+
class IR:R2=0;R1=1;M=2;L1=3;L2=4;masks=[1,2,4,8,16]
|
|
726
|
+
pin2.set_pull(pin2.NO_PULL)
|
|
727
|
+
delay=sleep
|
|
728
|
+
irR2=IRSensor(0)
|
|
729
|
+
irR1=IRSensor(1)
|
|
730
|
+
irRight=irR1
|
|
731
|
+
irM=IRSensor(2)
|
|
732
|
+
irL1=IRSensor(3)
|
|
733
|
+
irLeft=irL1
|
|
734
|
+
irL2=IRSensor(4)
|
|
735
|
+
motL=Motor(0)
|
|
736
|
+
motR=Motor(2)`
|
|
737
|
+
};
|
|
738
|
+
|
|
739
|
+
// microbit/libraries.raw.json
|
|
740
|
+
var libraries_raw_default2 = {
|
|
741
|
+
huskylens: `# For Huskylens (PRO inclusive) with Firmware version 0.5.1Norm or 0.5.3Alpha1.
|
|
742
|
+
from microbit import i2c, sleep, running_time
|
|
743
|
+
import math
|
|
744
|
+
|
|
745
|
+
_algorithm_names = ["FaceRecognition",
|
|
746
|
+
"ObjectTracking",
|
|
747
|
+
"ObjectRecognition",
|
|
748
|
+
"LineTracking",
|
|
749
|
+
"ColorRecognition",
|
|
750
|
+
"TagRecognition",
|
|
751
|
+
"ObjectClassification",
|
|
752
|
+
"QRRecognition",
|
|
753
|
+
"BarcodeRecognition"]
|
|
754
|
+
|
|
755
|
+
class Request_Command:
|
|
756
|
+
# """Command Codes that can be sent to the Huskylens.""""
|
|
757
|
+
KNOCK = 0x2C
|
|
758
|
+
ALGORITHM = 0x2D
|
|
759
|
+
ALL = 0x20
|
|
760
|
+
BLOCKS = 0x21
|
|
761
|
+
BLOCKS_LEARNED = 0x24
|
|
762
|
+
BLOCKS_OF_ID = 0x27
|
|
763
|
+
ARROWS = 0x22
|
|
764
|
+
ARROWS_LEARNED = 0x25
|
|
765
|
+
ARROWS_OF_ID = 0x28
|
|
766
|
+
LEARNED = 0x23
|
|
767
|
+
ALL_OF_ID = 0x26
|
|
768
|
+
LEARN = 0x36
|
|
769
|
+
FORGET = 0x37
|
|
770
|
+
CUSTOM_LABEL = 0x2F
|
|
771
|
+
CUSTOM_TEXT = 0x34
|
|
772
|
+
CLEAR_TEXT = 0x35
|
|
773
|
+
SAVE_MODEL = 0x32
|
|
774
|
+
LOAD_MODEL = 0x33
|
|
775
|
+
SAVE_PHOTO = 0x30
|
|
776
|
+
SAVE_SCREENSHOT = 0x39
|
|
777
|
+
IS_PRO = 0x3B
|
|
778
|
+
VERSION = 0x3C
|
|
779
|
+
|
|
780
|
+
class Return_Code:
|
|
781
|
+
# """Return codes that identify answer types, received from the Huskylens."""
|
|
782
|
+
ANY = 0x01 # custom command for "don't care", never returned by Huskylens
|
|
783
|
+
OK = 0x2E
|
|
784
|
+
BUSY = 0x3D
|
|
785
|
+
INFO = 0x29
|
|
786
|
+
BLOCK = 0x2A
|
|
787
|
+
ARROW = 0x2B
|
|
788
|
+
IS_PRO = 0x3B
|
|
789
|
+
NEED_PRO = 0x3E
|
|
790
|
+
|
|
791
|
+
class Algorithm:
|
|
792
|
+
FACE_RECOGNITION = 0
|
|
793
|
+
OBJECT_TRACKING = 1
|
|
794
|
+
OBJECT_RECOGNITION = 2
|
|
795
|
+
LINE_TRACKING = 3
|
|
796
|
+
COLOR_RECOGNITION = 4
|
|
797
|
+
TAG_RECOGNITION = 5
|
|
798
|
+
OBJECT_CLASSIFICATION = 6
|
|
799
|
+
QR_RECOGNITION = 7
|
|
800
|
+
BARCODE_RECOGNITION = 8
|
|
801
|
+
|
|
802
|
+
class Block:
|
|
803
|
+
# """Create a new Block with (x,y) as its center and (width,height) as its extent. Id is learned id on Huskylens.
|
|
804
|
+
# Blocks are returned by all Algorithms except for Line_Recognition. """
|
|
805
|
+
def __init__(self, x, y, width, height, id):
|
|
806
|
+
self.x = x
|
|
807
|
+
self.y = y
|
|
808
|
+
self.width = width
|
|
809
|
+
self.height = height
|
|
810
|
+
self.id = id
|
|
811
|
+
|
|
812
|
+
def __str__(self):
|
|
813
|
+
return "Block: ID_" + str(self.id) + " Pos: (" + str(self.x) + " " + str(self.y) + ") Size: (" + str(self.width) + " " + str(self.height) + ")"
|
|
814
|
+
|
|
815
|
+
class Arrow:
|
|
816
|
+
# """Create a new Arrow from (x,y)_tail to (x,y)_head. Id is learned id on Huskylens.
|
|
817
|
+
# Arrows are returned only for the Line Recognition Algorithm. """
|
|
818
|
+
def __init__(self, x_tail, y_tail, x_head, y_head, id):
|
|
819
|
+
self.x_tail = x_tail
|
|
820
|
+
self.y_tail = y_tail
|
|
821
|
+
self.x_head = x_head
|
|
822
|
+
self.y_head = y_head
|
|
823
|
+
self.id = id
|
|
824
|
+
|
|
825
|
+
def get_direction(self):
|
|
826
|
+
dx = self.x_head - self.x_tail
|
|
827
|
+
dy = self.y_head - self.y_tail
|
|
828
|
+
deg = 90 - math.degrees(math.atan2(dy, dx))
|
|
829
|
+
if deg < 0: deg = deg + 360
|
|
830
|
+
return int(deg)
|
|
831
|
+
|
|
832
|
+
def __str__(self):
|
|
833
|
+
return "Arrow: ID_" + str(self.id) + " (" + str(self.x_tail) + " " + str(self.y_tail) + ")->(" + str(self.x_head) + " " + str(self.y_head) + ")"
|
|
834
|
+
|
|
835
|
+
def byte_checksum(byte_list):
|
|
836
|
+
# """Computes the checksum and returns the low byte of the sum."""
|
|
837
|
+
return sum(byte_list) & 0xFF
|
|
838
|
+
|
|
839
|
+
def hexify(byte_array):
|
|
840
|
+
# """Takes arraylike of bytes and converts it to hex string for pretty-printing."""
|
|
841
|
+
if len(byte_array) == 0: return ""
|
|
842
|
+
return "0x" + "".join("{:02x}".format(i) for i in byte_array)
|
|
843
|
+
|
|
844
|
+
class Huskylens:
|
|
845
|
+
|
|
846
|
+
I2C_ADDR = 0x32
|
|
847
|
+
|
|
848
|
+
def __init__(self):
|
|
849
|
+
# """Create a new Huskylens instance.
|
|
850
|
+
#
|
|
851
|
+
# A Huskylens instance is necessary for further communication.
|
|
852
|
+
#
|
|
853
|
+
# The Huskylens has its own internal state that can't be known by this instance if
|
|
854
|
+
# a Huskylens is operated manually or has data stored in advance (names, learned id's).
|
|
855
|
+
# """
|
|
856
|
+
self.learned_slot_count = 0
|
|
857
|
+
self.id_slots = {} # id to learning-slots.
|
|
858
|
+
self.id_names = {} # id to string
|
|
859
|
+
self.algorithm = Algorithm.OBJECT_TRACKING
|
|
860
|
+
self.clear_texts()
|
|
861
|
+
self.pro_enabled = self.is_pro()
|
|
862
|
+
|
|
863
|
+
def initialize(self):
|
|
864
|
+
# """Establishes Connection to Huskylens. Then clears custom Texts and activates Algorithm Object Tracking.
|
|
865
|
+
|
|
866
|
+
# First, knocks at most 5 times to setup connection.
|
|
867
|
+
# If successful, clears custom text and changes Algorithm to OBJECT_TRACKING.
|
|
868
|
+
# Learned ids and labels are kept from previous uses if available.
|
|
869
|
+
#
|
|
870
|
+
# Returns:
|
|
871
|
+
# bool: Wheter initialization was successfull.
|
|
872
|
+
# """
|
|
873
|
+
success = False
|
|
874
|
+
for i in range(5):
|
|
875
|
+
self.knock()
|
|
876
|
+
success, _ = self.get_response(Return_Code.OK)
|
|
877
|
+
if success:
|
|
878
|
+
break
|
|
879
|
+
|
|
880
|
+
if success > 0:
|
|
881
|
+
s = self.clear_texts()
|
|
882
|
+
s2 = self.set_algorithm(Algorithm.OBJECT_TRACKING)
|
|
883
|
+
if s and s2:
|
|
884
|
+
print("Initialization successful!")
|
|
885
|
+
return True
|
|
886
|
+
else:
|
|
887
|
+
print("Initialization Failed. Couldn't change Algorithm")
|
|
888
|
+
return False
|
|
889
|
+
else:
|
|
890
|
+
print("Initialization Failed. Please check connection to Huskylens.")
|
|
891
|
+
return False
|
|
892
|
+
|
|
893
|
+
# Low Level communication commands (deal with byte-data yourself)
|
|
894
|
+
|
|
895
|
+
def send_request(self, command, data=None):
|
|
896
|
+
# """Request an action from the Huskylens through a command code and optional Data Bytes.
|
|
897
|
+
#
|
|
898
|
+
# Parameters:
|
|
899
|
+
# command (byte): a Request_Command byte to be sent.
|
|
900
|
+
# data (list of bytes | None): Optional data to attach to the request or None.
|
|
901
|
+
# """
|
|
902
|
+
buffer = bytearray(b'\\x55\\xAA\\x11\\x00\\x00')
|
|
903
|
+
buffer[3] = 0 if data is None else len(data)
|
|
904
|
+
buffer[4] = command
|
|
905
|
+
if data:
|
|
906
|
+
for b in data:
|
|
907
|
+
buffer.append(b)
|
|
908
|
+
buffer.append(byte_checksum(buffer))
|
|
909
|
+
# print("request:", hexify(buffer))
|
|
910
|
+
i2c.write(Huskylens.I2C_ADDR, buffer)
|
|
911
|
+
sleep(50)
|
|
912
|
+
|
|
913
|
+
def get_response(self, return_code=Return_Code.ANY, timeout=500):
|
|
914
|
+
# """Read a response from I2C. Returns after a timeout when no data available.
|
|
915
|
+
# If a specific return code is expected and given as a parameter, all other results will set the returned code to 0.
|
|
916
|
+
# If a timeout occured, the return code is set to -1. Other errors return -2.
|
|
917
|
+
# This way, the return code is positive upon success, and negative or 0 otherwise.
|
|
918
|
+
#
|
|
919
|
+
# Parameters:
|
|
920
|
+
# return_code (byte): Optional expected return code.
|
|
921
|
+
# timeout (int): milliseconds to wait for response before giving up
|
|
922
|
+
#
|
|
923
|
+
# Returns:
|
|
924
|
+
# (code | error, data list): A tuple containing the return code or the error code as a signed byte in the first part
|
|
925
|
+
# The data as a list of bytes in the second argument or an empty list if nothing was sent along.
|
|
926
|
+
# """
|
|
927
|
+
response_header = bytearray(b"\\x55\\0\\0\\0\\0")
|
|
928
|
+
start_time = running_time()
|
|
929
|
+
|
|
930
|
+
# bytewise polling, as there seem to be unpredictable 0 bytes between messages!
|
|
931
|
+
#response_header = i2c.read(Huskylens.I2C_ADDR, 5, True)
|
|
932
|
+
while running_time() - start_time < timeout:
|
|
933
|
+
byte = i2c.read(Huskylens.I2C_ADDR, 1)[0]
|
|
934
|
+
if byte == 0x55:
|
|
935
|
+
break
|
|
936
|
+
if byte != 0x55:
|
|
937
|
+
return -1, [] # Timeout error
|
|
938
|
+
|
|
939
|
+
# reading rest of header data
|
|
940
|
+
for i in range(4):
|
|
941
|
+
response_header[i+1] = i2c.read(Huskylens.I2C_ADDR, 1)[0]
|
|
942
|
+
|
|
943
|
+
if response_header[0:3] != b'\\x55\\xAA\\x11':
|
|
944
|
+
return -2, [] # Wrong header structure error
|
|
945
|
+
|
|
946
|
+
data_length = response_header[3]
|
|
947
|
+
response_type = response_header[4]
|
|
948
|
+
|
|
949
|
+
data = []
|
|
950
|
+
if data_length > 0:
|
|
951
|
+
response_body = i2c.read(Huskylens.I2C_ADDR, data_length+1)
|
|
952
|
+
data = response_body[0:-1]
|
|
953
|
+
response_checksum = response_body[-1]
|
|
954
|
+
else:
|
|
955
|
+
response_checksum = ord(i2c.read(Huskylens.I2C_ADDR, 1))
|
|
956
|
+
if response_checksum != byte_checksum(list(response_header) + data):
|
|
957
|
+
return -3, [] # Checksum Error
|
|
958
|
+
|
|
959
|
+
# print("response: " + hexify([response_type]) + " " + hexify(data))
|
|
960
|
+
if return_code == Return_Code.ANY or response_type == return_code:
|
|
961
|
+
return response_type, data
|
|
962
|
+
else:
|
|
963
|
+
return 0, data # not expected answer Hint
|
|
964
|
+
|
|
965
|
+
def knock(self):
|
|
966
|
+
self.send_request(Request_Command.KNOCK)
|
|
967
|
+
|
|
968
|
+
# High level commands for students
|
|
969
|
+
|
|
970
|
+
def set_algorithm(self, algorithm):
|
|
971
|
+
# """ Change the active algorithm on the Huskylens. Returns True on success."""
|
|
972
|
+
if (algorithm == Algorithm.QR_RECOGNITION or algorithm == Algorithm.BARCODE_RECOGNITION) and (not self.pro_enabled):
|
|
973
|
+
raise RuntimeError("Error: Huskylens PRO version is required for algorithm ", _algorithm_names[algorithm])
|
|
974
|
+
return False
|
|
975
|
+
data = [algorithm, 0x00]
|
|
976
|
+
self.send_request(Request_Command.ALGORITHM, data)
|
|
977
|
+
success, _ = self.get_response(Return_Code.OK)
|
|
978
|
+
if success > 0:
|
|
979
|
+
print("Current Algorithm:", _algorithm_names[algorithm])
|
|
980
|
+
self.algorithm = algorithm
|
|
981
|
+
return True if success > 0 else False
|
|
982
|
+
|
|
983
|
+
def get_all(self):
|
|
984
|
+
# """Get All detected objects, Blocks or Arrows as a list."""
|
|
985
|
+
return self._get_results(Request_Command.ALL)
|
|
986
|
+
|
|
987
|
+
def get_all_learned(self):
|
|
988
|
+
# """Get all detected objects that are learned as a list."""
|
|
989
|
+
return self._get_results(Request_Command.LEARNED)
|
|
990
|
+
|
|
991
|
+
def get_all_with_id(self, id):
|
|
992
|
+
# """Get all detected objects with specific id as a list."""
|
|
993
|
+
if id <= 0 or id > 255:
|
|
994
|
+
raise RuntimeError("Error: ID must be in range from 1 to 255.")
|
|
995
|
+
return self._get_results(Request_Command.ALL_OF_ID, id)
|
|
996
|
+
|
|
997
|
+
def get_one(self):
|
|
998
|
+
# """Get one instance of centermost detected object (Block or Arrow) else None."""
|
|
999
|
+
results = self._get_results(Request_Command.ALL)
|
|
1000
|
+
return self._get_centermost(results)
|
|
1001
|
+
|
|
1002
|
+
def get_one_learned(self):
|
|
1003
|
+
# """Get one instance of centermost detected object that has an id > 0. else None."""
|
|
1004
|
+
results = self._get_results(Request_Command.LEARNED)
|
|
1005
|
+
return self._get_centermost(results)
|
|
1006
|
+
|
|
1007
|
+
def get_one_with_id(self, id):
|
|
1008
|
+
# """Get one instance of centermost detected object that has given id. else None."""
|
|
1009
|
+
if id <= 0 or id > 255:
|
|
1010
|
+
raise RuntimeError("Error: ID must be in range from 1 to 255.")
|
|
1011
|
+
results = self._get_results(Request_Command.ALL_OF_ID, id)
|
|
1012
|
+
return self._get_centermost(results)
|
|
1013
|
+
|
|
1014
|
+
def attach_label(self, id, name):
|
|
1015
|
+
# """Attach the label "name" to a learned id of current Algorithm. Returns True on success."""
|
|
1016
|
+
if self.algorithm == Algorithm.OBJECT_TRACKING or \\
|
|
1017
|
+
self.algorithm == Algorithm.LINE_TRACKING: # single learn algorithms
|
|
1018
|
+
success = self._set_name(1, name)
|
|
1019
|
+
elif self.algorithm == Algorithm.FACE_RECOGNITION or \\
|
|
1020
|
+
self.algorithm == Algorithm.TAG_RECOGNITION or \\
|
|
1021
|
+
self.algorithm == Algorithm.OBJECT_CLASSIFICATION or \\
|
|
1022
|
+
self.algorithm == Algorithm.OBJECT_RECOGNITION: # multi learn algorithms
|
|
1023
|
+
# avoid naming unlearned id's to avert bugs.
|
|
1024
|
+
slots = self.id_slots.get(id)
|
|
1025
|
+
if slots == None:
|
|
1026
|
+
raise RuntimeError("Can't attach a name to an unlearned ID number")
|
|
1027
|
+
|
|
1028
|
+
self.id_names[id] = name
|
|
1029
|
+
|
|
1030
|
+
success = True
|
|
1031
|
+
for slot in slots:
|
|
1032
|
+
s = self._set_name(slot, name)
|
|
1033
|
+
success = success and s > 0
|
|
1034
|
+
else: # For color recognition (where it works properly!)
|
|
1035
|
+
self.id_names[id] = name
|
|
1036
|
+
success = self._set_name(id, name)
|
|
1037
|
+
return True if success > 0 else False
|
|
1038
|
+
|
|
1039
|
+
def clear_labels(self):
|
|
1040
|
+
# """Deletes all learned label names on Huskylens for the current algorithm."""
|
|
1041
|
+
self.id_names.clear()
|
|
1042
|
+
for i in range(10):
|
|
1043
|
+
self._set_name(i, "")
|
|
1044
|
+
|
|
1045
|
+
def add_text(self, text, position_x, position_y):
|
|
1046
|
+
# """Add a custom text to the Huskylens screen at a certain screen pixel-position (top left starting point).
|
|
1047
|
+
# Text must be less than 20 bytes long and within pixel-borders.
|
|
1048
|
+
# Multiple texts at the same location get overwritten. Returns True on success."""
|
|
1049
|
+
text_bytes = bytes(text, "utf-8")
|
|
1050
|
+
if len(text_bytes) > 19:
|
|
1051
|
+
raise RuntimeError("Custom Text must be less than 20 bytes long.")
|
|
1052
|
+
if position_x > 300 or position_x < 0 or position_y < 35 or position_y > 240:
|
|
1053
|
+
raise RuntimeError("Custom Text can't be placed outside of screen pixel size.")
|
|
1054
|
+
data = [len(text_bytes)]
|
|
1055
|
+
data.append(0xFF if position_x > 255 else 0x00)
|
|
1056
|
+
data.append(position_x % 255)
|
|
1057
|
+
data.append(240 - position_y) # reverse flipped y axis
|
|
1058
|
+
data.extend(list(text_bytes))
|
|
1059
|
+
self.send_request(Request_Command.CUSTOM_TEXT, data)
|
|
1060
|
+
success, _ = self.get_response(Return_Code.OK)
|
|
1061
|
+
return True if success > 0 else False
|
|
1062
|
+
|
|
1063
|
+
def clear_texts(self):
|
|
1064
|
+
# """Deletes all text on the Huskylens screen."""
|
|
1065
|
+
self.send_request(Request_Command.CLEAR_TEXT)
|
|
1066
|
+
success, _ = self.get_response(Return_Code.OK)
|
|
1067
|
+
return True if success > 0 else False
|
|
1068
|
+
|
|
1069
|
+
def learn(self, id, name=None):
|
|
1070
|
+
# """Learn and assign an id to the object currently centered on the huskylens camera.
|
|
1071
|
+
# Optionally attach label to this learned id. Returns True on success."""
|
|
1072
|
+
if id <= 0 or id > 255:
|
|
1073
|
+
raise RuntimeError("Parameter ID for learned item must be in range [0,255]")
|
|
1074
|
+
if self.algorithm == Algorithm.OBJECT_TRACKING or self.algorithm == Algorithm.LINE_TRACKING:
|
|
1075
|
+
id = 1
|
|
1076
|
+
timeout = 500
|
|
1077
|
+
if self.algorithm == Algorithm.OBJECT_CLASSIFICATION:
|
|
1078
|
+
timeout = 1000
|
|
1079
|
+
self.send_request(Request_Command.LEARN, [id, 0x00])
|
|
1080
|
+
success, _ = self.get_response(Return_Code.OK, timeout)
|
|
1081
|
+
if success > 0:
|
|
1082
|
+
# 1. remember slot for id
|
|
1083
|
+
self.learned_slot_count += 1
|
|
1084
|
+
if (not (id in self.id_slots)):
|
|
1085
|
+
self.id_slots[id] = [self.learned_slot_count]
|
|
1086
|
+
else:
|
|
1087
|
+
self.id_slots[id].append(self.learned_slot_count)
|
|
1088
|
+
# 2. attach, name if available.
|
|
1089
|
+
known_by = self.id_names.get(id)
|
|
1090
|
+
if known_by != None:
|
|
1091
|
+
success = self.attach_label(self.learned_slot_count, known_by)
|
|
1092
|
+
elif name != None:
|
|
1093
|
+
success = self.attach_label(id, name)
|
|
1094
|
+
return True if success > 0 else False
|
|
1095
|
+
|
|
1096
|
+
def forget(self):
|
|
1097
|
+
# """Forget all learned objects (ids) of the current algorithm.
|
|
1098
|
+
# Labels are unaffected. Returns True on success."""
|
|
1099
|
+
self.send_request(Request_Command.FORGET)
|
|
1100
|
+
success, _ = self.get_response(Return_Code.OK)
|
|
1101
|
+
if success:
|
|
1102
|
+
self.learned_slot_count = 0
|
|
1103
|
+
self.id_slots.clear()
|
|
1104
|
+
return True if success else False
|
|
1105
|
+
|
|
1106
|
+
|
|
1107
|
+
def save_photo(self):
|
|
1108
|
+
# """Save a photo to the SD-Card. No Feedback, fails on Huskylens screen if no SD-Card is available."""
|
|
1109
|
+
self.send_request(Request_Command.SAVE_PHOTO)
|
|
1110
|
+
success, _ = self.get_response(Return_Code.OK, 1000)
|
|
1111
|
+
return True if success > 0 else False
|
|
1112
|
+
|
|
1113
|
+
def save_screenshot(self):
|
|
1114
|
+
# """Save a screenshot (including texts) to the SD-Card. No Feedback, fails on Huskylens screen if no SD-Card is available."""
|
|
1115
|
+
self.send_request(Request_Command.SAVE_SCREENSHOT)
|
|
1116
|
+
success, _ = self.get_response(Return_Code.OK, 1000)
|
|
1117
|
+
return True if success > 0 else False
|
|
1118
|
+
|
|
1119
|
+
def save_model(self, model_id):
|
|
1120
|
+
# """Save the learned ids and labels to the SD-Card. No Feedback, see Huskylens screen for Result.
|
|
1121
|
+
# There can be at most 5 models per Algorithm, indexed by model_id."""
|
|
1122
|
+
if model_id < 0 or model_id > 4:
|
|
1123
|
+
raise RuntimeError("Invalid model_id. Must be number in range [0,4]")
|
|
1124
|
+
self.send_request(Request_Command.SAVE_MODEL, [model_id, 0x00])
|
|
1125
|
+
success, _ = self.get_response(Return_Code.OK, 1000)
|
|
1126
|
+
print("Model saving: Check Huskylens screen for Result!\\n\\tModel name:", _algorithm_names[self.algorithm] + "_Backup_" + str(model_id) + ".conf")
|
|
1127
|
+
return True if success > 0 else False
|
|
1128
|
+
|
|
1129
|
+
def load_model(self, model_id):
|
|
1130
|
+
# """Load a previously saved model for the active Algorithm from the SD-Card. No Feedback, see Huskylens screen for Result."""
|
|
1131
|
+
if model_id < 0 or model_id > 4:
|
|
1132
|
+
raise RuntimeError("Invalid model_id. Must be number in range [0,4]")
|
|
1133
|
+
self.send_request(Request_Command.LOAD_MODEL, [model_id, 0x00])
|
|
1134
|
+
success, _ = self.get_response(Return_Code.OK, 1000)
|
|
1135
|
+
print("Model Loading: Check Huskylens screen for Result!")
|
|
1136
|
+
return True if success > 0 else False
|
|
1137
|
+
|
|
1138
|
+
def is_pro(self):
|
|
1139
|
+
# """Checks wheter the Huskylens is the PRO version, which supports QR_RECOGNITION and BARCODE_RECOGNITION.
|
|
1140
|
+
# Returns True/False on success, else 0."""
|
|
1141
|
+
self.send_request(Request_Command.IS_PRO)
|
|
1142
|
+
success, data = self.get_response(Return_Code.IS_PRO)
|
|
1143
|
+
return bool(data[0]) if success > 0 else False
|
|
1144
|
+
|
|
1145
|
+
# hidden Utility functions
|
|
1146
|
+
|
|
1147
|
+
def _set_name(self, id, name):
|
|
1148
|
+
# """Set the name of block "id" to the given "name". Name must be less than 20 characters long."""
|
|
1149
|
+
name_bytes = bytes(name, "utf-8")
|
|
1150
|
+
if len(name_bytes) > 19: raise RuntimeError("Custom Name must be less than 20 bytes long.")
|
|
1151
|
+
data = [id, len(name_bytes)+1]
|
|
1152
|
+
data.extend(list(name_bytes))
|
|
1153
|
+
data.append(0x00)
|
|
1154
|
+
self.send_request(Request_Command.CUSTOM_LABEL, data)
|
|
1155
|
+
success, _ = self.get_response(Return_Code.OK)
|
|
1156
|
+
return True if success > 0 else False
|
|
1157
|
+
|
|
1158
|
+
def _get_results(self, request_command, id=-1):
|
|
1159
|
+
# """Get all detected Blocks or Arrows from the Huskylens, specified by the request and id.
|
|
1160
|
+
# Possible Requests: BLOCKS, BLOCKS_LEARNED, BLOCKS_OF_ID, ARROWS, ...
|
|
1161
|
+
# Returns a list of all the detected Block or Arrow instances.
|
|
1162
|
+
# """
|
|
1163
|
+
request_data = None if id < 0 else [id, 0]
|
|
1164
|
+
self.send_request(request_command, request_data)
|
|
1165
|
+
|
|
1166
|
+
# 1. get info header
|
|
1167
|
+
success, info = self.get_response(Return_Code.INFO)
|
|
1168
|
+
if not success:
|
|
1169
|
+
raise RuntimeError("Failed to request results. Got answer:" + str(success))
|
|
1170
|
+
|
|
1171
|
+
# Ignoring number of detected id's and current frame number.
|
|
1172
|
+
n_elements = info[0] + info[1]*255
|
|
1173
|
+
#n_ids = info[2] + info[3]*255
|
|
1174
|
+
# frame = info[4] + info[5]*255
|
|
1175
|
+
#print("result info:", n_elements, n_ids, frame)
|
|
1176
|
+
|
|
1177
|
+
# 2. receive data
|
|
1178
|
+
el = 0
|
|
1179
|
+
objects = []
|
|
1180
|
+
|
|
1181
|
+
while el < n_elements:
|
|
1182
|
+
response_type, el_data = self.get_response(Return_Code.ANY)
|
|
1183
|
+
if response_type == Return_Code.BLOCK and self.algorithm != 3:
|
|
1184
|
+
x = el_data[0] + el_data[1]*255
|
|
1185
|
+
y = 240 - el_data[2] + el_data[3]*255 # flipped y axis
|
|
1186
|
+
width = el_data[4] + el_data[5]*255
|
|
1187
|
+
height = el_data[6] + el_data[7]*255
|
|
1188
|
+
el_id = el_data[8]
|
|
1189
|
+
block = Block(x, y, width, height, el_id)
|
|
1190
|
+
objects.append(block)
|
|
1191
|
+
el += 1
|
|
1192
|
+
|
|
1193
|
+
elif response_type == Return_Code.ARROW:
|
|
1194
|
+
xtail = el_data[0] + el_data[1]*255
|
|
1195
|
+
ytail = 240 - el_data[2] + el_data[3]*255 # flipped y axis
|
|
1196
|
+
xhead = el_data[4] + el_data[5]*255
|
|
1197
|
+
yhead = 240 - el_data[6] + el_data[7]*255 # flipped y axis
|
|
1198
|
+
el_id = el_data[8]
|
|
1199
|
+
arrow = Arrow(xtail,ytail,xhead,yhead,el_id)
|
|
1200
|
+
objects.append(arrow)
|
|
1201
|
+
el += 1
|
|
1202
|
+
elif response_type == 0:
|
|
1203
|
+
return [] # Error (couldn't read all detected elements) fail silently.
|
|
1204
|
+
return objects
|
|
1205
|
+
|
|
1206
|
+
def _get_centermost(self, results):
|
|
1207
|
+
# """Select and Return the most centered (L1-distance) instance of all detected Objects in "results". """
|
|
1208
|
+
centermost = None
|
|
1209
|
+
max_offset = 320+120
|
|
1210
|
+
for obj in results:
|
|
1211
|
+
center_offset = 0
|
|
1212
|
+
if self.algorithm == Algorithm.LINE_TRACKING:
|
|
1213
|
+
center_offset = abs((obj.x_tail + (obj.x_tail - obj.x_head) // 2) - 160) \\
|
|
1214
|
+
+ abs((obj.y_tail + (obj.y_tail - obj.y_head) // 2) - 120)
|
|
1215
|
+
else:
|
|
1216
|
+
center_offset = abs(obj.x - 160) + abs(obj.y - 120)
|
|
1217
|
+
if center_offset < max_offset:
|
|
1218
|
+
centermost, max_offset = obj, center_offset
|
|
1219
|
+
return centermost
|
|
1220
|
+
`,
|
|
1221
|
+
mbbitbot: "from microbit import *\nfrom neopixel import *\nfrom utime import ticks_us, sleep_us\n\n\n_v = 200\n_nbLeds = 12\n_np = NeoPixel(pin13, _nbLeds)\n\n_powerDifferential = 0\n_powerOffset = 0\n\n\ndef w(leftforward, leftbackward, rightforward, rightbackward):\n if leftforward > 0 or rightforward > 0:\n pin16.write_analog(leftforward + _powerDifferential)\n pin8.write_analog(leftbackward)\n pin14.write_analog(rightforward - _powerDifferential)\n pin12.write_analog(rightbackward)\n elif leftbackward > 0 or rightbackward > 0:\n pin16.write_analog(leftforward)\n pin8.write_analog(leftbackward + _powerDifferential)\n pin14.write_analog(rightforward)\n pin12.write_analog(rightbackward - _powerDifferential)\n elif leftforward == 0 and rightforward == 0 and leftbackward == 0 and rightbackward == 0:\n pin16.write_analog(0)\n pin8.write_analog(0)\n pin14.write_analog(0)\n pin12.write_analog(0)\n\n\ndef forward():\n w(_v, 0, _v, 0)\n\n\ndef backward():\n w(0, _v, 0, _v)\n\n\ndef stop():\n w(0, 0, 0, 0)\n\n\ndef right():\n w(0, _v, _v, 0)\n\n\ndef left():\n w(_v, 0, 0, _v)\n\n\ndef set_led(pos, red, green, blue):\n _np[pos] = (red, green, blue)\n _np.show()\n\n\ndef fill(red, green, blue):\n for i in range(_nbLeds):\n _np[i] = (red, green, blue)\n _np.show()\n\n\ndef getDistance():\n pin15.write_digital(1)\n sleep_us(10)\n pin15.write_digital(0)\n pin15.set_pull(pin15.NO_PULL)\n while pin15.read_digital() == 0:\n pass\n start = ticks_us()\n while pin15.read_digital() == 1:\n pass\n end = ticks_us()\n echo = end-start\n distance = int(0.01715 * echo)\n return distance\n\n\ndef calibrate(offset, differential=0):\n global _powerDifferential\n global _powerOffset\n global _arcScaling\n\n _powerOffset = max(min(int(offset), 50), -50)\n _powerDifferential = max(min(int(differential), 50), -50)\n _arcScaling = max(min(arcScaling, 50), -50)\n setSpeed(_v / 1023 * 100)\n\n\ndef setSpeed(percent):\n global _v\n speed = percent / 100 * 1023\n speed += _powerOffset/100 * 1023\n speed = min(max(speed, 0), 1023)\n _v = speed\n\n\ndef getLine(bit):\n mask = 1 << bit\n value = 0\n try:\n value = i2c.read(0x1c, 1)[0]\n except OSError:\n pass\n if (value & mask) > 0:\n return 1\n else:\n return 0\n\n\ndef getLight(index):\n if index == 0:\n return pin1.read_analog()\n elif index == 1:\n return pin2.read_analog()\n\n\ndef leftArc(radius):\n speed = _v\n inner_wheel_speed = speed * (radius / (radius + 1))\n w(inner_wheel_speed, 0, speed, 0)\n\n\ndef rightArc(radius):\n speed = _v\n inner_wheel_speed = speed * (radius / (radius + 1))\n w(speed, 0, inner_wheel_speed, 0)\n",
|
|
1222
|
+
mbwait: `from microbit import button_a, button_b, pin0, pin1, pin2, pin_logo, sleep
|
|
1223
|
+
|
|
1224
|
+
# choose the reactivity of the press/touch sensing in ms. Default 100ms is 10 checks per second.
|
|
1225
|
+
POLLING_DELAY = 100
|
|
1226
|
+
|
|
1227
|
+
# Let the microbit wait until a certain button of choice was pressed (and released).
|
|
1228
|
+
|
|
1229
|
+
|
|
1230
|
+
def wait_for_press(button='any'):
|
|
1231
|
+
button_a.was_pressed()
|
|
1232
|
+
button_b.was_pressed()
|
|
1233
|
+
# busy wait until a chosen button was pressed.
|
|
1234
|
+
if button == 'a' or button == 'A':
|
|
1235
|
+
while not button_a.was_pressed():
|
|
1236
|
+
sleep(POLLING_DELAY)
|
|
1237
|
+
elif button == 'b' or button == 'B':
|
|
1238
|
+
while not button_b.was_pressed():
|
|
1239
|
+
sleep(POLLING_DELAY)
|
|
1240
|
+
elif button == 'any' or button == 'ANY':
|
|
1241
|
+
while (not button_a.was_pressed()) and (not button_b.was_pressed()):
|
|
1242
|
+
sleep(POLLING_DELAY)
|
|
1243
|
+
elif button == 'both' or button == 'ab' or button == 'AB':
|
|
1244
|
+
while not (button_a.is_pressed() and button_b.is_pressed()):
|
|
1245
|
+
sleep(POLLING_DELAY)
|
|
1246
|
+
while button_a.is_pressed() or button_b.is_pressed():
|
|
1247
|
+
sleep(POLLING_DELAY)
|
|
1248
|
+
else:
|
|
1249
|
+
raise RuntimeError(
|
|
1250
|
+
"Button to wait for must be either: 'a', 'b', 'any' or 'both'.")
|
|
1251
|
+
|
|
1252
|
+
# Let the microbit wait until a certain touch-sensitive pin of choice is touched.
|
|
1253
|
+
|
|
1254
|
+
|
|
1255
|
+
def wait_for_touch(pin='logo'):
|
|
1256
|
+
# busy wait until a given pin/logo is pressed.
|
|
1257
|
+
p = pin_logo
|
|
1258
|
+
if pin == 'pin0':
|
|
1259
|
+
p = pin0
|
|
1260
|
+
elif pin == 'pin1':
|
|
1261
|
+
p = pin1
|
|
1262
|
+
elif pin == 'pin2':
|
|
1263
|
+
p = pin2
|
|
1264
|
+
elif pin == 'logo' or pin == 'pin_logo' or pin == 'pinLogo':
|
|
1265
|
+
p = pin_logo
|
|
1266
|
+
else:
|
|
1267
|
+
raise RuntimeError(
|
|
1268
|
+
"Argument 'pin' must be one of: 'pin_logo', 'pin0', 'pin1' or 'pin2'.")
|
|
1269
|
+
p.set_touch_mode(p.CAPACITIVE)
|
|
1270
|
+
while not p.is_touched():
|
|
1271
|
+
sleep(POLLING_DELAY)
|
|
1272
|
+
`,
|
|
1273
|
+
mbled: `from neopixel import *
|
|
1274
|
+
import gc
|
|
1275
|
+
from microbit import pin2
|
|
1276
|
+
|
|
1277
|
+
|
|
1278
|
+
_nbLeds = 24
|
|
1279
|
+
_np = NeoPixel(pin2, _nbLeds)
|
|
1280
|
+
|
|
1281
|
+
# Fill all pixels with the same color.
|
|
1282
|
+
# The color is specified by giving values between 0 and 255 for red, green and blue
|
|
1283
|
+
|
|
1284
|
+
|
|
1285
|
+
def fill(red, green, blue):
|
|
1286
|
+
for i in range(_nbLeds):
|
|
1287
|
+
_np[i] = (red, green, blue)
|
|
1288
|
+
_np.show()
|
|
1289
|
+
|
|
1290
|
+
# Set the specified pixel to the specified color.
|
|
1291
|
+
# The pixel is specified by a number between 0 and 23.
|
|
1292
|
+
# The color is specified by giving values between 0 and 255 for red, green and blue
|
|
1293
|
+
|
|
1294
|
+
|
|
1295
|
+
def set_led(pos, red, green, blue):
|
|
1296
|
+
_np[pos] = (red, green, blue)
|
|
1297
|
+
_np.show()
|
|
1298
|
+
|
|
1299
|
+
# clears all LEDS by turning them off
|
|
1300
|
+
|
|
1301
|
+
|
|
1302
|
+
def clear():
|
|
1303
|
+
_np.clear()
|
|
1304
|
+
|
|
1305
|
+
# shift the displayed image by as many led-positions as specified in parameter "amount".
|
|
1306
|
+
# LED strip behaves like a cycle and wraps around when shifting across borders.
|
|
1307
|
+
|
|
1308
|
+
|
|
1309
|
+
def shift_by(amount):
|
|
1310
|
+
shifted_copy = [None]*_nbLeds
|
|
1311
|
+
for n in range(0, _nbLeds):
|
|
1312
|
+
next_i = (n + amount) % _nbLeds
|
|
1313
|
+
shifted_copy[n] = _np[next_i]
|
|
1314
|
+
for n in range(0, _nbLeds):
|
|
1315
|
+
_np[n] = shifted_copy[n]
|
|
1316
|
+
_np.show()
|
|
1317
|
+
|
|
1318
|
+
# get linearly interpolated colors in RGB space.
|
|
1319
|
+
# "percent" is the blending distance between the two colors. 0 is first color only, and 100 is second color only.
|
|
1320
|
+
|
|
1321
|
+
|
|
1322
|
+
def lerp_RGB(r1, g1, b1, r2, g2, b2, percent):
|
|
1323
|
+
if percent < 0.0 or percent > 100.0:
|
|
1324
|
+
raise RuntimeError("Argument 'percent' must be between 0 and 100.")
|
|
1325
|
+
red = max(0, min(255, round(r1 + (r2 - r1) * (percent/100.0))))
|
|
1326
|
+
green = max(0, min(255, round(g1 + (g2 - g1) * (percent/100.0))))
|
|
1327
|
+
blue = max(0, min(255, round(b1 + (b2 - b1) * (percent/100.0))))
|
|
1328
|
+
return red, green, blue
|
|
1329
|
+
`,
|
|
1330
|
+
mbrobot_plus: `# mbrobot_plus.py
|
|
1331
|
+
# Version 1.4 (1.10.2022)
|
|
1332
|
+
|
|
1333
|
+
from microbit import i2c,pin0,pin1,pin2,sleep
|
|
1334
|
+
import machine
|
|
1335
|
+
import gc
|
|
1336
|
+
import music
|
|
1337
|
+
|
|
1338
|
+
_v = 50
|
|
1339
|
+
_axe = 0.082
|
|
1340
|
+
|
|
1341
|
+
def w(d1, d2, s1, s2):
|
|
1342
|
+
try:
|
|
1343
|
+
i2c.write(0x10, bytearray([0x00,d1, d2, s1, s2]))
|
|
1344
|
+
except:
|
|
1345
|
+
print("Please switch on mbRobot!")
|
|
1346
|
+
|
|
1347
|
+
def setSpeed(speed):
|
|
1348
|
+
global _v
|
|
1349
|
+
if speed < 30 and speed != 0:
|
|
1350
|
+
setPID(1)
|
|
1351
|
+
_v = speed + 30
|
|
1352
|
+
elif speed >= 30 and speed < 32:
|
|
1353
|
+
setPID(0)
|
|
1354
|
+
_v = speed + 2
|
|
1355
|
+
else:
|
|
1356
|
+
setPID(0)
|
|
1357
|
+
_v = speed
|
|
1358
|
+
|
|
1359
|
+
def setPID(pd):
|
|
1360
|
+
i2c.write(0x10, bytearray([0x0A, pd]))
|
|
1361
|
+
|
|
1362
|
+
def stop():
|
|
1363
|
+
setPID(0)
|
|
1364
|
+
w(0, 0, 0, 0)
|
|
1365
|
+
|
|
1366
|
+
def resetSpeed():
|
|
1367
|
+
setPID(0)
|
|
1368
|
+
_v = 50
|
|
1369
|
+
|
|
1370
|
+
def forward():
|
|
1371
|
+
w(1, _v, 1, _v)
|
|
1372
|
+
|
|
1373
|
+
def backward():
|
|
1374
|
+
w(2, _v, 2, _v)
|
|
1375
|
+
|
|
1376
|
+
def left():
|
|
1377
|
+
m = 1.825 -0.0175 * _v
|
|
1378
|
+
w(2, int(_v * m), 1, int(_v * m))
|
|
1379
|
+
|
|
1380
|
+
def right():
|
|
1381
|
+
m = 1.825 -0.0175 * _v
|
|
1382
|
+
w(1, int(_v * m), 2, int(_v * m))
|
|
1383
|
+
|
|
1384
|
+
def rightArc(r):
|
|
1385
|
+
v = abs(_v)
|
|
1386
|
+
if r < _axe:
|
|
1387
|
+
v1 = 0
|
|
1388
|
+
else:
|
|
1389
|
+
f = (r - _axe) / (r + _axe) * (1 - v * v / 200000)
|
|
1390
|
+
v1 = int(f * v)
|
|
1391
|
+
if _v > 0:
|
|
1392
|
+
w(1, v, 1, v1)
|
|
1393
|
+
else:
|
|
1394
|
+
w(2, v1, 2, v)
|
|
1395
|
+
|
|
1396
|
+
def leftArc(r):
|
|
1397
|
+
v = abs(_v)
|
|
1398
|
+
if r < _axe:
|
|
1399
|
+
v1 = 0
|
|
1400
|
+
else:
|
|
1401
|
+
f = (r - _axe) / (r + _axe) * (1 - v * v / 200000)
|
|
1402
|
+
v1 = int(f * v)
|
|
1403
|
+
if _v > 0:
|
|
1404
|
+
w(1, v1, 1, v)
|
|
1405
|
+
else:
|
|
1406
|
+
w(2, v, 2, v1)
|
|
1407
|
+
|
|
1408
|
+
def getDistance():
|
|
1409
|
+
pin1.write_digital(1)
|
|
1410
|
+
pin1.write_digital(0)
|
|
1411
|
+
p = machine.time_pulse_us(pin2, 1, 50000)
|
|
1412
|
+
cm = int(p / 58.2 - 0.5)
|
|
1413
|
+
return cm if cm > 0 else 255
|
|
1414
|
+
|
|
1415
|
+
class Motor:
|
|
1416
|
+
def __init__(self, id):
|
|
1417
|
+
self._id = 2 * id
|
|
1418
|
+
|
|
1419
|
+
def _w(self, d, s):
|
|
1420
|
+
try:
|
|
1421
|
+
i2c.write(0x10, bytearray([self._id, d, s]))
|
|
1422
|
+
except:
|
|
1423
|
+
print("Please switch on mbRobot!")
|
|
1424
|
+
while True:
|
|
1425
|
+
pass
|
|
1426
|
+
|
|
1427
|
+
def rotate(self, s):
|
|
1428
|
+
p = abs(s)
|
|
1429
|
+
if s > 0:
|
|
1430
|
+
self._w(1, p)
|
|
1431
|
+
elif s < 0:
|
|
1432
|
+
self._w(2, p)
|
|
1433
|
+
else:
|
|
1434
|
+
self._w(0, 0)
|
|
1435
|
+
|
|
1436
|
+
class LEDState:
|
|
1437
|
+
OFF = 0
|
|
1438
|
+
RED = 1
|
|
1439
|
+
GREEN = 2
|
|
1440
|
+
YELLOW = 3
|
|
1441
|
+
BLUE = 4
|
|
1442
|
+
PINK = 5
|
|
1443
|
+
CYAN = 6
|
|
1444
|
+
WHITE = 7
|
|
1445
|
+
|
|
1446
|
+
def setLED(state, stateR=None):
|
|
1447
|
+
stateR = stateR or state
|
|
1448
|
+
i2c.write(0x10, bytearray([0x0B, state, stateR]))
|
|
1449
|
+
|
|
1450
|
+
def setLEDLeft(state):
|
|
1451
|
+
i2c.write(0x10, bytearray([0x0B, state]))
|
|
1452
|
+
|
|
1453
|
+
def setLEDRight(state):
|
|
1454
|
+
i2c.write(0x10, bytearray([0x0C, state]))
|
|
1455
|
+
|
|
1456
|
+
def setAlarm(on):
|
|
1457
|
+
if on:
|
|
1458
|
+
music.play(_m, wait = False, loop = True)
|
|
1459
|
+
else:
|
|
1460
|
+
music.stop()
|
|
1461
|
+
|
|
1462
|
+
def beep():
|
|
1463
|
+
music.pitch(2000, 200, wait = False)
|
|
1464
|
+
|
|
1465
|
+
def ir_read_values_as_byte():
|
|
1466
|
+
i2c.write(0x10, bytearray([0x1D]))
|
|
1467
|
+
buf = i2c.read(0x10, 1)
|
|
1468
|
+
return ~buf[0]
|
|
1469
|
+
|
|
1470
|
+
def setServo(S, Angle):
|
|
1471
|
+
if S == "S1":
|
|
1472
|
+
Servo = 0x14
|
|
1473
|
+
if S == "S2":
|
|
1474
|
+
Servo = 0x15
|
|
1475
|
+
buf = Servo, Angle
|
|
1476
|
+
i2c.write(0x10, bytes(buf))
|
|
1477
|
+
|
|
1478
|
+
class IR:
|
|
1479
|
+
L3 = 0
|
|
1480
|
+
L2 = 1
|
|
1481
|
+
L1 = 2
|
|
1482
|
+
R1 = 3
|
|
1483
|
+
R2 = 4
|
|
1484
|
+
R3 = 5
|
|
1485
|
+
masks = [0x01,0x02,0x04,0x08,0x10,0x20]
|
|
1486
|
+
|
|
1487
|
+
class IRSensor:
|
|
1488
|
+
def __init__(self, index):
|
|
1489
|
+
self.index = index
|
|
1490
|
+
|
|
1491
|
+
def read_digital(self):
|
|
1492
|
+
byte = ir_read_values_as_byte()
|
|
1493
|
+
return (byte & IR.masks[self.index]) >> self.index
|
|
1494
|
+
|
|
1495
|
+
irLeft = IRSensor(IR.L1)
|
|
1496
|
+
irRight = IRSensor(IR.R1)
|
|
1497
|
+
irL1 = IRSensor(IR.L1)
|
|
1498
|
+
irR1 = IRSensor(IR.R1)
|
|
1499
|
+
irL2 = IRSensor(IR.L2)
|
|
1500
|
+
irR2 = IRSensor(IR.R2)
|
|
1501
|
+
irL3 = IRSensor(IR.L3)
|
|
1502
|
+
irR3 = IRSensor(IR.R3)
|
|
1503
|
+
pin2.set_pull(pin2.NO_PULL)
|
|
1504
|
+
motL = Motor(0)
|
|
1505
|
+
motR = Motor(1)
|
|
1506
|
+
delay = sleep
|
|
1507
|
+
_m = ['c6:1', 'r', 'c6,1', 'r', 'r', 'r']
|
|
1508
|
+
|
|
1509
|
+
`,
|
|
1510
|
+
mbrobot: `# mbrobot.py
|
|
1511
|
+
# Date 10/09/24
|
|
1512
|
+
|
|
1513
|
+
from microbit import i2c, pin1, pin2, pin8, pin12, pin13, pin14, pin15, sleep
|
|
1514
|
+
import gc
|
|
1515
|
+
import machine
|
|
1516
|
+
import music
|
|
1517
|
+
import neopixel
|
|
1518
|
+
|
|
1519
|
+
# Motor state
|
|
1520
|
+
_speedPercent = 50
|
|
1521
|
+
_powerByteL = 40
|
|
1522
|
+
_powerByteR = 40
|
|
1523
|
+
_motorState = bytearray(5)
|
|
1524
|
+
_servoBytes = bytearray(2)
|
|
1525
|
+
_powerBytesLUT = bytes(b'\\x00\\x0b\\x0b\\x0c\\x0c\\x0d\\x0d\\x0d\\x0e\\x0e\\x0f\\x0f\\x0f\\x10\\x10\\x11\\x11\\x11\\x12\\x12\\x13\\x13\\x13\\x14\\x14\\x15\\x15\\x15\\x16\\x16\\x17\\x17\\x17\\x18\\x19\\x1a\\x1b\\x1b\\x1c\\x1d\\x1e\\x1f\\x20\\x21\\x22\\x23\\x23\\x24\\x25\\x26\\x27\\x28\\x29\\x2a\\x2b\\x2b\\x2c\\x2d\\x2e\\x2f\\x30\\x31\\x32\\x33\\x34\\x36\\x38\\x3a\\x3c\\x3f\\x41\\x44\\x46\\x49\\x4c\\x4f\\x53\\x56\\x5a\\x5e\\x62\\x67\\x6b\\x70\\x75\\x7b\\x81\\x87\\x8d\\x94\\x9b\\xa3\\xab\\xb4\\xbd\\xc6\\xd0\\xdb\\xe6\\xf2\\xff')
|
|
1526
|
+
|
|
1527
|
+
# Calibration data
|
|
1528
|
+
_powerOffset = 0
|
|
1529
|
+
_powerDifferential = 0
|
|
1530
|
+
_arcScaling = 0
|
|
1531
|
+
|
|
1532
|
+
# signaling objects
|
|
1533
|
+
_underglowNP = neopixel.NeoPixel(pin15, 4)
|
|
1534
|
+
np_rgb_pixels = _underglowNP
|
|
1535
|
+
_alarmSequence = ['c5:1', 'r', 'c5:1', 'r:3']
|
|
1536
|
+
|
|
1537
|
+
_UNCONNECTEDERRORMSG = "Please connect to Maqueen robot and switch it on."
|
|
1538
|
+
|
|
1539
|
+
# Utility functions
|
|
1540
|
+
|
|
1541
|
+
|
|
1542
|
+
def _setMotors(dirL, powerL, dirR, powerR):
|
|
1543
|
+
# """Write Motor State via i2c
|
|
1544
|
+
|
|
1545
|
+
# Parameters:
|
|
1546
|
+
# dirL (0/1): Direction of left Wheel. 0=forward, 1=backward
|
|
1547
|
+
# powerL (int): Power of left Wheel in range [0,255].
|
|
1548
|
+
# dirR (0/1): Direction of right Wheel. 0=forward, 1=backward
|
|
1549
|
+
# powerR (int): Power of right Wheel in range [0,255].
|
|
1550
|
+
#
|
|
1551
|
+
# raises:
|
|
1552
|
+
# RuntimeError: if Robot is switched off or unconnected. (replaces ENODEV error)
|
|
1553
|
+
# """
|
|
1554
|
+
global _motorState
|
|
1555
|
+
_motorState[1] = dirL
|
|
1556
|
+
_motorState[2] = powerL
|
|
1557
|
+
_motorState[3] = dirR
|
|
1558
|
+
_motorState[4] = powerR
|
|
1559
|
+
try:
|
|
1560
|
+
i2c.write(0x10, _motorState)
|
|
1561
|
+
except:
|
|
1562
|
+
raise RuntimeError(_UNCONNECTEDERRORMSG)
|
|
1563
|
+
|
|
1564
|
+
|
|
1565
|
+
def _setSingleMotor(side, dir, power):
|
|
1566
|
+
# """Write Motor State of a single Motor via i2c
|
|
1567
|
+
|
|
1568
|
+
# Parameters:
|
|
1569
|
+
# side (0/2): Selection of the Wheel. 0=left, 2=right
|
|
1570
|
+
# dir (0/1): Direction for that Wheel. 0=forward, 1=backward
|
|
1571
|
+
# power (int): Power for that Wheel in range [0,255].
|
|
1572
|
+
#
|
|
1573
|
+
# raises:
|
|
1574
|
+
# RuntimeError: if Robot is switched off or unconnected. (replaces ENODEV error)
|
|
1575
|
+
# """
|
|
1576
|
+
global _motorState
|
|
1577
|
+
_motorState[1 + side] = dir
|
|
1578
|
+
_motorState[2 + side] = power
|
|
1579
|
+
try:
|
|
1580
|
+
i2c.write(0x10, _motorState)
|
|
1581
|
+
except:
|
|
1582
|
+
raise RuntimeError(_UNCONNECTEDERRORMSG)
|
|
1583
|
+
|
|
1584
|
+
# _getPowerByte was used to create LookUpTable (_powerBytesLUT) used in _getPowerByteLUT to reduce Memory Leaks.
|
|
1585
|
+
# def _getPowerByte(speed, offset):
|
|
1586
|
+
# # """Computes the power value for a given speed in %.
|
|
1587
|
+
# # It accouts for the nonlinearity of motor strength.
|
|
1588
|
+
# # This is the original function that generates the LUT.
|
|
1589
|
+
#
|
|
1590
|
+
# # Parameter:
|
|
1591
|
+
# # speed (number): Desired speed of the Robot in %. Range [0,100].
|
|
1592
|
+
# # offset (number): basic power offset to add to the function.
|
|
1593
|
+
# # Returns:
|
|
1594
|
+
# # int: Power in range [0,255] to write to the motors via i2c.
|
|
1595
|
+
# # """
|
|
1596
|
+
# if speed <= 0:
|
|
1597
|
+
# return 0
|
|
1598
|
+
# if speed <= 33:
|
|
1599
|
+
# return int(0.4 * speed + 11 + offset)
|
|
1600
|
+
# elif speed <= 63:
|
|
1601
|
+
# return int(0.89 * speed - 5 + offset)
|
|
1602
|
+
# elif speed < 100:
|
|
1603
|
+
# return int(min(1.0561 ** speed + 20 + offset, 255))
|
|
1604
|
+
# else:
|
|
1605
|
+
# return 255
|
|
1606
|
+
|
|
1607
|
+
|
|
1608
|
+
def _getPowerByteLUT(speed, offset):
|
|
1609
|
+
# """Lookup Table version of _getPowerByte"""
|
|
1610
|
+
return min(_powerBytesLUT[speed] + offset, 255)
|
|
1611
|
+
|
|
1612
|
+
|
|
1613
|
+
def _getArcBytes(r):
|
|
1614
|
+
# """Computes the power bytes to drive an arc.
|
|
1615
|
+
|
|
1616
|
+
# Parameter:
|
|
1617
|
+
# r (float): Radius in meters of the desired Arc.
|
|
1618
|
+
# Measured from the center of the axle.
|
|
1619
|
+
# Returns:
|
|
1620
|
+
# (int, int): Power byte values for each motor.
|
|
1621
|
+
# First the outer Wheels byte [0,255],
|
|
1622
|
+
# then the inner Wheels byte [0,255].
|
|
1623
|
+
# """
|
|
1624
|
+
outerSpeed = _speedPercent
|
|
1625
|
+
rCm = int(r * 100) # r in cm instead of meters
|
|
1626
|
+
# adjust outer speed for unhealthy combinations
|
|
1627
|
+
# That is: too low speeds and big arcs.
|
|
1628
|
+
threshold = outerSpeed - max(rCm + 20, 40)
|
|
1629
|
+
if threshold <= 0:
|
|
1630
|
+
outerSpeed = min(max(rCm + 40, 40), 100)
|
|
1631
|
+
reducedSpeed = 0
|
|
1632
|
+
if rCm >= 4: # minimal radius is half the axle size: 3.5 cm rounded.
|
|
1633
|
+
flattening = (100 - outerSpeed) // 2
|
|
1634
|
+
reducedSpeed = (rCm * 10 - 35) / \\
|
|
1635
|
+
(rCm * (11 + (_arcScaling-4)/10) + 90 + flattening)
|
|
1636
|
+
reducedSpeed = reducedSpeed * outerSpeed
|
|
1637
|
+
innerByte = _getPowerByteLUT(int(reducedSpeed), 0)
|
|
1638
|
+
outerByte = _getPowerByteLUT(int(outerSpeed), 0)
|
|
1639
|
+
return (innerByte, outerByte)
|
|
1640
|
+
|
|
1641
|
+
# Movement Functions
|
|
1642
|
+
|
|
1643
|
+
|
|
1644
|
+
def calibrate(offset, differential=0, arcScaling=0):
|
|
1645
|
+
# """Adjust the driving behaviour of the robots
|
|
1646
|
+
|
|
1647
|
+
# Parameters:
|
|
1648
|
+
# offset (int): Offsets the minimal power of the motors.
|
|
1649
|
+
# Range [-10,50]. Highly affected by battery level.
|
|
1650
|
+
# Adjust this value until it starts moving at speed 1%.
|
|
1651
|
+
# differential (int, optional): Adjusts power difference
|
|
1652
|
+
# of left and right Wheel. Range [-150, 150].
|
|
1653
|
+
# Varies unpredictably with different speeds.
|
|
1654
|
+
# If a Robot steers left when driving forward: negative value
|
|
1655
|
+
# If a Robot steers right when driving forward: positive value
|
|
1656
|
+
# Perfectly straight driving Robots can leave this at 0.
|
|
1657
|
+
# arcScaling (int, optional): Adjusts the radius
|
|
1658
|
+
# driven by leftArc/rightArc. Valid range [-50, 50].
|
|
1659
|
+
# If the Robots radius is too large: positive value
|
|
1660
|
+
# If the Robots radius is too small: negative value
|
|
1661
|
+
# This then adjusts all radii for this Robot, by
|
|
1662
|
+
# scaling it's internal function to the new range.
|
|
1663
|
+
# """
|
|
1664
|
+
global _powerDifferential
|
|
1665
|
+
global _powerOffset
|
|
1666
|
+
global _arcScaling
|
|
1667
|
+
_powerOffset = max(min(int(offset), 50), -10)
|
|
1668
|
+
_powerDifferential = max(min(int(differential), 150), -150)
|
|
1669
|
+
_arcScaling = max(min(arcScaling, 50), -15)
|
|
1670
|
+
setSpeed(_speedPercent)
|
|
1671
|
+
|
|
1672
|
+
|
|
1673
|
+
def setSpeed(speed):
|
|
1674
|
+
# """sets the speed for future motion
|
|
1675
|
+
|
|
1676
|
+
# Parameter:
|
|
1677
|
+
# speed (int): in Range [0,100] as % of desired velocity.
|
|
1678
|
+
# """
|
|
1679
|
+
global _speedPercent
|
|
1680
|
+
global _powerByteL
|
|
1681
|
+
global _powerByteR
|
|
1682
|
+
_speedPercent = int(min(max(speed, 0), 100))
|
|
1683
|
+
powerByte = _getPowerByteLUT(_speedPercent, _powerOffset)
|
|
1684
|
+
boost = round((1 - _speedPercent / 100) *
|
|
1685
|
+
abs(_powerDifferential)) if _speedPercent > 0 else 0
|
|
1686
|
+
reduction = round((_speedPercent / 100) * abs(_powerDifferential))
|
|
1687
|
+
if _powerDifferential > 0:
|
|
1688
|
+
_powerByteL = powerByte - reduction
|
|
1689
|
+
_powerByteR = powerByte + boost
|
|
1690
|
+
else:
|
|
1691
|
+
_powerByteL = powerByte + boost
|
|
1692
|
+
_powerByteR = powerByte - reduction
|
|
1693
|
+
|
|
1694
|
+
|
|
1695
|
+
def resetSpeed():
|
|
1696
|
+
setSpeed(50)
|
|
1697
|
+
|
|
1698
|
+
|
|
1699
|
+
def stop():
|
|
1700
|
+
_setMotors(0, 0, 0, 0)
|
|
1701
|
+
|
|
1702
|
+
|
|
1703
|
+
def forward():
|
|
1704
|
+
_setMotors(0, _powerByteL, 0, _powerByteR)
|
|
1705
|
+
|
|
1706
|
+
|
|
1707
|
+
def backward():
|
|
1708
|
+
_setMotors(1, _powerByteL, 1, _powerByteR)
|
|
1709
|
+
|
|
1710
|
+
|
|
1711
|
+
def left():
|
|
1712
|
+
_setMotors(1, _powerByteL, 0, _powerByteR)
|
|
1713
|
+
|
|
1714
|
+
|
|
1715
|
+
def right():
|
|
1716
|
+
_setMotors(0, _powerByteL, 1, _powerByteR)
|
|
1717
|
+
|
|
1718
|
+
|
|
1719
|
+
def rightArc(radius):
|
|
1720
|
+
# """radius must be given in meters."""
|
|
1721
|
+
inner, outer = _getArcBytes(radius)
|
|
1722
|
+
_setMotors(0, outer, 0, inner)
|
|
1723
|
+
|
|
1724
|
+
|
|
1725
|
+
def leftArc(radius):
|
|
1726
|
+
# """radius must be given in meters."""
|
|
1727
|
+
inner, outer = _getArcBytes(radius)
|
|
1728
|
+
_setMotors(0, inner, 0, outer)
|
|
1729
|
+
|
|
1730
|
+
|
|
1731
|
+
class Motor:
|
|
1732
|
+
def __init__(self, side):
|
|
1733
|
+
# """Create a single motor.
|
|
1734
|
+
|
|
1735
|
+
# Parameter:
|
|
1736
|
+
# side (8/2): 0=left, 2=right
|
|
1737
|
+
# """
|
|
1738
|
+
self._side = side
|
|
1739
|
+
|
|
1740
|
+
def rotate(self, speed):
|
|
1741
|
+
# """Controls rotation of this motor.
|
|
1742
|
+
|
|
1743
|
+
# Parameters:
|
|
1744
|
+
# speed (int): Desired speed in %.
|
|
1745
|
+
# Valid range [-100,100].
|
|
1746
|
+
# Negative values are for backward turning.
|
|
1747
|
+
# """
|
|
1748
|
+
speedClamped = int(min(max(abs(speed), 0), 100))
|
|
1749
|
+
power = _getPowerByteLUT(speedClamped, _powerOffset)
|
|
1750
|
+
direction = 0 if speed > 0 else 1
|
|
1751
|
+
_setSingleMotor(self._side, direction, power)
|
|
1752
|
+
|
|
1753
|
+
|
|
1754
|
+
def setServo(servo, angle):
|
|
1755
|
+
# """Moves the Servo to position angle.
|
|
1756
|
+
# Servos must be connected to the Maqueen Lite's Servo connectors.
|
|
1757
|
+
# They are located in front of the left wheel.
|
|
1758
|
+
|
|
1759
|
+
# Parameters:
|
|
1760
|
+
# servo (str): Desired Servo Port. Either 'S1' or 'S2'.
|
|
1761
|
+
# angle (int): Desired angle in degrees. Range [0,180].
|
|
1762
|
+
|
|
1763
|
+
# raises:
|
|
1764
|
+
# ValueError: if arguments are out of valid range
|
|
1765
|
+
# RuntimeError: if Robot is switched off or unconnected.
|
|
1766
|
+
# """
|
|
1767
|
+
global _servoBytes
|
|
1768
|
+
# Acceptance of "P0" and "P1" is for compatibility with V2.
|
|
1769
|
+
if servo == 'S1' or servo == 'P0':
|
|
1770
|
+
_servoBytes[0] = 0x14
|
|
1771
|
+
elif servo == 'S2' or servo == 'P1':
|
|
1772
|
+
_servoBytes[0] = 0x15
|
|
1773
|
+
else:
|
|
1774
|
+
raise ValueError("Unknown Servo. Please use 'S1' or 'S2'.")
|
|
1775
|
+
|
|
1776
|
+
if angle < 0 or angle > 180:
|
|
1777
|
+
raise ValueError("Invalid angle. Must be between 0 and 180")
|
|
1778
|
+
|
|
1779
|
+
_servoBytes[1] = angle
|
|
1780
|
+
try:
|
|
1781
|
+
i2c.write(0x10, _servoBytes)
|
|
1782
|
+
except:
|
|
1783
|
+
raise RuntimeError(_UNCONNECTEDERRORMSG)
|
|
1784
|
+
|
|
1785
|
+
# Sensor functions
|
|
1786
|
+
|
|
1787
|
+
|
|
1788
|
+
class IRSensor():
|
|
1789
|
+
def __init__(self, pin):
|
|
1790
|
+
# """Create a new IR sensor.
|
|
1791
|
+
|
|
1792
|
+
# Parameter:
|
|
1793
|
+
# pin (object): pin13=left, pin14=right
|
|
1794
|
+
# """
|
|
1795
|
+
self._pin = pin
|
|
1796
|
+
|
|
1797
|
+
def read_digital(self):
|
|
1798
|
+
# """Returns if the surface below is dark or bright.
|
|
1799
|
+
# Result can be adjusted by putting the sensor on the dark
|
|
1800
|
+
# surface and pressing the LineKey calibration button on the
|
|
1801
|
+
# Robot for a few seconds, until the LED's blink.
|
|
1802
|
+
|
|
1803
|
+
# Returns:
|
|
1804
|
+
# 0 if the surface is dark. No light was reflected (in Air).
|
|
1805
|
+
# 1 if the surface is bright. A lot of light was reflected.
|
|
1806
|
+
# """
|
|
1807
|
+
return self._pin.read_digital()
|
|
1808
|
+
|
|
1809
|
+
def read_analog(self):
|
|
1810
|
+
raise NameError(
|
|
1811
|
+
"Maqueen Lite does not support reading analog sensor values.")
|
|
1812
|
+
|
|
1813
|
+
|
|
1814
|
+
def getDistance():
|
|
1815
|
+
# """uses the ultrasonic sensor to measure distance
|
|
1816
|
+
|
|
1817
|
+
# Returns:
|
|
1818
|
+
# int: valid Distance as cm in range [0,500].
|
|
1819
|
+
# For measurement errors or larger distances: 255.
|
|
1820
|
+
# """
|
|
1821
|
+
pin1.write_digital(1)
|
|
1822
|
+
pin1.write_digital(0)
|
|
1823
|
+
p = machine.time_pulse_us(pin2, 1, 50000)
|
|
1824
|
+
# approximate division: cm = p / 57.5
|
|
1825
|
+
cm = (p >> 6) + (p >> 10) + (p >> 11) + (p >> 12) + 1
|
|
1826
|
+
return max(min(cm, 500), 0) if cm > 0 else 255
|
|
1827
|
+
|
|
1828
|
+
# Signaling functions
|
|
1829
|
+
|
|
1830
|
+
|
|
1831
|
+
def setLED(state, stateR=None):
|
|
1832
|
+
# """Set the front red LED's.
|
|
1833
|
+
|
|
1834
|
+
# Parameters:
|
|
1835
|
+
# state (0/1): Sets the state of the left LED.
|
|
1836
|
+
# if stateR is omitted, then both LEDS.
|
|
1837
|
+
# 0=Off, 1=On
|
|
1838
|
+
# stateR (0/1/None, optional): Sets the right LED state.
|
|
1839
|
+
# 0=Off, 1=On, Default=None uses "state" for right LED.
|
|
1840
|
+
# """
|
|
1841
|
+
stateR = stateR if stateR != None else state
|
|
1842
|
+
pin8.write_digital(state)
|
|
1843
|
+
pin12.write_digital(stateR)
|
|
1844
|
+
|
|
1845
|
+
|
|
1846
|
+
def setLEDLeft(state):
|
|
1847
|
+
# """state: 0=Off, 1=On"""
|
|
1848
|
+
pin8.write_digital(state)
|
|
1849
|
+
|
|
1850
|
+
|
|
1851
|
+
def setLEDRight(state):
|
|
1852
|
+
# """state: 0=Off, 1=On"""
|
|
1853
|
+
pin12.write_digital(state)
|
|
1854
|
+
|
|
1855
|
+
|
|
1856
|
+
def fillRGB(red, green, blue):
|
|
1857
|
+
# """Uses Neopixel to set all 4 bottom RGB LEDs color.
|
|
1858
|
+
# Parameters (red,green,blue) are each a byte in Range [0,255].
|
|
1859
|
+
# """
|
|
1860
|
+
for i in range(4):
|
|
1861
|
+
_underglowNP[i] = (red, green, blue)
|
|
1862
|
+
_underglowNP.show()
|
|
1863
|
+
setRGB = fillRGB
|
|
1864
|
+
|
|
1865
|
+
|
|
1866
|
+
def clearRGB():
|
|
1867
|
+
_underglowNP.clear()
|
|
1868
|
+
|
|
1869
|
+
|
|
1870
|
+
def posRGB(position, red, green, blue):
|
|
1871
|
+
# """Uses Neopixel to set a single RGB LED of the robot.
|
|
1872
|
+
|
|
1873
|
+
# Parameters:
|
|
1874
|
+
# position (int): position of the targeted LED.
|
|
1875
|
+
# Numbers are visible at underside of Robot.
|
|
1876
|
+
# 0=front left
|
|
1877
|
+
# 1=back left
|
|
1878
|
+
# 2=back right
|
|
1879
|
+
# 3=front right
|
|
1880
|
+
# red, green, blue (int): color byte value.
|
|
1881
|
+
# each in range [0,255].
|
|
1882
|
+
|
|
1883
|
+
# raises:
|
|
1884
|
+
# ValueError: if position argument is out of valid range
|
|
1885
|
+
# """
|
|
1886
|
+
if position < 0 or position > 3:
|
|
1887
|
+
raise ValueError("invalid RGB-LED position. Must be 0,1,2 or 3.")
|
|
1888
|
+
_underglowNP[position] = (red, green, blue)
|
|
1889
|
+
_underglowNP.show()
|
|
1890
|
+
|
|
1891
|
+
|
|
1892
|
+
def setAlarm(state):
|
|
1893
|
+
# """state: 0=Off, 1=On"""
|
|
1894
|
+
if state:
|
|
1895
|
+
music.play(_alarmSequence, wait=False, loop=True)
|
|
1896
|
+
else:
|
|
1897
|
+
music.stop()
|
|
1898
|
+
|
|
1899
|
+
|
|
1900
|
+
def beep():
|
|
1901
|
+
music.pitch(440, 200, wait=False)
|
|
1902
|
+
|
|
1903
|
+
|
|
1904
|
+
# Default instances
|
|
1905
|
+
pin2.set_pull(pin2.NO_PULL)
|
|
1906
|
+
delay = sleep
|
|
1907
|
+
irLeft = IRSensor(pin13)
|
|
1908
|
+
irRight = IRSensor(pin14)
|
|
1909
|
+
motL = Motor(0)
|
|
1910
|
+
motR = Motor(2)
|
|
1911
|
+
`,
|
|
1912
|
+
mbrobot_plusV3: `# mbrobot_plusV3.py
|
|
1913
|
+
# Date 25/11/25
|
|
1914
|
+
|
|
1915
|
+
from microbit import i2c, sleep, running_time, pin0, pin1, pin2
|
|
1916
|
+
import neopixel
|
|
1917
|
+
import music
|
|
1918
|
+
|
|
1919
|
+
_motorState = bytearray(5)
|
|
1920
|
+
_speedPercent = 50
|
|
1921
|
+
_powerByteL = 50
|
|
1922
|
+
_powerByteR = 50
|
|
1923
|
+
_powerBytesLUT = bytes(b'\\x00\\x0f\\x10\\x10\\x11\\x12\\x12\\x13\\x13\\x14\\x15\\x15\\x16\\x16\\x17\\x18\\x18\\x19\\x19\\x1a\\x1b\\x1b\\x1c\\x1c\\x1d\\x1e\\x1e\\x1f\\x1f\\x20\\x21\\x21\\x22\\x22\\x23\\x24\\x24\\x25\\x25\\x26\\x27\\x29\\x2a\\x2b\\x2c\\x2d\\x2e\\x2f\\x30\\x31\\x32\\x33\\x34\\x35\\x36\\x37\\x38\\x39\\x3a\\x3b\\x3b\\x3c\\x3e\\x3f\\x41\\x43\\x45\\x47\\x49\\x4b\\x4d\\x50\\x52\\x55\\x58\\x5b\\x5e\\x61\\x65\\x69\\x6d\\x71\\x75\\x7a\\x7f\\x84\\x89\\x8f\\x95\\x9b\\xa2\\xa9\\xb1\\xb9\\xc1\\xca\\xd3\\xdd\\xe8\\xf3\\xff')
|
|
1924
|
+
_arcScaling = 0
|
|
1925
|
+
_UNCONNECTEDERRORMSG = "Please connect to Maqueen robot and switch it on."
|
|
1926
|
+
_underglowNP = neopixel.NeoPixel(pin1, 4)
|
|
1927
|
+
_alarmSequence = ['c5:1', 'r', 'c5,1', 'r:3']
|
|
1928
|
+
_buff1 = bytearray(1)
|
|
1929
|
+
_buff2 = bytearray(2)
|
|
1930
|
+
_add_mq = 0x10
|
|
1931
|
+
_servoMinPulse=25
|
|
1932
|
+
_servoMaxPulse=131
|
|
1933
|
+
_lidarMode=8
|
|
1934
|
+
|
|
1935
|
+
def _wr1(reg):
|
|
1936
|
+
_buff1[0] = reg
|
|
1937
|
+
i2c.write(_add_mq, _buff1)
|
|
1938
|
+
|
|
1939
|
+
def _wr2(reg, val):
|
|
1940
|
+
_buff2[0] = reg
|
|
1941
|
+
_buff2[1] = val
|
|
1942
|
+
i2c.write(_add_mq, _buff2)
|
|
1943
|
+
|
|
1944
|
+
def _setMotors(dirL, powerL, dirR, powerR):
|
|
1945
|
+
global _motorState
|
|
1946
|
+
_motorState[1] = dirL
|
|
1947
|
+
_motorState[2] = powerL
|
|
1948
|
+
_motorState[3] = dirR
|
|
1949
|
+
_motorState[4] = powerR
|
|
1950
|
+
try:
|
|
1951
|
+
i2c.write(0x10, _motorState)
|
|
1952
|
+
except:
|
|
1953
|
+
raise RuntimeError(_UNCONNECTEDERRORMSG)
|
|
1954
|
+
|
|
1955
|
+
def _setSingleMotor(side, dir, power):
|
|
1956
|
+
global _motorState
|
|
1957
|
+
_motorState[1 + side] = dir
|
|
1958
|
+
_motorState[2 + side] = power
|
|
1959
|
+
try:
|
|
1960
|
+
i2c.write(0x10, _motorState)
|
|
1961
|
+
except:
|
|
1962
|
+
raise RuntimeError(_UNCONNECTEDERRORMSG)
|
|
1963
|
+
|
|
1964
|
+
def setSpeed(speed):
|
|
1965
|
+
global _speedPercent
|
|
1966
|
+
global _powerByteL
|
|
1967
|
+
global _powerByteR
|
|
1968
|
+
_speedPercent = int(min(max(speed,0),100))
|
|
1969
|
+
_powerByteL = int(round(2.4*speed+14))
|
|
1970
|
+
_powerByteR = int(round(2.4*speed+14))
|
|
1971
|
+
|
|
1972
|
+
def resetSpeed():
|
|
1973
|
+
setSpeed(50)
|
|
1974
|
+
|
|
1975
|
+
def stop():
|
|
1976
|
+
_setMotors(0,0,0,0)
|
|
1977
|
+
|
|
1978
|
+
def forward():
|
|
1979
|
+
_setMotors(0, _powerByteL, 0, _powerByteR)
|
|
1980
|
+
|
|
1981
|
+
def backward():
|
|
1982
|
+
_setMotors(1, _powerByteL, 1, _powerByteR)
|
|
1983
|
+
|
|
1984
|
+
def left():
|
|
1985
|
+
_setMotors(1, _powerByteL, 0, _powerByteR)
|
|
1986
|
+
|
|
1987
|
+
def right():
|
|
1988
|
+
_setMotors(0, _powerByteL, 1, _powerByteR)
|
|
1989
|
+
|
|
1990
|
+
def _getPowerByteLUT(speed, offset):
|
|
1991
|
+
return min(_powerBytesLUT[speed] + offset, 255)
|
|
1992
|
+
|
|
1993
|
+
def _getArcBytes(r):
|
|
1994
|
+
rmm = int(r * 100)
|
|
1995
|
+
outerSpeed = _speedPercent
|
|
1996
|
+
if outerSpeed < 25:
|
|
1997
|
+
outerSpeed = 25
|
|
1998
|
+
reducedSpeed = 0
|
|
1999
|
+
if rmm > 5:
|
|
2000
|
+
n = outerSpeed * (3 * _arcScaling - outerSpeed - 9 * rmm + 220)
|
|
2001
|
+
d = -14 * _arcScaling + outerSpeed - 200 + 3 * outerSpeed - 10 * rmm - 290
|
|
2002
|
+
reducedSpeed = int(n/d)
|
|
2003
|
+
if reducedSpeed < 2:
|
|
2004
|
+
reducedSpeed = 2 if rmm > 15 else 1
|
|
2005
|
+
innerByte = _getPowerByteLUT(int(reducedSpeed), 0)
|
|
2006
|
+
outerByte = _getPowerByteLUT(int(outerSpeed), 0)
|
|
2007
|
+
return (innerByte, outerByte)
|
|
2008
|
+
|
|
2009
|
+
def rightArc(radius):
|
|
2010
|
+
inner, outer = _getArcBytes(radius)
|
|
2011
|
+
_setMotors(0, outer, 0, inner)
|
|
2012
|
+
|
|
2013
|
+
|
|
2014
|
+
def leftArc(radius):
|
|
2015
|
+
inner, outer = _getArcBytes(radius)
|
|
2016
|
+
_setMotors(0, inner, 0, outer)
|
|
2017
|
+
|
|
2018
|
+
class Motor:
|
|
2019
|
+
def __init__(self, side):
|
|
2020
|
+
self._side = side
|
|
2021
|
+
|
|
2022
|
+
def rotate(self, speed):
|
|
2023
|
+
speedClamped = int(min(max(abs(speed), 0), 100))
|
|
2024
|
+
power = speedClamped
|
|
2025
|
+
direction = 0 if speed > 0 else 1
|
|
2026
|
+
_setSingleMotor(self._side, direction, power)
|
|
2027
|
+
|
|
2028
|
+
def setServo(servo,angle):
|
|
2029
|
+
if angle < 0 or angle >180:
|
|
2030
|
+
raise ValueError("Invalid angle. Must be between 0 and 180")
|
|
2031
|
+
if servo in ["P0", "S1"]:
|
|
2032
|
+
pin = pin0
|
|
2033
|
+
elif servo in ["P1", "S2"]:
|
|
2034
|
+
pin = pin1
|
|
2035
|
+
elif servo in ["P2", "S3"]:
|
|
2036
|
+
pin = pin2
|
|
2037
|
+
else:
|
|
2038
|
+
raise ValueError("Valid servo names: S1, S2, S3 or P0, P1, P2")
|
|
2039
|
+
frac = (_servoMaxPulse - _servoMinPulse) * int(angle)
|
|
2040
|
+
offset = (frac >> 8)+ (frac >> 10) + (frac >> 11)+ (frac >> 12) # / 180
|
|
2041
|
+
usPulseTime = _servoMinPulse + offset
|
|
2042
|
+
pin.set_analog_period(20)
|
|
2043
|
+
pin.write_analog(usPulseTime)
|
|
2044
|
+
|
|
2045
|
+
class IRSensor:
|
|
2046
|
+
_address = bytes(b'\\x1D')
|
|
2047
|
+
|
|
2048
|
+
def __init__(self, index):
|
|
2049
|
+
self.index = index
|
|
2050
|
+
|
|
2051
|
+
def read_digital(self):
|
|
2052
|
+
try:
|
|
2053
|
+
i2c.write(0x10, IRSensor._address)
|
|
2054
|
+
except:
|
|
2055
|
+
raise RuntimeError(_UNCONNECTEDERRORMSG)
|
|
2056
|
+
byte = ~i2c.read(0x10, 1)[0]
|
|
2057
|
+
return (byte & (2 ** self.index)) >> self.index
|
|
2058
|
+
|
|
2059
|
+
def read_analog(self):
|
|
2060
|
+
try:
|
|
2061
|
+
_wr1(0x1D)
|
|
2062
|
+
except:
|
|
2063
|
+
raise RuntimeError(_UNCONNECTEDERRORMSG)
|
|
2064
|
+
buffer = i2c.read(0x10,11)
|
|
2065
|
+
return buffer[2+2*self.index] << 8 | buffer[1+2*self.index]
|
|
2066
|
+
|
|
2067
|
+
def setLEDs(rgbl, rgbr):
|
|
2068
|
+
_wr2(11, rgbl)
|
|
2069
|
+
_wr2(12, rgbr)
|
|
2070
|
+
|
|
2071
|
+
def setLED(state, stateR=None):
|
|
2072
|
+
stateR = stateR if stateR != None else state
|
|
2073
|
+
setLEDs(state, stateR)
|
|
2074
|
+
|
|
2075
|
+
def setLEDLeft(rgbl):
|
|
2076
|
+
_wr2(11, rgbl)
|
|
2077
|
+
|
|
2078
|
+
def setLEDRight(rgbr):
|
|
2079
|
+
_wr2(12, rgbr)
|
|
2080
|
+
|
|
2081
|
+
def fillRGB(red, green, blue):
|
|
2082
|
+
_underglowNP.clear()
|
|
2083
|
+
_underglowNP.fill((red,green,blue))
|
|
2084
|
+
_underglowNP.show()
|
|
2085
|
+
|
|
2086
|
+
def setRGB(r,g,b):
|
|
2087
|
+
fillRGB(r,g,b)
|
|
2088
|
+
|
|
2089
|
+
def clearRGB():
|
|
2090
|
+
_underglowNP.clear()
|
|
2091
|
+
|
|
2092
|
+
def posRGB(position, red, green, blue):
|
|
2093
|
+
if position < 0 or position > 3:
|
|
2094
|
+
raise ValueError("invalid RGB-LED position. Must be 0,1,2 or 3.")
|
|
2095
|
+
_underglowNP[position] = (red, green, blue)
|
|
2096
|
+
_underglowNP.show()
|
|
2097
|
+
|
|
2098
|
+
def setAlarm(state):
|
|
2099
|
+
if state:
|
|
2100
|
+
music.play(_alarmSequence, wait=False, loop=True)
|
|
2101
|
+
else:
|
|
2102
|
+
music.stop()
|
|
2103
|
+
|
|
2104
|
+
def beep():
|
|
2105
|
+
music.pitch(440, 200, wait=False)
|
|
2106
|
+
|
|
2107
|
+
def readLightIntensity(side):
|
|
2108
|
+
_wr1(78)
|
|
2109
|
+
LightBuffer = i2c.read(0x10, 4, repeat=False)
|
|
2110
|
+
if side == 1:
|
|
2111
|
+
|
|
2112
|
+
return LightBuffer[0] << 8 | LightBuffer[1]
|
|
2113
|
+
else:
|
|
2114
|
+
return LightBuffer[2] << 8 | LightBuffer[3]
|
|
2115
|
+
|
|
2116
|
+
def setPatrolSpeed(speed):
|
|
2117
|
+
_wr2(63, speed)
|
|
2118
|
+
|
|
2119
|
+
def setIntersectionRunMode(mode):
|
|
2120
|
+
_wr2(69, mode)
|
|
2121
|
+
|
|
2122
|
+
def setTRordRunMode(mode):
|
|
2123
|
+
_wr2(70, mode)
|
|
2124
|
+
|
|
2125
|
+
def setLeftOrStraightRunMode(mode):
|
|
2126
|
+
_wr2(71, mode)
|
|
2127
|
+
|
|
2128
|
+
def setRightOrStraightRunMode(mode):
|
|
2129
|
+
_wr2(72, mode)
|
|
2130
|
+
|
|
2131
|
+
def patrolling(patrol):
|
|
2132
|
+
if patrol == 1:
|
|
2133
|
+
val = 0x04 | 0x01
|
|
2134
|
+
else:
|
|
2135
|
+
val = 0x08
|
|
2136
|
+
_wr2(60, val)
|
|
2137
|
+
|
|
2138
|
+
def intersectionDetecting():
|
|
2139
|
+
_wr1(61)
|
|
2140
|
+
data = i2c.read(0x10, 1)[0]
|
|
2141
|
+
return data
|
|
2142
|
+
|
|
2143
|
+
def pidControlDistance(dir, distance, interruption):
|
|
2144
|
+
speed = 2
|
|
2145
|
+
if distance >= 6000:
|
|
2146
|
+
distance = 60000
|
|
2147
|
+
_wr2(64, dir)
|
|
2148
|
+
_wr2(85, speed)
|
|
2149
|
+
_wr2(65, distance >> 8)
|
|
2150
|
+
_wr2(66, distance)
|
|
2151
|
+
_wr2(60, 0x04 | 0x02)
|
|
2152
|
+
if (interruption == 1):
|
|
2153
|
+
_wr1(0x57)
|
|
2154
|
+
flagBuffer = i2c.read(0x10, 1)
|
|
2155
|
+
while flagBuffer[0] == 1:
|
|
2156
|
+
sleep(10)
|
|
2157
|
+
flagBuffer = i2c.read(0x10, 1)
|
|
2158
|
+
|
|
2159
|
+
def pidControlAngle(angle, interruption):
|
|
2160
|
+
speed = 2
|
|
2161
|
+
if angle >= 0:
|
|
2162
|
+
buf = 1
|
|
2163
|
+
else:
|
|
2164
|
+
buf = 2
|
|
2165
|
+
angle = -angle
|
|
2166
|
+
_wr2(67, buf)
|
|
2167
|
+
_wr2(86, speed)
|
|
2168
|
+
_wr2(68, angle)
|
|
2169
|
+
_wr2(60, 0x04 | 0x02)
|
|
2170
|
+
if interruption == 1:
|
|
2171
|
+
_wr1(0x57)
|
|
2172
|
+
buff = i2c.read(0x10, 1)
|
|
2173
|
+
while buff[0] == 1:
|
|
2174
|
+
sleep(10)
|
|
2175
|
+
buff = i2c.read(0x10, 1)
|
|
2176
|
+
|
|
2177
|
+
def pidControlStop():
|
|
2178
|
+
_wr2(60, 0x10)
|
|
2179
|
+
|
|
2180
|
+
def readRealTimeSpeed(type):
|
|
2181
|
+
_wr2(76, 1)
|
|
2182
|
+
buff = i2c.read(0x10, 2)
|
|
2183
|
+
if type == 1:
|
|
2184
|
+
return buff[0] / 5
|
|
2185
|
+
else:
|
|
2186
|
+
return buff[1] / 5
|
|
2187
|
+
|
|
2188
|
+
def _sendLidarCommand(cmd, args=[]):
|
|
2189
|
+
args_len = len(args)
|
|
2190
|
+
packet_len = args_len + 1
|
|
2191
|
+
packet = bytearray(4 + args_len)
|
|
2192
|
+
packet[0] = 0x55
|
|
2193
|
+
packet[1] = (packet_len >> 8) & 0xFF
|
|
2194
|
+
packet[2] = packet_len & 0xFF
|
|
2195
|
+
packet[3] = cmd
|
|
2196
|
+
for i, arg in enumerate(args):
|
|
2197
|
+
packet[4 + i] = arg
|
|
2198
|
+
# chunkwise sending? TODO
|
|
2199
|
+
i2c.write(0x33, packet)
|
|
2200
|
+
|
|
2201
|
+
def _receiveLidarData(expectedCommand):
|
|
2202
|
+
TIMEOUT = 1000
|
|
2203
|
+
MAX_SIZE = 32
|
|
2204
|
+
start_time = running_time()
|
|
2205
|
+
success = False
|
|
2206
|
+
data = None
|
|
2207
|
+
started = False
|
|
2208
|
+
|
|
2209
|
+
while running_time() - start_time < TIMEOUT and started == False:
|
|
2210
|
+
status = i2c.read(0x33, 1)[0]
|
|
2211
|
+
if status == 0x53:
|
|
2212
|
+
started = True
|
|
2213
|
+
elif status == 0x63:
|
|
2214
|
+
return success, data
|
|
2215
|
+
sleep(16)
|
|
2216
|
+
|
|
2217
|
+
if started == True:
|
|
2218
|
+
header = i2c.read(0x33, 3)
|
|
2219
|
+
cmd = header[0]
|
|
2220
|
+
data_length = header[1] | (header[2] << 8)
|
|
2221
|
+
|
|
2222
|
+
if cmd == expectedCommand:
|
|
2223
|
+
success = True
|
|
2224
|
+
data = bytearray()
|
|
2225
|
+
remaining_bytes = data_length
|
|
2226
|
+
chunk_size = min(remaining_bytes, MAX_SIZE)
|
|
2227
|
+
while running_time() - start_time < TIMEOUT and remaining_bytes > 0:
|
|
2228
|
+
try:
|
|
2229
|
+
chunk = i2c.read(0x33, chunk_size)
|
|
2230
|
+
data.extend(chunk)
|
|
2231
|
+
remaining_bytes -= len(chunk)
|
|
2232
|
+
except:
|
|
2233
|
+
sleep(1)
|
|
2234
|
+
return success, data
|
|
2235
|
+
|
|
2236
|
+
def setLidarMode(mode=8):
|
|
2237
|
+
global _lidarMode
|
|
2238
|
+
if mode not in [4, 8]:
|
|
2239
|
+
raise ValueError("Lidar mode must be 4 or 8")
|
|
2240
|
+
mode_text = "4x4" if mode == 4 else "8x8"
|
|
2241
|
+
print("Switching Lidar Mode to " + mode_text + ".\\nPlease wait up to 10 seconds.")
|
|
2242
|
+
success = False
|
|
2243
|
+
|
|
2244
|
+
for i in range(10):
|
|
2245
|
+
_sendLidarCommand(1, [0, 0, 0, mode])
|
|
2246
|
+
success, _ = _receiveLidarData(1)
|
|
2247
|
+
|
|
2248
|
+
if success == True: break
|
|
2249
|
+
sleep(17)
|
|
2250
|
+
|
|
2251
|
+
if success:
|
|
2252
|
+
_lidarMode = mode
|
|
2253
|
+
sleep(5000)
|
|
2254
|
+
else:
|
|
2255
|
+
raise RuntimeError("Failed to switch Lidar Mode")
|
|
2256
|
+
|
|
2257
|
+
def getDistanceAt(x_pos, y_pos):
|
|
2258
|
+
_sendLidarCommand(0x3, [x_pos, y_pos])
|
|
2259
|
+
success, data = _receiveLidarData(0x3)
|
|
2260
|
+
if success and len(data) >= 2:
|
|
2261
|
+
distance = (data[0] | (data[1] << 8)) // 10
|
|
2262
|
+
return distance
|
|
2263
|
+
else:
|
|
2264
|
+
return 1023
|
|
2265
|
+
|
|
2266
|
+
def getDistanceList():
|
|
2267
|
+
_sendLidarCommand(0x2)
|
|
2268
|
+
success, data = _receiveLidarData(0x2)
|
|
2269
|
+
if success and len(data) >= 32:
|
|
2270
|
+
distList = []
|
|
2271
|
+
for i in range(0,len(data),2):
|
|
2272
|
+
d = data[i] | (data[i+1] << 8)
|
|
2273
|
+
distList.append(d // 10)
|
|
2274
|
+
return distList
|
|
2275
|
+
else:
|
|
2276
|
+
return []
|
|
2277
|
+
|
|
2278
|
+
def getDistance():
|
|
2279
|
+
global _lidarMode
|
|
2280
|
+
mid = int(_lidarMode/2)
|
|
2281
|
+
topLeft = getDistanceAt(mid-1, mid-1)
|
|
2282
|
+
topRight = getDistanceAt(mid, mid-1)
|
|
2283
|
+
bottomLeft = getDistanceAt(mid-1, mid)
|
|
2284
|
+
bottomRight = getDistanceAt(mid, mid)
|
|
2285
|
+
distanceList = [topLeft, topRight, bottomLeft, bottomRight]
|
|
2286
|
+
return min(distanceList)
|
|
2287
|
+
|
|
2288
|
+
|
|
2289
|
+
def getDistanceGrid():
|
|
2290
|
+
_sendLidarCommand(0x2)
|
|
2291
|
+
success, data = _receiveLidarData(0x2)
|
|
2292
|
+
if success and len(data) >= 32:
|
|
2293
|
+
distGrid = []
|
|
2294
|
+
stride = 16 if len(data) == 128 else 8
|
|
2295
|
+
for i in range(0,len(data),stride):
|
|
2296
|
+
row = []
|
|
2297
|
+
for j in range(0,stride,2):
|
|
2298
|
+
d = data[i+j] | (data[i+j+1] << 8)
|
|
2299
|
+
row.append(d // 10)
|
|
2300
|
+
distGrid.append(row)
|
|
2301
|
+
return distGrid
|
|
2302
|
+
else:
|
|
2303
|
+
return []
|
|
2304
|
+
|
|
2305
|
+
def getDistanceColumn(index):
|
|
2306
|
+
_sendLidarCommand(0x5, [index])
|
|
2307
|
+
success, data = _receiveLidarData(0x5)
|
|
2308
|
+
if success and len(data) >= 8:
|
|
2309
|
+
row = []
|
|
2310
|
+
for i in range(0, len(data), 2):
|
|
2311
|
+
distance = data[i] | (data[i+1] << 8)
|
|
2312
|
+
row.append(distance // 10)
|
|
2313
|
+
return row
|
|
2314
|
+
return []
|
|
2315
|
+
|
|
2316
|
+
def getDistanceRow(index):
|
|
2317
|
+
_sendLidarCommand(0x6, [index])
|
|
2318
|
+
success, data = _receiveLidarData(0x6)
|
|
2319
|
+
if success and len(data) >= 8:
|
|
2320
|
+
col = []
|
|
2321
|
+
for i in range(0, len(data), 2):
|
|
2322
|
+
distance = data[i] | (data[i+1] << 8)
|
|
2323
|
+
col.append(distance // 10)
|
|
2324
|
+
return col
|
|
2325
|
+
return []
|
|
2326
|
+
|
|
2327
|
+
|
|
2328
|
+
pin2.set_pull(pin2.NO_PULL)
|
|
2329
|
+
delay = sleep
|
|
2330
|
+
irR2 = IRSensor(0)
|
|
2331
|
+
irR1 = IRSensor(1)
|
|
2332
|
+
irRight = irR1
|
|
2333
|
+
irM = IRSensor(2)
|
|
2334
|
+
irL1 = IRSensor(3)
|
|
2335
|
+
irLeft = irL1
|
|
2336
|
+
irL2 = IRSensor(4)
|
|
2337
|
+
motL = Motor(0)
|
|
2338
|
+
motR = Motor(2)`,
|
|
2339
|
+
mbmarsrover: "from microbit import *\nfrom neopixel import *\nimport utime\n\n_servoArray = [0, 9, 11, 13, 15]\n_servoOffset = [0, 0, 0, 0, 0]\n_initI2C = False\n\n_nbLeds = 4\n_np = NeoPixel(pin2, _nbLeds)\n\n_speedPercent = 50\n_powerByteL = 40\n_powerByteR = 40\n\n_powerOffset = 0\n_powerDifferential = 0\n_arcScaling = 0\n\n_MAX_CM_DISTANCE = 500\n_CM_PER_MICROSECOND = 29.1\n\n\ndef _setMotors(dirL, powerL, dirR, powerR):\n pinsL = (pin1, pin12)\n pinsR = (pin8, pin0)\n\n pinsL[dirL].write_analog(powerL)\n pinsL[1 - dirL].write_analog(0)\n\n pinsR[dirR].write_analog(powerR)\n pinsR[1 - dirR].write_analog(0)\n\n\ndef _setSingleMotor(side, direction, power):\n if side == 0:\n pins = (pin1, pin12)\n elif side == 1:\n pins = (pin8, pin0)\n\n pins[direction].write_analog(power)\n pins[1 - direction].write_analog(0)\n\n\ndef _getArcBytes(r):\n outerSpeed = _speedPercent\n rCm = int(r * 100)\n threshold = outerSpeed - max(rCm + 20, 40)\n if (threshold <= 0):\n outerSpeed = min(max(rCm + 40, 40), 100)\n reducedSpeed = 0\n if rCm >= 4:\n flattening = (100 - outerSpeed) // 2\n reducedSpeed = (rCm * 10 - 35) / \\\n (rCm * (11 + (_arcScaling - 4) / 10) + 90 + flattening)\n reducedSpeed = reducedSpeed * outerSpeed\n innerByte = _convertSpeedToAnalogValue(int(reducedSpeed), 0)\n outerByte = _convertSpeedToAnalogValue(int(outerSpeed), 0)\n return (innerByte, outerByte)\n\n\ndef _initPCA():\n global _initI2C\n _initI2C = True\n i2cData = bytearray(2)\n i2cData[0] = 0\n i2cData[1] = 0x10\n i2c.write(0x40, i2cData)\n\n i2cData[0] = 0xFE\n i2cData[1] = 101\n i2c.write(0x40, i2cData)\n\n i2cData[0] = 0\n i2cData[1] = 0x81\n i2c.write(0x40, i2cData)\n\n\ndef _pulse_in(pin, value, timeout):\n start_time = utime.ticks_us()\n\n while pin.read_digital() != value:\n if utime.ticks_diff(utime.ticks_us(), start_time) > timeout:\n return 0\n\n start_time = utime.ticks_us()\n while pin.read_digital() == value:\n if utime.ticks_diff(utime.ticks_us(), start_time) > timeout:\n return 0\n\n return utime.ticks_diff(utime.ticks_us(), start_time)\n\n\ndef calibrate(offset, differential=0, arcScaling=0):\n global _powerDifferential\n global _powerOffset\n global _arcScaling\n\n _powerOffset = max(min(int(offset), 500), -50)\n _powerDifferential = max(min(int(differential), 150), -150)\n _arcScaling = max(min(arcScaling, 50), -15)\n setSpeed(_speedPercent)\n\n\ndef setSpeed(speed):\n global _speedPercent\n global _powerByteL\n global _powerByteR\n\n _speedPercent = int(min(max(speed, 0), 100))\n powerValue = _convertSpeedToAnalogValue(_speedPercent, _powerOffset)\n boost = round((1 - _speedPercent / 100) *\n abs(_powerDifferential)) if _speedPercent > 0 else 0\n reduction = round((_speedPercent / 100) * abs(_powerDifferential))\n if _powerDifferential > 0:\n _powerByteL = powerValue - reduction\n _powerByteR = powerValue + boost\n else:\n _powerByteL = powerValue + boost\n _powerByteR = powerValue - reduction\n\n\ndef _convertSpeedToAnalogValue(speed, offset):\n analogValue = int(speed * 1023 / 100) + offset\n return min(max(analogValue, 0), 1023)\n\n\ndef stop():\n _setMotors(0, 0, 0, 0)\n\n\ndef forward():\n _setMotors(0, _powerByteL, 0, _powerByteR)\n\n\ndef backward():\n _setMotors(1, _powerByteL, 1, _powerByteR)\n\n\ndef setServo(servoNum, angle):\n global _servoArray, _initI2C\n if _initI2C == False:\n _initPCA()\n offsetNum = servoNum\n servoNum = _servoArray[servoNum]\n i2cData = bytearray(2)\n start = 0\n angle = max(min(angle, 90), -90)\n stop = 396 + (angle + _servoOffset[offsetNum]) * 223 / 90\n i2cData[0] = 0x06 + servoNum * 4 + 2\n i2cData[1] = int(stop) + 0xff\n i2c.write(0x40, i2cData)\n\n i2cData[0] = 0x06 + servoNum*4 + 3\n i2cData[1] = int(stop) >> 8\n i2c.write(0x40, i2cData)\n\n\ndef clearServos():\n setServo(0, 0)\n setServo(1, 0)\n setServo(2, 0)\n setServo(3, 0)\n setServo(4, 0)\n sleep(500)\n\n\ndef steer(angle):\n angle = max(min(angle, 45), -45)\n setServo(1, angle)\n setServo(2, -angle)\n setServo(3, -angle)\n setServo(4, angle)\n sleep(500)\n\n\ndef setLED(pos, red, green, blue):\n _np[pos] = (red, green, blue)\n _np.show()\n\n\ndef fill(red, green, blue):\n for i in range(_nbLeds):\n _np[i] = (red, green, blue)\n _np.show()\n\n\ndef getDistance():\n trig = pin13\n echo = pin13\n\n d = 10\n\n trig.set_pull(trig.NO_PULL)\n\n for _ in range(10):\n trig.write_digital(0)\n utime.sleep_us(2)\n trig.write_digital(1)\n utime.sleep_us(10)\n trig.write_digital(0)\n\n duration = _pulse_in(echo, 1, _MAX_CM_DISTANCE * _CM_PER_MICROSECOND)\n if duration > 0:\n d = duration\n break\n\n return round(d / _CM_PER_MICROSECOND)\n\n\ndef calibrateServo(pos, angleDifferential):\n global _servoOffset\n\n angleDifferential = max(min(angleDifferential, 90), -90)\n _servoOffset[pos] = angleDifferential\n clearServos()\n",
|
|
2340
|
+
mbrobotmot: '# mbrobotmot.py\n# Version 1.2, Aug 9, 2019\n\nimport gc\nfrom microbit import i2c, pin1, pin2, pin8, pin12, pin13, pin14, sleep\nimport machine\n\nclass Motor:\n def __init__(self, id):\n self._id = 2 * id\n\n def rotate(self, s):\n v = abs(s)\n if s > 0:\n self._w(0, v) \n elif s < 0:\n self._w(1, v) \n else: \n self._w(0, 0) \n \n\n def _w(self, d, s):\n try:\n i2c.write(0x10, bytearray([self._id, d, s]))\n except:\n print("Please switch on mbRobot!")\n while True:\n pass\n\ndelay = sleep\n\ndef getDistance():\n pin1.write_digital(1)\n pin1.write_digital(0)\n p = machine.time_pulse_us(pin2, 1, 50000)\n cm = int(p / 58.2 + 0.5)\n return cm if cm > 0 else 255\n\ndef setLED(on):\n pin8.write_digital(on)\n pin12.write_digital(on)\n\npin2.set_pull(pin2.NO_PULL)\nirLeft = pin13\nirRight = pin14\nledLeft = pin8\nledRight = pin12\nmotL = Motor(0)\nmotR = Motor(1)',
|
|
2341
|
+
controller: '# Controller v1.0, Date 21.06.24\n# Enables easy use of the DFRobot Micro:bit GamePad by simulating buttons the same way as the microbit internal ones.\nfrom microbit import run_every, pin13, pin14, pin15, pin16, pin8, pin1, pin2, pin12, button_a, button_b, sleep\n\nclass _Controller_Button:\n #""" Simulated Button for regular pins with same functionality as the micro:bit buttons a and b."""\n \n def __init__(self, pin):\n # """create a new simulated button from a digital pin\n\n # Parameter:\n # pin (MicroBitDigitalPin): pin to use as button\n # """\n self._pin = pin\n self.previous_state = 0\n self._press_count = 0\n self._pressed_before = False\n pin.set_pull(pin.PULL_UP)\n \n def _update_state(self):\n # """ internally update the buttons state."""\n current_state = 1 - self._pin.read_digital()\n \n if current_state == 1 and self.previous_state == 0: # Low -> High\n self._press_count += 1\n self._pressed_before = True\n \n self.previous_state = current_state\n\n def is_pressed(self):\n # """ check if the button is currently pressed.\n\n # Returns:\n # bool: if button is currently pressed.\n # """\n return False if self._pin.read_digital() else True\n \n def was_pressed(self):\n # """ check if button was pressed down before. Resets upon call. \n\n # Returns:\n # bool: True if button was pressed down after last call of this function.\n # """\n state = self._pressed_before\n self._pressed_before = False\n return state\n \n def get_presses(self):\n # """ get the amount of button presses since last call.\n \n # Returns:\n # int: number of button (down) presses since last call of this function.\n # """\n count = self._press_count\n self._press_count = 0\n return count\n\nclass _Controller_Analog_Stick:\n # """ class that encapsulates the right analog stick of the controller. \n # This includes the turning in an xy-plane and the pressing (z-button)."""\n\n def __init__(self, pinX, pinY, pinZ):\n # """ creates a new analog-stick given its input pins.\n \n # Parameters:\n # pinX (MicroBitAnalogDigitalPin): The pin to use for the x-axis (left-right).\n # pinY (MicroBitAnalogDigitalPin): The pin to use for the y-axis (up-down).\n # pinZ (MicroBitDigitalPin): The pin to use for the z-button.\n # """\n self.pin_x = pinX\n self.pin_y = pinY\n self.button_z = _Controller_Button(pinZ)\n self.dead_zone = 0.01\n self.center_x = 0\n self.center_y = 0\n self.min_x = -1\n self.max_x = 1\n self.min_y = -1\n self.max_y = 1\n \n def calibrate(self, dead_zone, \n center_x=0.0, center_y=0.0, \n x_min=-1.0, x_max=1.0, \n y_min=-1.0, y_max=1.0):\n # """ calibrate the analog-stick to remove drift (nonzero values even in resting position) \n # and rescale to full [-1,1] range for the xy-inputs.\n \n # After calibration, the analog-stick \n # - does not react (returns 0) until it exceeds a value above the "dead_zone" range.\n # - returns values in the range [-1, 1] when pushed to the extremes and 0 at rest.\n \n # Parameters:\n # dead_zone (int): range of xy-values (negative too) that should be mapped to 0. \n # Default: 0.01 \n # center_x (int): returned value for get_x, when the stick is not moved (before calibration). \n # Default: 0.0\n # center_y (int): returned value for get_y, when the stick is not moved (before calibration). \n # Default: 0.0\n # x_min (int): minimal possible value for x (before calibration). Default: -1.0\n # x_max (int): maximal possible value for x (before calibration). Default: 1.0\n # y_min (int): minimal possible value for y (before calibration). Default: -1.0\n # y_max (int): maximal possible value for y (before calibration). Default: 1.0\n # """\n self.dead_zone = min(0.2, max(0.01,dead_zone))\n self.center_x = min(0.4, max(-0.4,center_x))\n self.center_y = min(0.5, max(-0.5,center_y))\n self.min_x = min(-0.5, max(-1.0,x_min))\n self.max_x = min(1.0, max(0.5,x_max))\n self.min_y = min(-0.5, max(-1.0,y_min))\n self.max_y = min(1.0, max(0.5,y_max))\n\n def _remap(self, value, center, v_min, v_max):\n # """ internal function that applies remapping of the values according to the calibration.\n # The returned value will live in a possible space from [-1,1] with center at 0.\n \n # Parameters:\n # value (float): the input value to remap to the desired range.\n # center (float): the resting value before calibration.\n # v_min (float): the minimally attainable value before calibration.\n # v_max (float): the maximally attainable value before calibration.\n\n # Returns:\n # v (float): remapped parameter value according to calibration.\n # """\n v = value - 1.0 - center\n if abs(v) - self.dead_zone/2.0 > 0.0:\n if v < 0.0:\n v = v / abs(v_min - center)\n else:\n v = v / abs(v_max - center)\n v = min(1.0, max(-1.0, v))\n v = 0.0 if abs(v) <= self.dead_zone else v\n return v\n\n def get_x(self):\n # """ get the current x-axis value of the analog stick.\n \n # Returns:\n # (float): position of x-axis in range [-1,1] (left to right). 0 is resting position. (after calibration)\n # """\n v_raw = (self.pin_x.read_analog() / 512.0)\n v = self._remap(v_raw, self.center_x, self.min_x, self.max_x)\n return v\n\n def get_y(self):\n # """ get the current y-axis value of the analog stick.\n \n # Returns:\n # (float): position of y-axis in range [-1,1] (down to up). 0 is resting position. (after calibration)\n # """\n v_raw = (self.pin_y.read_analog() / 512.0)\n v = self._remap(v_raw, self.center_y, self.min_y, self.max_y)\n return v\n\n def get_z(self):\n # """ get the current z-button state as an int. 1 = pressed, 0 = not pressed. """\n return 1 if self.button_z.is_pressed() else 0\n\n def is_pressed(self):\n # """ returns if the z-button is currently pressed as a bool."""\n return self.button_z.is_pressed()\n \n def was_pressed(self):\n # """ returns if the z-button was pressed (down) before the last call to this function."""\n return self.button_z.was_pressed()\n \n def get_presses(self):\n # """ returns how often the z-button was pressed (down) before the last call to this function."""\n return self.button_z.get_presses()\n\n# Controller LED and vibration motor\ndef vibrate(state):\n # """ Set the state of the controllers vibration motor. 0=Off, 1=On.\n # Also affects the controllers blue LED next to the motor. """\n pin12.write_digital(state)\n\n# Left side of controller (black joystick)\njoystick = _Controller_Analog_Stick(pin1, pin2, pin8)\n# also make the joystick button accessible independently.\nbutton_z = joystick.button_z\n\n# Right side of controller (4 colored buttons)\nbutton_c = _Controller_Button(pin13)\nbutton_green = button_c\nbutton_d = _Controller_Button(pin14)\nbutton_yellow = button_d\nbutton_e = _Controller_Button(pin15)\nbutton_red = button_e\nbutton_f = _Controller_Button(pin16)\nbutton_blue = button_f\n\n# Back side of controller (2 white buttons, same as default buttons)\ntrigger_left = button_a\ntrigger_right = button_b\n\n\ndef _update_controller_buttons():\n # """ function to be called repeatedly that updates the state of all the controllers buttons. \n # It is necessary to run this function regularly to simulate the standard microbit button behaviour. """\n global button_c\n global button_d\n global button_e\n global button_f\n global joystick\n\n button_c._update_state()\n button_d._update_state()\n button_e._update_state()\n button_f._update_state()\n joystick.button_z._update_state()\n\nrun_every(_update_controller_buttons, days=0, h=0, min=0, s=0, ms=33) # ~30 Updates per Second\n',
|
|
2342
|
+
mbxgo: "from microbit import *\n\n_speed = 50\n_XGOInit = False\n_TX = pin14\n_RX = pin13\n\n\ndef checkInit(func):\n def wrapper(*args, **kwargs):\n global _XGOInit\n if not _XGOInit:\n init_xgo_serial(_TX, _RX)\n return func(*args, **kwargs)\n return wrapper\n\n\ndef _map(speed, in_min, in_max, out_min, out_max):\n return (speed - in_min) * (out_max - out_min) // (in_max - in_min) + out_min\n\n\n@checkInit\ndef _move(direction, speed):\n move_buffer = bytearray(9)\n move_buffer[0] = 0x55\n move_buffer[1] = 0x00\n move_buffer[2] = 0x09\n move_buffer[3] = 0x00\n move_buffer[7] = 0x00\n move_buffer[8] = 0xAA\n\n speed = max(0, min(100, speed))\n\n if direction == 0:\n move_buffer[4] = 0x30\n move_buffer[5] = _map(speed, 0, 100, 128, 255)\n elif direction == 1:\n move_buffer[4] = 0x30\n move_buffer[5] = _map(speed, 0, 100, 128, 0)\n elif direction == 2:\n move_buffer[4] = 0x31\n move_buffer[5] = _map(speed, 0, 100, 128, 0)\n elif direction == 3:\n move_buffer[4] = 0x31\n move_buffer[5] = _map(speed, 0, 100, 128, 255)\n\n move_buffer[6] = ~(0x09 + 0x00 + move_buffer[4] + move_buffer[5]) & 0xFF\n\n uart.write(move_buffer)\n\n\n@checkInit\ndef clampX(milimeters=50):\n clampBuffer = bytearray(9)\n clampBuffer[0] = 0x55\n clampBuffer[1] = 0x00\n clampBuffer[2] = 0x09\n clampBuffer[3] = 0x00\n clampBuffer[4] = 0x73\n clampBuffer[7] = 0x00\n clampBuffer[8] = 0xAA\n\n clampBuffer[5] = milimeters\n clampBuffer[6] = ~(0x09 + 0x00 + 0x73 + clampBuffer[5]) & 0xFF\n\n uart.write(clampBuffer)\n sleep(1000)\n\n\n@checkInit\ndef clampZ(milimeters=50):\n clampBuffer = bytearray(9)\n clampBuffer[0] = 0x55\n clampBuffer[1] = 0x00\n clampBuffer[2] = 0x09\n clampBuffer[3] = 0x00\n clampBuffer[4] = 0x74\n clampBuffer[7] = 0x00\n clampBuffer[8] = 0xAA\n\n clampBuffer[5] = milimeters\n clampBuffer[6] = ~(0x09 + 0x00 + 0x74 + clampBuffer[5]) & 0xFF\n\n uart.write(clampBuffer)\n sleep(1000)\n\n\n@checkInit\ndef clamp(force):\n clampBuffer = bytearray(9)\n clampBuffer[0] = 0x55\n clampBuffer[1] = 0x00\n clampBuffer[2] = 0x09\n clampBuffer[3] = 0x00\n clampBuffer[4] = 0x71\n clampBuffer[7] = 0x00\n clampBuffer[8] = 0xAA\n\n clampBuffer[5] = force\n clampBuffer[6] = ~(0x09 + 0x00 + 0x71 + clampBuffer[5]) & 0xFF\n\n uart.write(clampBuffer)\n sleep(1000)\n\n\ndef init_xgo_serial(tx_pin, rx_pin, baudrate=115200):\n global _XGOInit\n uart.init(baudrate=baudrate, tx=tx_pin, rx=rx_pin)\n init_action()\n _XGOInit = True\n\n\ndef init_action():\n commands_buffer = bytearray(9)\n commands_buffer[0] = 0x55\n commands_buffer[1] = 0x00\n commands_buffer[2] = 0x09\n commands_buffer[3] = 0x00\n commands_buffer[4] = 0x3E\n commands_buffer[5] = 0xFF\n commands_buffer[6] = ~(0x09 + 0x00 + 0x3E + 0xFF) & 0xFF\n commands_buffer[7] = 0x00\n commands_buffer[8] = 0xAA\n\n uart.write(commands_buffer)\n sleep(2000)\n\n\n@checkInit\ndef action(id):\n commands_buffer = bytearray(9)\n commands_buffer[0] = 0x55\n commands_buffer[1] = 0x00\n commands_buffer[2] = 0x09\n commands_buffer[3] = 0x00\n commands_buffer[4] = 0x3E\n commands_buffer[5] = id\n commands_buffer[6] = ~(0x09 + 0x00 + 0x3E + id) & 0xFF\n commands_buffer[7] = 0x00\n commands_buffer[8] = 0xAA\n\n uart.write(commands_buffer)\n sleep(2000)\n\n\ndef changeInit(tx, rx):\n global _TX\n global _RX\n _TX = tx\n _RX = rx\n init_xgo_serial(_TX, _RX)\n\n\n@checkInit\ndef setSpeed(speed):\n global _speed\n _speed = speed\n\n\ndef forward():\n _move(0, _speed)\n\n\ndef backward():\n _move(1, _speed)\n\n\ndef left():\n _move(2, _speed)\n\n\ndef right():\n _move(3, _speed)\n",
|
|
2343
|
+
mbglow: '# mbglow.py\n# V1.5, June 30, 2018\n# Module for a crash course in Python\n# An implementation of a glowbug, firefly (Leuchtkaefer)\n\nfrom microbit import *\n\n_x = 0\n_y = 0\n_dir = 0\n_speed = 50\n_trace = True\n_visible = False\n\ndef makeGlow():\n global _visible\n display.set_pixel(2, 2, 9)\n _visible = True\n\ndef clear():\n display.clear()\n \ndef forward(): \n _forward(1)\n\ndef back(): \n _forward(-1)\n \ndef right(angle):\n global _dir\n _dir = (_dir + angle) % 360\n\ndef left(angle):\n right(-angle)\n\ndef setPos(x, y):\n global _x, _y\n _x = x\n _y = y\n _render()\n\ndef getPos():\n return _x, _y\n\ndef setSpeed(speed):\n global _speed\n _speed = speed\n \ndef showTrace(enable):\n global _trace\n _trace = enable \n\ndef isLit():\n return (display.get_pixel(_x + 2, 4 - (_y + 2)) == 9)\n \ndef _forward(s):\n global _x, _y\n sleep(2000 - _speed * 20)\n d = _dir // 45\n if d in [1, 2, 3]: \n _x += s\n if d in [5, 6, 7]: \n _x -= s\n if d in [0, 1, 7]: \n _y += s\n if d in [3, 4, 5]: \n _y -= s\n _render()\n\ndef _render():\n if not _visible:\n print("Use \\"makeGlow()\\" to create a Glow.")\n raise Exception("Glow not initialized.")\n if not _trace:\n display.clear()\n if -2 <= _x <= 2 and -2 <= _y <= 2: \n display.set_pixel(_x + 2, 4 - (_y + 2), 9)',
|
|
2344
|
+
mbthetabot: "from microbit import *\nfrom utime import ticks_us, sleep_us\nfrom neopixel import *\n\n_speedPercent = 50\n_powerByteL = 40\n_powerByteR = 40\n_powerBytesLUT = bytes(b'\\x00\\x0b\\x0b\\x0c\\x0c\\x0d\\x0d\\x0d\\x0e\\x0e\\x0f\\x0f\\x0f\\x10\\x10\\x11\\x11\\x11\\x12\\x12\\x13\\x13\\x13\\x14\\x14\\x15\\x15\\x15\\x16\\x16\\x17\\x17\\x17\\x18\\x19\\x1a\\x1b\\x1b\\x1c\\x1d\\x1e\\x1f\\x20\\x21\\x22\\x23\\x23\\x24\\x25\\x26\\x27\\x28\\x29\\x2a\\x2b\\x2b\\x2c\\x2d\\x2e\\x2f\\x30\\x31\\x32\\x33\\x34\\x36\\x38\\x3a\\x3c\\x3f\\x41\\x44\\x46\\x49\\x4c\\x4f\\x53\\x56\\x5a\\x5e\\x62\\x67\\x6b\\x70\\x75\\x7b\\x81\\x87\\x8d\\x94\\x9b\\xa3\\xab\\xb4\\xbd\\xc6\\xd0\\xdb\\xe6\\xf2\\xff')\n\n_powerOffset = 0\n_powerDifferential = 0\n_arcScaling = 0\n\n_ADDR_ATM = 0x22\n_nbLeds = 14\n\n\ndef _setMotors(dirL, powerL, dirR, powerR):\n pinsL = (pin14, pin13)\n pinsR = (pin16, pin15)\n\n pinsL[dirL].write_analog(powerL)\n pinsL[1 - dirL].write_analog(0)\n\n pinsR[dirR].write_analog(powerR)\n pinsR[1 - dirR].write_analog(0)\n\n\ndef _setSingleMotor(side, direction, power):\n if side == 0:\n pins = (pin14, pin13)\n elif side == 2:\n pins = (pin16, pin15)\n\n pins[direction].write_analog(power)\n pins[1 - direction].write_analog(0)\n\n\ndef _getArcBytes(r):\n outerSpeed = _speedPercent\n rCm = int(r * 100)\n threshold = outerSpeed - max(rCm + 20, 40)\n if threshold <= 0:\n outerSpeed = min(max(rCm + 40, 40), 100)\n reducedSpeed = 0\n if rCm >= 4:\n flattening = (100 - outerSpeed) // 2\n reducedSpeed = (rCm * 10 - 35) / \\\n (rCm * (11 + (_arcScaling-4)/10) + 90 + flattening)\n reducedSpeed = reducedSpeed * outerSpeed\n innerByte = _getPowerByteLUT(int(reducedSpeed), 0)\n outerByte = _getPowerByteLUT(int(outerSpeed), 0)\n return (innerByte, outerByte)\n\n\ndef calibrate(offset, differential=0, arcScaling=0):\n global _powerDifferential\n global _powerOffset\n global _arcScaling\n\n _powerOffset = max(min(int(offset), 150), -10)\n _powerDifferential = max(min(int(differential), 150), -150)\n _arcScaling = max(min(arcScaling, 50), -15)\n setSpeed(_speedPercent)\n\n\ndef setSpeed(speed):\n global _speedPercent\n global _powerByteL\n global _powerByteR\n\n _speedPercent = int(min(max(speed, 0), 100))\n powerByte = _getPowerByteLUT(_speedPercent, _powerOffset)\n boost = round((1 - _speedPercent / 100) *\n abs(_powerDifferential)) if _speedPercent > 0 else 0\n reduction = round((_speedPercent / 100) * abs(_powerDifferential))\n if _powerDifferential > 0:\n _powerByteL = powerByte - reduction\n _powerByteR = powerByte + boost\n else:\n _powerByteL = powerByte + boost\n _powerByteR = powerByte - reduction\n\n\ndef _getPowerByteLUT(speed, offset):\n speedIndex = int(speed * (len(_powerBytesLUT) - 1) / 100)\n return min(_powerBytesLUT[speedIndex] + offset, 1023)\n\n\ndef stop():\n _setMotors(0, 0, 0, 0)\n\n\ndef forward():\n _setMotors(0, _powerByteL, 0, _powerByteR)\n\n\ndef backward():\n _setMotors(1, _powerByteL, 1, _powerByteR)\n\n\ndef left():\n _setMotors(1, _powerByteL, 0, _powerByteR)\n\n\ndef right():\n _setMotors(0, _powerByteL, 1, _powerByteR)\n\n\ndef rightArc(radius):\n inner, outer = _getArcBytes(radius)\n _setMotors(0, outer, 0, inner)\n\n\ndef leftArc(radius):\n inner, outer = _getArcBytes(radius)\n _setMotors(0, inner, 0, outer)\n\n\ndef getDistance():\n pin12.write_digital(1)\n sleep_us(10)\n pin12.write_digital(0)\n pin12.set_pull(pin15.NO_PULL)\n while pin12.read_digital() == 0:\n pass\n start = ticks_us()\n while pin12.read_digital() == 1:\n pass\n end = ticks_us()\n echo = end-start\n distance = int(0.01715 * echo)\n return distance\n\n\ndef setLED(position, red, green, blue):\n global _ADDR_ATM\n i2c_data = bytearray(5)\n\n i2c_data[0] = 1\n\n i2c_data[1] = position\n\n i2c_data[2] = red\n i2c_data[3] = green\n i2c_data[4] = blue\n\n i2c.write(_ADDR_ATM, i2c_data)\n\n\ndef fill(red, green, blue):\n for position in range(_nbLeds):\n setLED(position, red, green, blue)\n\n\ndef readLine(side): # 0=left 1=right\n i2c.write(_ADDR_ATM, bytearray([side + 1]), False)\n result = i2c.read(_ADDR_ATM, 2)\n result = result[0] + (result[1] << 8)\n return result\n\n\ndef readLight(side): # 0=left 1=right\n i2c.write(_ADDR_ATM, bytearray([side + 3]), False)\n result = i2c.read(_ADDR_ATM, 2)\n result = result[0] + (result[1] << 8)\n return result\n",
|
|
2345
|
+
mbrobot_legacy: 'import gc\nfrom microbit import i2c, pin1, pin2, pin8, pin12, pin13, pin14, sleep\nimport machine\n\n_axe = 0.097\ndef w(d1, d2, s1, s2):\n try:\n i2c.write(0x10, bytearray([0, d1, s1]))\n i2c.write(0x10, bytearray([2, d2, s2]))\n except:\n print("Please switch on mbRobot!")\n while True:\n pass\n \ndef setSpeed(speed):\n global _v\n if speed < 20:\n _v = speed + 5\n else:\n _v = speed \n\ndef forward():\n w(0, 0, _v, _v)\n\ndef backward():\n w(1, 1, _v, _v)\n \ndef stop():\n w(0, 0, 0, 0)\n \ndef right():\n w(0 if _v > 0 else 1, 1 if _v > 0 else 0, int(_v * 0.9), int(_v * 0.9)) \n\ndef left():\n w(1 if _v > 0 else 0, 0 if _v > 0 else 1, int(_v * 0.9) , int(_v * 0.9))\n\ndef rightArc(r):\n v = abs(_v)\n if r < _axe:\n v1 = 0\n else: \n f = (r - _axe) / (r + _axe) * (1 - v * v / 200000) \n v1 = int(f * v)\n if _v > 0:\n w(0, 0, v, v1)\n else:\n w(1, 1, v1, v)\n\ndef leftArc(r):\n v = abs(_v)\n if r < _axe:\n v1 = 0\n else:\n f = (r - _axe) / (r + _axe) * (1 - v * v / 200000) \n v1 = int(f * v)\n if _v > 0:\n w(0, 0, v1, v)\n else:\n w(1, 1, v, v1)\n\nexit = stop\ndelay = sleep\n\ndef getDistance():\n pin1.write_digital(1)\n pin1.write_digital(0)\n p = machine.time_pulse_us(pin2, 1, 50000)\n cm = int(p / 58.2 + 0.5)\n return cm if cm > 0 else 255\n\ndef setLED(on):\n pin8.write_digital(on)\n pin12.write_digital(on)\n \ndef setServo(S, Angle):\n if S == "S1":\n Servo = 0x14\n if S == "S2":\n Servo = 0x15\n buf = Servo, Angle\n i2c.write(0x10, bytes(buf)) \n\npin2.set_pull(pin2.NO_PULL)\n_v = 50 # entspricht default 50\nirLeft = pin13\nirRight = pin14\nledLeft = pin8\nledRight = pin12\n\nforward()',
|
|
2346
|
+
mbalarm: "# mbalarm.py\n\nimport music\n\n_m = ['c6:1', 'r', 'c6,1', 'r', 'r', 'r']\n\ndef setAlarm(on):\n if on:\n music.play(_m, wait = False, loop = True) \n else:\n music.stop()\n \n \ndef beep():\n music.pitch(2000, 200, wait = False)",
|
|
2347
|
+
mbminibit: "from microbit import *\nfrom neopixel import *\nimport utime\n\n_nbLeds = 4\n_np = NeoPixel(pin13, _nbLeds)\n\n_speedPercent = 50\n_powerLeft = 90\n_powerRight = 90\n\n_powerOffset = 0\n_powerDifferential = 0\n_arcScaling = 0\n\n_MAX_CM_DISTANCE = 500\n_CM_PER_MICROSECOND = 29.1\n\n\ndef _setMotors(dirL, powerL, dirR, powerR):\n pin8.write_analog(powerL if dirL == 1 else 0)\n pin12.write_analog(0 if dirL == 1 else powerL)\n pin16.write_analog(0 if dirR == 1 else powerR)\n pin14.write_analog(powerR if dirR == 1 else 0)\n\n\ndef _pulse_in(pin, value, timeout):\n start_time = utime.ticks_us()\n while pin.read_digital() != value:\n if utime.ticks_diff(utime.ticks_us(), start_time) > timeout:\n return 0\n start_time = utime.ticks_us()\n while pin.read_digital() == value:\n if utime.ticks_diff(utime.ticks_us(), start_time) > timeout:\n return 0\n return utime.ticks_diff(utime.ticks_us(), start_time)\n\n\ndef _getArcBytes(r):\n outerSpeed = _speedPercent\n if r > 0:\n innerSpeed = outerSpeed * max(0.2, min(1, 1 - r / 100))\n else:\n innerSpeed = 0\n\n innerByte = _convertSpeedToAnalogValue(int(innerSpeed), 0)\n outerByte = _convertSpeedToAnalogValue(int(outerSpeed), 0)\n return (innerByte, outerByte)\n\n\ndef _convertSpeedToAnalogValue(speed, offset):\n analogValue = int(speed * 255 / 100) + offset\n return min(max(analogValue, 0), 255)\n\n\ndef calibrate(offset, differential=0, arcScaling=0):\n global _powerDifferential\n global _powerOffset\n global _arcScaling\n _powerOffset = max(min(int(offset), 100), -10)\n _powerDifferential = max(min(int(differential), 150), -150)\n _arcScaling = max(min(arcScaling, 50), -15)\n setSpeed(_speedPercent)\n\n\ndef setSpeed(speed):\n global _speedPercent\n global _powerLeft\n global _powerRight\n _speedPercent = int(min(max(speed, 0), 100))\n powerByte = _convertSpeedToAnalogValue(_speedPercent, _powerOffset)\n boost = round((1 - _speedPercent / 100) *\n abs(_powerDifferential)) if _speedPercent > 0 else 0\n reduction = round((_speedPercent / 100) * abs(_powerDifferential))\n if _powerDifferential > 0:\n _powerLeft = powerByte - reduction\n _powerRight = powerByte + boost\n else:\n _powerLeft = powerByte + boost\n _powerRight = powerByte - reduction\n\n\ndef stop():\n _setMotors(0, 0, 0, 0)\n\n\ndef forward():\n _setMotors(0, _powerLeft, 0, _powerRight)\n\n\ndef backward():\n _setMotors(1, _powerLeft, 1, _powerRight)\n\n\ndef left():\n _setMotors(1, _powerLeft, 0, _powerRight)\n\n\ndef right():\n _setMotors(0, _powerLeft, 1, _powerRight)\n\n\ndef rightArc(radius):\n inner, outer = _getArcBytes(radius)\n _setMotors(0, outer, 0, inner)\n\n\ndef leftArc(radius):\n inner, outer = _getArcBytes(radius)\n _setMotors(0, inner, 0, outer)\n\n\ndef setLED(pos, red, green, blue):\n _np[pos] = (red, green, blue)\n _np.show()\n\n\ndef fill(red, green, blue):\n for i in range(_nbLeds):\n _np[i] = (red, green, blue)\n _np.show()\n\n\ndef getDistance():\n trig = pin15\n echo = pin15\n d = 10\n trig.set_pull(trig.NO_PULL)\n for _ in range(10):\n trig.write_digital(0)\n utime.sleep_us(2)\n trig.write_digital(1)\n utime.sleep_us(10)\n trig.write_digital(0)\n duration = _pulse_in(echo, 1, _MAX_CM_DISTANCE * _CM_PER_MICROSECOND)\n if duration > 0:\n d = duration\n break\n return round(d / _CM_PER_MICROSECOND)\n",
|
|
2348
|
+
mbrobot_plusV2: `# mbrobot_plusV2.py
|
|
2349
|
+
# Date 10/09/24
|
|
2350
|
+
|
|
2351
|
+
from microbit import i2c, pin0, pin1, pin2, pin13, pin14, pin15, sleep
|
|
2352
|
+
import gc
|
|
2353
|
+
import machine
|
|
2354
|
+
import music
|
|
2355
|
+
import neopixel
|
|
2356
|
+
|
|
2357
|
+
# Motor state
|
|
2358
|
+
_speedPercent = 50
|
|
2359
|
+
_powerByteL = 50
|
|
2360
|
+
_powerByteR = 50
|
|
2361
|
+
_motorState = bytearray(5)
|
|
2362
|
+
_powerBytesLUT = bytes(b'\\x00\\x0f\\x10\\x10\\x11\\x12\\x12\\x13\\x13\\x14\\x15\\x15\\x16\\x16\\x17\\x18\\x18\\x19\\x19\\x1a\\x1b\\x1b\\x1c\\x1c\\x1d\\x1e\\x1e\\x1f\\x1f\\x20\\x21\\x21\\x22\\x22\\x23\\x24\\x24\\x25\\x25\\x26\\x27\\x29\\x2a\\x2b\\x2c\\x2d\\x2e\\x2f\\x30\\x31\\x32\\x33\\x34\\x35\\x36\\x37\\x38\\x39\\x3a\\x3b\\x3b\\x3c\\x3e\\x3f\\x41\\x43\\x45\\x47\\x49\\x4b\\x4d\\x50\\x52\\x55\\x58\\x5b\\x5e\\x61\\x65\\x69\\x6d\\x71\\x75\\x7a\\x7f\\x84\\x89\\x8f\\x95\\x9b\\xa2\\xa9\\xb1\\xb9\\xc1\\xca\\xd3\\xdd\\xe8\\xf3\\xff')
|
|
2363
|
+
|
|
2364
|
+
# Calibration data
|
|
2365
|
+
_powerOffset = 0
|
|
2366
|
+
_powerDifferential = 0
|
|
2367
|
+
_arcScaling = 0
|
|
2368
|
+
_servoMinPulse = 25
|
|
2369
|
+
_servoMaxPulse = 131
|
|
2370
|
+
|
|
2371
|
+
# Signaling objects and buffers
|
|
2372
|
+
_ledState = bytearray(b'\\x0B\\0\\0')
|
|
2373
|
+
_underglowNP = neopixel.NeoPixel(pin15, 4)
|
|
2374
|
+
np_rgb_pixels = _underglowNP
|
|
2375
|
+
_alarmSequence = ['c5:1', 'r', 'c5,1', 'r:3']
|
|
2376
|
+
|
|
2377
|
+
_UNCONNECTEDERRORMSG = "Please connect to Maqueen robot and switch it on."
|
|
2378
|
+
|
|
2379
|
+
# Utility functions
|
|
2380
|
+
|
|
2381
|
+
|
|
2382
|
+
def _setMotors(dirL, powerL, dirR, powerR):
|
|
2383
|
+
# """Write Motor State via i2c
|
|
2384
|
+
|
|
2385
|
+
# Parameters:
|
|
2386
|
+
# dirL (0/1): Direction of left Wheel. 0=forward, 1=backward
|
|
2387
|
+
# powerL (int): Power of left Wheel in range [0,255].
|
|
2388
|
+
# dirR (0/1): Direction of right Wheel. 0=forward, 1=backward
|
|
2389
|
+
# powerR (int): Power of right Wheel in range [0,255].
|
|
2390
|
+
#
|
|
2391
|
+
# raises:
|
|
2392
|
+
# RuntimeError: if Robot is switched off or unconnected. (replaces ENODEV error)
|
|
2393
|
+
# """
|
|
2394
|
+
global _motorState
|
|
2395
|
+
_motorState[1] = dirL
|
|
2396
|
+
_motorState[2] = powerL
|
|
2397
|
+
_motorState[3] = dirR
|
|
2398
|
+
_motorState[4] = powerR
|
|
2399
|
+
try:
|
|
2400
|
+
i2c.write(0x10, _motorState)
|
|
2401
|
+
except:
|
|
2402
|
+
raise RuntimeError(_UNCONNECTEDERRORMSG)
|
|
2403
|
+
|
|
2404
|
+
|
|
2405
|
+
def _setSingleMotor(side, dir, power):
|
|
2406
|
+
# """Write Motor State of a single Motor via i2c
|
|
2407
|
+
|
|
2408
|
+
# Parameters:
|
|
2409
|
+
# side (0/2): Selection of the Wheel. 0=left, 2=right
|
|
2410
|
+
# dir (0/1): Direction for that Wheel. 0=forward, 1=backward
|
|
2411
|
+
# power (int): Power for that Wheel in range [0,255].
|
|
2412
|
+
#
|
|
2413
|
+
# raises:
|
|
2414
|
+
# RuntimeError: if Robot is switched off or unconnected. (replaces ENODEV error)
|
|
2415
|
+
# """
|
|
2416
|
+
global _motorState
|
|
2417
|
+
_motorState[1 + side] = dir
|
|
2418
|
+
_motorState[2 + side] = power
|
|
2419
|
+
try:
|
|
2420
|
+
i2c.write(0x10, _motorState)
|
|
2421
|
+
except:
|
|
2422
|
+
raise RuntimeError(_UNCONNECTEDERRORMSG)
|
|
2423
|
+
|
|
2424
|
+
# _getPowerByte was used to create LookUpTable (_powerBytesLUT) used in _getPowerByteLUT to reduce Memory Leaks.
|
|
2425
|
+
# def _getPowerByte(speed, offset):
|
|
2426
|
+
# # """Computes the power value for a given speed in %.
|
|
2427
|
+
# # It accouts for the nonlinearity of motor strength.
|
|
2428
|
+
# # This is the original function that generates the LUT.
|
|
2429
|
+
#
|
|
2430
|
+
# # Parameter:
|
|
2431
|
+
# # speed (number): Desired speed of the Robot in %. Range [0,100].
|
|
2432
|
+
# # offset (int): basic power offset to add to the function.
|
|
2433
|
+
# # Returns:
|
|
2434
|
+
# # int: Power in range [0,255] to write to the motors via i2c.
|
|
2435
|
+
# # """
|
|
2436
|
+
# if speed <= 0:
|
|
2437
|
+
# return 0
|
|
2438
|
+
# elif speed < 40:
|
|
2439
|
+
# return int(0.6 * speed + 15 + offset)
|
|
2440
|
+
# elif speed < 60:
|
|
2441
|
+
# return int(speed + offset)
|
|
2442
|
+
# elif speed < 100:
|
|
2443
|
+
# return int(min(1.05546 ** speed + 34 + offset, 255))
|
|
2444
|
+
# else:
|
|
2445
|
+
# return 255
|
|
2446
|
+
|
|
2447
|
+
|
|
2448
|
+
def _getPowerByteLUT(speed, offset):
|
|
2449
|
+
# """Lookup Table version of _getPowerByte"""
|
|
2450
|
+
return min(_powerBytesLUT[speed] + offset, 255)
|
|
2451
|
+
|
|
2452
|
+
|
|
2453
|
+
def _getArcBytes(r):
|
|
2454
|
+
# """Computes the power bytes to drive an arc.
|
|
2455
|
+
|
|
2456
|
+
# Parameter:
|
|
2457
|
+
# r (float): Radius in meters of the desired Arc.
|
|
2458
|
+
# Measured from the center of the axle.
|
|
2459
|
+
# Returns:
|
|
2460
|
+
# (int, int): Power byte values for each motor.
|
|
2461
|
+
# First the outer Wheels byte [0,255],
|
|
2462
|
+
# then the inner Wheels byte [0,255].
|
|
2463
|
+
# """
|
|
2464
|
+
rmm = int(r * 100) # radius in mm
|
|
2465
|
+
outerSpeed = _speedPercent
|
|
2466
|
+
# adjust outer speed for unhealthy values
|
|
2467
|
+
if outerSpeed < 25:
|
|
2468
|
+
outerSpeed = 25
|
|
2469
|
+
speedFix = min(abs(outerSpeed - 70), 20) / 20
|
|
2470
|
+
reducedSpeed = 0
|
|
2471
|
+
if rmm > 5:
|
|
2472
|
+
# formula derived from data and simplified
|
|
2473
|
+
n = outerSpeed * (3 * _arcScaling - outerSpeed - 9 * rmm + 220)
|
|
2474
|
+
d = -14 * _arcScaling + outerSpeed - 200 + 3 * outerSpeed - 10 * rmm - 290
|
|
2475
|
+
reducedSpeed = int(n/d)
|
|
2476
|
+
if reducedSpeed < 2: # fix values at low radii (negative values too)
|
|
2477
|
+
reducedSpeed = 2 if rmm > 15 else 1
|
|
2478
|
+
innerByte = _getPowerByteLUT(int(reducedSpeed), 0)
|
|
2479
|
+
outerByte = _getPowerByteLUT(int(outerSpeed), 0)
|
|
2480
|
+
return (innerByte, outerByte)
|
|
2481
|
+
|
|
2482
|
+
# Movement functions
|
|
2483
|
+
|
|
2484
|
+
|
|
2485
|
+
def calibrate(offset, differential=0, arcScaling=0):
|
|
2486
|
+
# """Adjust the driving behaviour of the robots
|
|
2487
|
+
|
|
2488
|
+
# Parameters:
|
|
2489
|
+
# offset (int): Offsets the minimal power of the motors.
|
|
2490
|
+
# Range [-10,50]. Highly affected by battery level.
|
|
2491
|
+
# Adjust this value until it starts moving at speed 1%.
|
|
2492
|
+
# differential (int, optional): Adjusts power difference
|
|
2493
|
+
# of left and right Wheel. Range [-150, 150].
|
|
2494
|
+
# Varies unpredictably with different speeds.
|
|
2495
|
+
# If a Robot steers left when driving forward: negative value
|
|
2496
|
+
# If a Robot steers right when driving forward: positive value
|
|
2497
|
+
# Perfectly straight driving Robots can leave this at 0.
|
|
2498
|
+
# arcScaling (int, optional): Adjusts the radius
|
|
2499
|
+
# driven by leftArc/rightArc. Valid range [-50, 50].
|
|
2500
|
+
# If the Robots radius is too large: positive value
|
|
2501
|
+
# If the Robots radius is too small: negative value
|
|
2502
|
+
# This then adjusts all radii for this Robot, by
|
|
2503
|
+
# scaling it's internal function to the new range.
|
|
2504
|
+
# """
|
|
2505
|
+
global _powerDifferential
|
|
2506
|
+
global _powerOffset
|
|
2507
|
+
global _arcScaling
|
|
2508
|
+
_powerOffset = max(min(int(offset), 50), -14)
|
|
2509
|
+
_powerDifferential = max(min(int(differential), 150), -150)
|
|
2510
|
+
_arcScaling = max(min(arcScaling, 50), -50)
|
|
2511
|
+
setSpeed(_speedPercent)
|
|
2512
|
+
|
|
2513
|
+
|
|
2514
|
+
def setSpeed(speed):
|
|
2515
|
+
# """sets the speed for future motion
|
|
2516
|
+
|
|
2517
|
+
# Parameter:
|
|
2518
|
+
# speed (int): in Range [0,100] as % of desired velocity.
|
|
2519
|
+
# """
|
|
2520
|
+
global _speedPercent
|
|
2521
|
+
global _powerByteL
|
|
2522
|
+
global _powerByteR
|
|
2523
|
+
_speedPercent = int(min(max(speed, 0), 100))
|
|
2524
|
+
powerByte = _getPowerByteLUT(_speedPercent, _powerOffset)
|
|
2525
|
+
boost = round((1 - _speedPercent / 100) *
|
|
2526
|
+
abs(_powerDifferential)) if _speedPercent > 0 else 0
|
|
2527
|
+
reduction = round((_speedPercent / 100) * abs(_powerDifferential))
|
|
2528
|
+
if _powerDifferential > 0:
|
|
2529
|
+
_powerByteL = powerByte - reduction
|
|
2530
|
+
_powerByteR = powerByte + boost
|
|
2531
|
+
else:
|
|
2532
|
+
_powerByteL = powerByte + boost
|
|
2533
|
+
_powerByteR = powerByte - reduction
|
|
2534
|
+
|
|
2535
|
+
|
|
2536
|
+
def resetSpeed():
|
|
2537
|
+
setSpeed(50)
|
|
2538
|
+
|
|
2539
|
+
|
|
2540
|
+
def stop():
|
|
2541
|
+
_setMotors(0, 0, 0, 0)
|
|
2542
|
+
|
|
2543
|
+
|
|
2544
|
+
def forward():
|
|
2545
|
+
_setMotors(0, _powerByteL, 0, _powerByteR)
|
|
2546
|
+
|
|
2547
|
+
|
|
2548
|
+
def backward():
|
|
2549
|
+
_setMotors(1, _powerByteL, 1, _powerByteR)
|
|
2550
|
+
|
|
2551
|
+
|
|
2552
|
+
def left():
|
|
2553
|
+
_setMotors(1, _powerByteL, 0, _powerByteR)
|
|
2554
|
+
|
|
2555
|
+
|
|
2556
|
+
def right():
|
|
2557
|
+
_setMotors(0, _powerByteL, 1, _powerByteR)
|
|
2558
|
+
|
|
2559
|
+
|
|
2560
|
+
def rightArc(radius):
|
|
2561
|
+
# """radius must be given in meters."""
|
|
2562
|
+
inner, outer = _getArcBytes(radius)
|
|
2563
|
+
_setMotors(0, outer, 0, inner)
|
|
2564
|
+
|
|
2565
|
+
|
|
2566
|
+
def leftArc(radius):
|
|
2567
|
+
# """radius must be given in meters."""
|
|
2568
|
+
inner, outer = _getArcBytes(radius)
|
|
2569
|
+
_setMotors(0, inner, 0, outer)
|
|
2570
|
+
|
|
2571
|
+
|
|
2572
|
+
class Motor:
|
|
2573
|
+
def __init__(self, side):
|
|
2574
|
+
# """Create a single motor.
|
|
2575
|
+
|
|
2576
|
+
# Parameter:
|
|
2577
|
+
# side (0/2): 0=left, 2=right
|
|
2578
|
+
# """
|
|
2579
|
+
self._side = side
|
|
2580
|
+
|
|
2581
|
+
def rotate(self, speed):
|
|
2582
|
+
# """Controls rotation of this motor.
|
|
2583
|
+
|
|
2584
|
+
# Parameters:
|
|
2585
|
+
# speed (int): Desired speed in %.
|
|
2586
|
+
# Valid range [-100,100].
|
|
2587
|
+
# Negative values are for backward turning.
|
|
2588
|
+
# """
|
|
2589
|
+
speedClamped = int(min(max(abs(speed), 0), 100))
|
|
2590
|
+
power = _getPowerByteLUT(speedClamped, _powerOffset)
|
|
2591
|
+
direction = 0 if speed > 0 else 1
|
|
2592
|
+
_setSingleMotor(self._side, direction, power)
|
|
2593
|
+
|
|
2594
|
+
|
|
2595
|
+
def setServo(servo, angle):
|
|
2596
|
+
# """Moves the Servo to position angle.
|
|
2597
|
+
# Servos must be connected to the Maqueen Plus V2's "P" connectors.
|
|
2598
|
+
# They are located at the back of the robot.
|
|
2599
|
+
|
|
2600
|
+
# Parameters:
|
|
2601
|
+
# servo (str): Desired Servo Port. Either 'P0', 'P1' or 'P2'.
|
|
2602
|
+
# angle (int): Desired angle in degrees. Range [0,180].
|
|
2603
|
+
|
|
2604
|
+
# raises:
|
|
2605
|
+
# ValueError: if arguments are out of valid range
|
|
2606
|
+
# """
|
|
2607
|
+
if servo == "P0" or servo == 'S1':
|
|
2608
|
+
pin = pin0
|
|
2609
|
+
elif servo == "P1" or servo == 'S2':
|
|
2610
|
+
pin = pin1
|
|
2611
|
+
elif servo == "P2":
|
|
2612
|
+
pin = pin2
|
|
2613
|
+
else:
|
|
2614
|
+
raise ValueError("Unknown Servo. Please use 'P0', 'P1' or 'P2'.")
|
|
2615
|
+
|
|
2616
|
+
if angle < 0 or angle > 180:
|
|
2617
|
+
raise ValueError("Invalid angle. Must be between 0 and 180")
|
|
2618
|
+
|
|
2619
|
+
frac = (_servoMaxPulse - _servoMinPulse) * int(angle)
|
|
2620
|
+
offset = (frac >> 8) + (frac >> 10) + (frac >> 11) + (frac >> 12) # / 180
|
|
2621
|
+
usPulseTime = _servoMinPulse + offset # min + (max-min) * (angle / 180)
|
|
2622
|
+
pin.set_analog_period(20)
|
|
2623
|
+
pin.write_analog(usPulseTime)
|
|
2624
|
+
|
|
2625
|
+
|
|
2626
|
+
def setMinAngleVal(duty):
|
|
2627
|
+
# """ Sets the minimal pulse duty cycle for the servo.
|
|
2628
|
+
# It should match 1ms of a 20ms period, where the duty is in range [0,1024].
|
|
2629
|
+
# Theoretically: 1/20*1024 = 51. Practically: Default is 25. adjust carefully in steps of 1.
|
|
2630
|
+
#
|
|
2631
|
+
# Parameter:
|
|
2632
|
+
# duty (int): The minimal duty amount for a write_analog signal with 20ms pulse.
|
|
2633
|
+
# It should approximate 1ms. 1ms/20ms*1024 = min duty = 0-degree position for the servo.
|
|
2634
|
+
# """
|
|
2635
|
+
global _servoMinPulse
|
|
2636
|
+
_servoMinPulse = int(duty)
|
|
2637
|
+
|
|
2638
|
+
|
|
2639
|
+
def setMaxAngleVal(duty):
|
|
2640
|
+
# """ Sets the maximal pulse duty cycle for the servo.
|
|
2641
|
+
# It should match 2ms of a 20ms period, where the duty is in range [0,1024].
|
|
2642
|
+
# Theoretically: 2/20*1024 = 102. Practically: Default is 131. adjust carefully to increase range of the servo.
|
|
2643
|
+
#
|
|
2644
|
+
# Parameter:
|
|
2645
|
+
# duty (int): The maximal duty amount for a write_analog signal with 20ms pulse.
|
|
2646
|
+
# It should approximate 2ms. 2ms/20ms*1024 = max duty = 180-degree position for the servo.
|
|
2647
|
+
# """
|
|
2648
|
+
global _servoMaxPulse
|
|
2649
|
+
_servoMaxPulse = int(duty)
|
|
2650
|
+
|
|
2651
|
+
# Sensor functions
|
|
2652
|
+
|
|
2653
|
+
|
|
2654
|
+
class IRSensor:
|
|
2655
|
+
_address = bytes(b'\\x1D')
|
|
2656
|
+
|
|
2657
|
+
def __init__(self, index):
|
|
2658
|
+
# """Create a new IR sensor.
|
|
2659
|
+
|
|
2660
|
+
# Parameter:
|
|
2661
|
+
# index (int): 0=R2, 1=R1, 2=M, 3=L1, 4=L2
|
|
2662
|
+
# """
|
|
2663
|
+
self._index = index
|
|
2664
|
+
|
|
2665
|
+
def read_digital(self):
|
|
2666
|
+
# """Returns if the surface below is dark or bright.
|
|
2667
|
+
# Result can be adjusted by putting the sensor on the dark
|
|
2668
|
+
# surface and pressing the LineKey calibration button on the
|
|
2669
|
+
# Robot for a few seconds, until the LED's blink.
|
|
2670
|
+
|
|
2671
|
+
# Returns:
|
|
2672
|
+
# 0 if the surface is dark. No light was reflected (in Air).
|
|
2673
|
+
# 1 if the surface is bright. A lot of light was reflected.
|
|
2674
|
+
#
|
|
2675
|
+
# raises:
|
|
2676
|
+
# RuntimeError: if Robot is switched off or unconnected. (replaces ENODEV error)
|
|
2677
|
+
# """
|
|
2678
|
+
try:
|
|
2679
|
+
i2c.write(0x10, IRSensor._address)
|
|
2680
|
+
except:
|
|
2681
|
+
raise RuntimeError(_UNCONNECTEDERRORMSG)
|
|
2682
|
+
byte = ~i2c.read(0x10, 1)[0]
|
|
2683
|
+
# mask out corresponding bit, from returned byte.
|
|
2684
|
+
return (byte & (2 ** self._index)) >> self._index
|
|
2685
|
+
|
|
2686
|
+
def read_analog(self):
|
|
2687
|
+
# """Returns the brightness of the surface as a byte.
|
|
2688
|
+
|
|
2689
|
+
# Returns:
|
|
2690
|
+
# int: amount of reflection of light in range [0,255].
|
|
2691
|
+
#
|
|
2692
|
+
# raises:
|
|
2693
|
+
# RuntimeError: if Robot is switched off or unconnected. (replaces ENODEV error)
|
|
2694
|
+
# """
|
|
2695
|
+
try:
|
|
2696
|
+
i2c.write(0x10, IRSensor._address)
|
|
2697
|
+
except:
|
|
2698
|
+
raise RuntimeError(_UNCONNECTEDERRORMSG)
|
|
2699
|
+
# Buffer structure:
|
|
2700
|
+
# 1 Byte Bitmask (for digital redout),
|
|
2701
|
+
# then 10 Bytes analog byte values (every second entry is a value).
|
|
2702
|
+
buffer = i2c.read(0x10, 11)
|
|
2703
|
+
return buffer[1 + self._index * 2]
|
|
2704
|
+
|
|
2705
|
+
|
|
2706
|
+
def ir_read_values_as_byte():
|
|
2707
|
+
# """get single byte with bit encoding of each Infrared sensor state
|
|
2708
|
+
# Bits: 0=R2, 1=R1, 2=M, 3=L1, 4=L2
|
|
2709
|
+
# """
|
|
2710
|
+
i2c.write(0x10, bytearray([0x1D]))
|
|
2711
|
+
buf = i2c.read(0x10, 1)
|
|
2712
|
+
return ~buf[0]
|
|
2713
|
+
|
|
2714
|
+
|
|
2715
|
+
def getDistance():
|
|
2716
|
+
# """uses the ultrasonic sensor to measure distance
|
|
2717
|
+
|
|
2718
|
+
# Returns:
|
|
2719
|
+
# int: valid Distance as cm in range [0,500].
|
|
2720
|
+
# For measurement errors or larger distances: 255.
|
|
2721
|
+
# """
|
|
2722
|
+
pin13.write_digital(1)
|
|
2723
|
+
pin13.write_digital(0)
|
|
2724
|
+
p = machine.time_pulse_us(pin14, 1, 50000)
|
|
2725
|
+
# approximate division: p / 58.2 - 0.5
|
|
2726
|
+
cm = (p >> 6) + (p >> 10) + (p >> 11) + (p >> 12) + 1
|
|
2727
|
+
return max(min(cm, 500), 0) if cm > 0 else 255
|
|
2728
|
+
|
|
2729
|
+
# Signaling functions
|
|
2730
|
+
|
|
2731
|
+
|
|
2732
|
+
def setLED(state, stateR=None):
|
|
2733
|
+
# """Set the front red LED's.
|
|
2734
|
+
|
|
2735
|
+
# Parameters:
|
|
2736
|
+
# state (0/1): Sets the state of the left LED.
|
|
2737
|
+
# if stateR is omitted, then both LEDS.
|
|
2738
|
+
# 0=Off, 1=On
|
|
2739
|
+
# stateR (0/1/None, optional): Sets the right LED state.
|
|
2740
|
+
# 0=Off, 1=On, Default=None uses "state" for right LED.
|
|
2741
|
+
# """
|
|
2742
|
+
global _ledState
|
|
2743
|
+
stateR = stateR if stateR != None else state
|
|
2744
|
+
_ledState[1] = state
|
|
2745
|
+
_ledState[2] = stateR
|
|
2746
|
+
i2c.write(0x10, _ledState)
|
|
2747
|
+
|
|
2748
|
+
|
|
2749
|
+
def setLEDLeft(state):
|
|
2750
|
+
# """state: 0=Off, 1=On"""
|
|
2751
|
+
global _ledState
|
|
2752
|
+
_ledState[1] = state
|
|
2753
|
+
i2c.write(0x10, _ledState)
|
|
2754
|
+
|
|
2755
|
+
|
|
2756
|
+
def setLEDRight(state):
|
|
2757
|
+
# """state: 0=Off, 1=On"""
|
|
2758
|
+
global _ledState
|
|
2759
|
+
_ledState[2] = state
|
|
2760
|
+
i2c.write(0x10, _ledState)
|
|
2761
|
+
|
|
2762
|
+
|
|
2763
|
+
def fillRGB(red, green, blue):
|
|
2764
|
+
# """Uses Neopixel to set all 4 bottom RGB LEDs color.
|
|
2765
|
+
# Parameters (red,green,blue) are each a byte in Range [0,255].
|
|
2766
|
+
# """
|
|
2767
|
+
for i in range(4):
|
|
2768
|
+
_underglowNP[i] = (red, green, blue)
|
|
2769
|
+
_underglowNP.show()
|
|
2770
|
+
setRGB=fillRGB
|
|
2771
|
+
|
|
2772
|
+
|
|
2773
|
+
def clearRGB():
|
|
2774
|
+
_underglowNP.clear()
|
|
2775
|
+
|
|
2776
|
+
def posRGB(position, red, green, blue):
|
|
2777
|
+
# """Uses Neopixel to set a single RGB LED of the robot.
|
|
2778
|
+
|
|
2779
|
+
# Parameters:
|
|
2780
|
+
# position (int): position of the targeted LED.
|
|
2781
|
+
# Numbers are visible at underside of Robot.
|
|
2782
|
+
# 0=front left
|
|
2783
|
+
# 1=back left
|
|
2784
|
+
# 2=back right
|
|
2785
|
+
# 3=front right
|
|
2786
|
+
# red, green, blue (int): color byte value.
|
|
2787
|
+
# each in range [0,255].
|
|
2788
|
+
|
|
2789
|
+
# raises:
|
|
2790
|
+
# ValueError: if position argument is out of valid range
|
|
2791
|
+
# """
|
|
2792
|
+
if position < 0 or position > 3:
|
|
2793
|
+
raise ValueError("invalid RGB-LED position. Must be 0,1,2 or 3.")
|
|
2794
|
+
_underglowNP[position] = (red, green, blue)
|
|
2795
|
+
_underglowNP.show()
|
|
2796
|
+
|
|
2797
|
+
|
|
2798
|
+
def setAlarm(state):
|
|
2799
|
+
if state:
|
|
2800
|
+
music.play(_alarmSequence, wait=False, loop=True)
|
|
2801
|
+
else:
|
|
2802
|
+
music.stop()
|
|
2803
|
+
|
|
2804
|
+
|
|
2805
|
+
def beep():
|
|
2806
|
+
music.pitch(440, 200, wait=False)
|
|
2807
|
+
|
|
2808
|
+
# Class constants (for compatibilty)
|
|
2809
|
+
|
|
2810
|
+
|
|
2811
|
+
class LEDState:
|
|
2812
|
+
ON = 1
|
|
2813
|
+
OFF = 0
|
|
2814
|
+
RED = 1
|
|
2815
|
+
|
|
2816
|
+
|
|
2817
|
+
class IR:
|
|
2818
|
+
R2 = 0
|
|
2819
|
+
R1 = 1
|
|
2820
|
+
M = 2
|
|
2821
|
+
L1 = 3
|
|
2822
|
+
L2 = 4
|
|
2823
|
+
masks = [0x01, 0x02, 0x04, 0x08, 0x10]
|
|
2824
|
+
|
|
2825
|
+
|
|
2826
|
+
# Default instances
|
|
2827
|
+
pin2.set_pull(pin2.NO_PULL)
|
|
2828
|
+
delay = sleep
|
|
2829
|
+
irR2 = IRSensor(0)
|
|
2830
|
+
irR1 = IRSensor(1)
|
|
2831
|
+
irRight = irR1
|
|
2832
|
+
irM = IRSensor(2)
|
|
2833
|
+
irL1 = IRSensor(3)
|
|
2834
|
+
irLeft = irL1
|
|
2835
|
+
irL2 = IRSensor(4)
|
|
2836
|
+
motL = Motor(0)
|
|
2837
|
+
motR = Motor(2)
|
|
2838
|
+
`
|
|
2839
|
+
};
|
|
2840
|
+
|
|
2841
|
+
// src/index.ts
|
|
2842
|
+
var minified = {
|
|
2843
|
+
calliope: libraries_default,
|
|
2844
|
+
microbit: libraries_default2
|
|
2845
|
+
};
|
|
2846
|
+
var raw = {
|
|
2847
|
+
calliope: libraries_raw_default,
|
|
2848
|
+
microbit: libraries_raw_default2
|
|
2849
|
+
};
|
|
2850
|
+
var DEVICES = Object.keys(minified);
|
|
2851
|
+
function assertDevice(device) {
|
|
2852
|
+
if (!(device in minified)) {
|
|
2853
|
+
throw new Error(
|
|
2854
|
+
`Unknown device: ${device}. Valid devices are: ${DEVICES.join(", ")}`
|
|
2855
|
+
);
|
|
2856
|
+
}
|
|
2857
|
+
}
|
|
2858
|
+
function listDevices() {
|
|
2859
|
+
return [...DEVICES];
|
|
2860
|
+
}
|
|
2861
|
+
function listLibraries(device) {
|
|
2862
|
+
assertDevice(device);
|
|
2863
|
+
return Object.keys(minified[device]);
|
|
2864
|
+
}
|
|
2865
|
+
function getLibrary(device, name) {
|
|
2866
|
+
assertDevice(device);
|
|
2867
|
+
const source = minified[device][name];
|
|
2868
|
+
if (source === void 0) {
|
|
2869
|
+
throw new Error(
|
|
2870
|
+
`Unknown ${device} library: ${name}. Valid libraries are: ${listLibraries(device).join(", ")}`
|
|
2871
|
+
);
|
|
2872
|
+
}
|
|
2873
|
+
return source;
|
|
2874
|
+
}
|
|
2875
|
+
function getRawLibrary(device, name) {
|
|
2876
|
+
assertDevice(device);
|
|
2877
|
+
const source = raw[device][name];
|
|
2878
|
+
if (source === void 0) {
|
|
2879
|
+
throw new Error(
|
|
2880
|
+
`Unknown ${device} raw library: ${name}. Valid libraries are: ${listLibraries(device).join(", ")}`
|
|
2881
|
+
);
|
|
2882
|
+
}
|
|
2883
|
+
return source;
|
|
2884
|
+
}
|
|
2885
|
+
// Annotate the CommonJS export names for ESM import in node:
|
|
2886
|
+
0 && (module.exports = {
|
|
2887
|
+
calliopeLibraries,
|
|
2888
|
+
calliopeRawLibraries,
|
|
2889
|
+
getLibrary,
|
|
2890
|
+
getRawLibrary,
|
|
2891
|
+
listDevices,
|
|
2892
|
+
listLibraries,
|
|
2893
|
+
microbitLibraries,
|
|
2894
|
+
microbitRawLibraries
|
|
2895
|
+
});
|
|
2896
|
+
//# sourceMappingURL=index.js.map
|