/
picosmars.py
49 lines (41 loc) · 988 Bytes
/
picosmars.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
# PicoSMARS
# A MircoPython version of the SMARS robot, also for the SMARS Mini
from machine import Pin, PWM, I2C
from smars import SMARS
from time import sleep
# I2C
sda = Pin(26)
scl = Pin(27)
id = 1
# create the i2c object
i2c = I2C(id=id, sda=sda, scl=scl)
robot = SMARS(i2c=i2c)
def avoid():
while True:
print("I'm working")
if robot.distance >= 5:
robot.forward()
print('forward',robot.distance)
else:
robot.backward()
robot.turnright()
print('backward',robot.distance)
def motor_test():
while True:
print("Testing left")
robot.turnleft()
sleep(0.25)
print("Testing right")
robot.turnright()
sleep(0.25)
print("Testing forward")
robot.forward()
sleep(0.25)
print("Testing backward")
robot.backward()
sleep(0.25)
def motor_stop():
robot.stop()
# motor_test()
avoid()
# motor_stop()