#hardware platform: pyboard V1.1
from pyb import Pin
import time
down = Pin('B10', Pin.IN, Pin.PULL_UP)
up = Pin('A5', Pin.IN, Pin.PULL_UP)
left = Pin('A15', Pin.IN, Pin.PULL_UP)
right = Pin('C13', Pin.IN, Pin.PULL_UP)
red = Pin('C9',Pin.OUT)
green = Pin('C8',Pin.OUT)
blue = Pin('C6',Pin.OUT)
red.on()
while True:
print(down.value())
if(down.value()==0):
red.on()
elif(up.value()==0):
green.on()
elif(left.value()==0):
blue.on()
elif(right.value()==0):
red.on()
else:
red.off()
green.off()
blue.off()
time.sleep(0.05)
class RomiMotor :
"""
We reuse the same timer for all instances of the class, so this is the callback
of the class, which calls the handlers in each instance.
"""
@classmethod
def class_rpm_handler(cls, tim) :
for h in cls.rpm_handlers :
h(tim)
# List of the rpm handlers of the instances, necessary because we share a single timer.
rpm_handlers = []
# The shared timer
rpmtimer = None
"""
Initialize a RomiMotor, connected either to the 'X' side or the 'Y' side of the Pyboard.
On either side (? is 'X' or 'Y'):
- PWM is on pin ?1
- DIR is on pin ?2
- SLEEP is on pin ?3
- ENCA is on pin ?4
- ENCB is on pin ?5
"""
def __init__(self, X=True) :
if X :
self.pwmpin = Pin('X6', Pin.OUT_PP)
self.pwmtim = Timer(2, freq=5000)
self.pwm = self.pwmtim.channel(1, mode=Timer.PWM, pin=self.pwmpin)
self.pwm.pulse_width(0)
self.dir = Pin('X2', Pin.OUT_PP)
self.dir.off() # O = forward, 1 = reverse
self.sleep = Pin('X3', Pin.OUT_PP)
self.sleep.value(0) # 0 = sleep, 1 = active
self.enca = Pin('X4', Pin.IN, Pin.PULL_UP)
self.encb = Pin('X5', Pin.IN, Pin.PULL_UP)
else :
self.pwmpin = Pin('Y1', Pin.OUT_PP)
self.pwmtim = Timer(8, freq=5000)
self.pwm = self.pwmtim.channel(1, mode=Timer.PWM, pin=self.pwmpin)
self.pwm.pulse_width(0)
self.dir = Pin('Y2', Pin.OUT_PP)
self.dir.off() # O = forward, 1 = reverse
self.sleep = Pin('Y3', Pin.OUT_PP)
self.sleep.value(0) # 0 = sleep, 1 = active
self.enca = Pin('Y4', Pin.IN, Pin.PULL_UP)
self.encb = Pin('Y5', Pin.IN, Pin.PULL_UP)
self.pwmscale = (self.pwmtim.period() + 1) // 10000 # scale factot for permyriad(10000 as it allows to get more distinct points and avoid divisions) power
self.count_a = 0 # counter for impulses on the A output of the encoder
self.target_a = 0 # target value for the A counter (for controlled rotation)
self.count_b = 0 # counter for impulses on the B output of the encoder
self.time_a = 0 # last time we got an impulse on the A output of the encoder
self.time_b = 0 # last time we got an impulse on the B output of the encoder
self.elapsed_a_b = 0 # time elapsed between an impulse on A and an impulse on B
self.dirsensed = 0 # direction sensed through the phase of the A and B outputs
self.rpm = 0 # current speed in rotations per second
self.rpm_last_a = 0 # value of the A counter when we last computed the rpms
self.cruise_rpm = 0 # target value for the rpms
self.walking = False # Boolean that indicates if the robot is walking or stationary
self.desiredDir = True# Boolean that indecates the desired direction of the motor for move function
self._control = PID() # PID control for the rotation of the wheels
self._control.setTunings(KP, KI, KD);
self._control.setSampleTime(1);
self._control.setOutputLimits(-10000, 10000)
self._control_distance = PID() # PID control for the cascade of the distance
self._control_distance.setTunings(KP_DISTANCE, KI_DISTANCE, KD_DISTANCE);
self._control_distance.setSampleTime(1);
self._control_distance.setOutputLimits(-MAXIMUM_VELOCITY, MAXIMUM_VELOCITY)
ExtInt(self.enca, ExtInt.IRQ_RISING, Pin.PULL_UP, self.enca_handler)
ExtInt(self.encb, ExtInt.IRQ_RISING, Pin.PULL_UP, self.encb_handler)
if RomiMotor.rpmtimer is None : # create only one shared timer for all instances
RomiMotor.rpmtimer = Timer(4)
RomiMotor.rpmtimer.init(freq=100, callback=RomiMotor.class_rpm_handler)
RomiMotor.rpm_handlers.append(self.rpm_handler) # register the handler for this instance
"""
Handler for interrupts caused by impulses on the A output of the encoder.
This is where we sense the rotation direction and adjust the throttle to
reach a target number of rotations of the wheel.
"""
def enca_handler(self, pin) :
self.count_a += 1
if self.encb.value():
self.dirsensed = -1 # A occurs before B
else :
self.dirsensed = 1 # B occurs before A
if self.target_a > 0 : # If we have a target rotation
if self.count_a >= self.target_a :
self.cruise_rpm = 0
self.pwm.pulse_width(0) # If we reached of exceeded the rotation, stop the motor
self.target_a = 0 # remove the target
self.walking = False
else: # The cascade control for the distance
self._control_distance.setSetPoint(self.target_a)
self.cruise_rpm = self._control_distance.compute(self.count_a)
self.walking = True
"""
Handler for interrupts caused by impulses on the B output of the encoder.
"""
def encb_handler(self, pin) :
self.count_b += 1
"""
This is the handler of the timer interrupts to compute the rpm
"""
def rpm_handler(self, tim) :
self.rpm = 100*(self.count_a - self.rpm_last_a) # The timer is at 100Hz
self.rpm_last_a = self.count_a # Memorize the number of impulses on A
if self.cruise_rpm != 0 : # If we have an rpm target
self._control.setSetPoint(self.cruise_rpm)
output = self._control.compute(self.rpm)
if output < 0 or self.desiredDir == False: # Corrects the control output for the desired direction
if output < 0:
output = -output
self.dir.off()
else :
self.dir.on()
self.pwm.pulse_width(output * self.pwmscale)
self.walking = True
self.sleep.on()
else:
self.walking = False
"""
Set the power of the motor in percents.
Positive values go forward, negative values go backward.
"""
def throttle(self, pct) :
if pct is None :
return
if pct < 0 :
self.dir.off()
pct = -pct
else :
self.dir.on()
self.pwm.pulse_width(100 * pct * self.pwmscale)
self.walking = True
self.sleep.on()
"""
Get the current power as a percentage of the max power.
The result is positive if the motor runs forward, negative if it runs backward.
"""
def getThrottle(self) :
thr = self.pwm.pulse_width() // self.pwmscale
if self.dir.value() > 0 :
thr = -thr
return thr
"""
Release the motor to let it rotate freely.
"""
def release(self, release=True) :
if release :
self.sleep.off()
else :
self.sleep.on()
"""
Perform 'tics' 1/360 rotation of the wheel at 'power' percents of the max power.
If 'turns' is positive, the wheel turns forward, if it is negative, it turns backward.
"""
def rotatewheel(self, tics, power=20):
if tics < 0 :
self.desiredDir = False
tics = -tics
else :
self.desiredDir = True
self.count_a = 0
self.count_b = 0
self._control_distance.setOutputLimits(-power*100, power*100)
self.target_a = int(tics)
"""
Wait for the rotations requested by 'rotatewheel' to be done.
"""
def wait(self) :
while self.target_a !=0 :
pass
"""
Set a target rpms. The wheel turns in its current rotation direction,
'rpm' should be non negative.
"""
def cruise(self, rpm) :
if rpm < 0 :
self.desiredDir = False
rpm = -rpm
else :
self.desiredDir = True
self.cruise_rpm = int(rpm * 6)
"""
Get the current rpms. This is always non negative, regardless of the rotation direction.
"""
def get_rpms(self) :
return self.rpm / 60
"""
Cancel all targets of rotation and rpm
"""
def clear(self) :
self.target_a = 0
self.cruise_rpm = 0
self.desiredDir = True
self.rpm = 0
self.rpm_last_a = 0
self.count_a = 0
self.count_b = 0
self.pwm.pulse_width(0)
"""
Stop the motor.
"""
def stop(self) :
self.clear()
self.throttle(0)
LED_no_connection.on()
buzz = 1
# adding back the receiver_testing wire in receiving list
Receiving.append(r_test)
s_test.low()
return output_led
# for sending_pin,testing_pin in zip(sending,receiving):
# testing(sending_pin,testing_pin)
# condition for testing each wire from sending end and actual receiving end.
if (testing(s_1, r_1)) is 1:
LED1_output.on()
if (testing(s_2, r_2)) is 1:
LED2_output.on()
if (testing(s_3, r_3)) is 1:
LED3_output.on()
if (testing(s_4, r_4)) is 1:
LED4_output.on()
if (testing(s_5, r_5)) is 1:
LED5_output.on()
# to display the error flg
print(buzz)
class Ultrasound(object):
'''
Driver for the HC-SR04 ultrasonic distance sensor
'''
PULSE2CM = 17241.3793 # cm/s
MAX_RANGE = 3500 # cm
OUT_OF_RANGE = 0xffffffff
def __init__(self, triggerPort, echoPort, nSamples=3):
'''
Constructor
@param triggerPort: Port number of the trigger signal
@param echoPort: Port number of the echo port
@param nSamples: Number of samples to measure the distance
'''
self._nSamples = nSamples
#Configure ports
self._trigger = Pin(triggerPort, Pin.OUT)
self._trigger.value(0)
self._echo = Pin(echoPort, Pin.IN)
sleep(1)
def read(self):
'''
Measures distance
@return: Distance as centimeters
'''
POLL_TIMEOUT = 500000 # microseconds
i = 0
dist = 0
while i < self._nSamples and dist != Ultrasound.OUT_OF_RANGE:
# Send start signal
self._trigger.on()
sleep_us(10)
self._trigger.off()
# Wait for response
pollStart = ticks_us()
while self._echo.value() == 0:
if ticks_diff(ticks_us(), pollStart) > POLL_TIMEOUT:
raise Exception("Timeout waiting for echo HIGH")
# Measure signal length
pulseStart = ticks_us()
while self._echo.value() == 1:
if ticks_diff(ticks_us(), pollStart) > POLL_TIMEOUT:
raise Exception("Timeout waiting for echo LOW")
pulseEnd = ticks_us()
pulseDuration = ticks_diff(pulseEnd, pulseStart) # as microseconds
distSample = pulseDuration * Ultrasound.PULSE2CM / 1e6 # cm
if distSample < Ultrasound.MAX_RANGE:
dist += distSample
else:
dist = Ultrasound.OUT_OF_RANGE
i += 1
return dist / i if dist != Ultrasound.OUT_OF_RANGE else Ultrasound.OUT_OF_RANGE
def cleanup(self):
'''
Frees resources
'''
#self._trigger.cleanup()
#self._echo.cleanup()
pass
from pyb import Switch, Pin
led = Pin('LED_BLUE', Pin.OUT_PP)
btn = Switch()
def cb():
state = btn.value()
led.value(not led.value())
print('ON' if led.value() else 'OFF')
btn.callback(cb)
led.on()
# Charge d'un condensateur à travers une résistance
# R = 100k et C = 220 nF
from pyb import Pin, ADC, Timer
from time import sleep_ms
import array
T = 0.02 # microsecond
f = 1 / (T * 1E-3)
nb = 30
pinE = Pin('A2', Pin.OUT) # Alimentation du circuit RC
adc = ADC(Pin('A3')) # Activation du CAN
buf = array.array("h", nb * [0x7FFF]) # h = signed short (int 2 octets)
tim = Timer(6, freq=f) # create a timer running at 10Hz
pinE.off() # Décharge du condensateur
sleep_ms(1000) # Attendre 2 s
pinE.on() # Début de la charge
adc.read_timed(buf, tim) # Mesures
# Données
f = tim.freq()
x = [i / f * 1E3 for i in range(nb)] # Millisecond
y = [val for val in buf]
print((x, y))
from ht16k33_seg import Seg7x4
i2c = I2C(1)
aff = Seg7x4(i2c, address=0x70)
f = 750E3 # max = 750 kHz [84Mhz/4/(12+15) = 778 kHz]
nb = 1000 # Nombre de points de mesure
pinE = Pin('A2', Pin.OUT) # Alimentation du circuit RC
adc = ADC(Pin('A3')) # Activation du CAN
buf = array.array("h", nb * [0x0FFF]) # h = signed short (int 2 octets)
tim = Timer(6, freq=f)
#tim = Timer(6, prescaler=0, period=120) # create a timer running at 10Hz
while True:
pinE.on() # Décharge du condensateur
sleep_ms(100) # Attendre 2 s
pinE.off() # Début de la charge
adc.read_timed(buf, tim) # Mesures
seuil = buf[0] * 0.368
for i in range(nb):
if buf[i] < seuil:
tau = i / tim.freq() * 1E6
break
R = 9.93
C = tau / R # nF
h = round(1.56 * C - 33.7, 1)
#print("C=", C, "h =", h)
aff.fill(0) # Efface l'affichage précédent
from pyb import Pin # Gestion des broches E/S
from time import sleep_ms # Gestion du temps
led = Pin("D5", Pin.OUT_PP) # LED sur D5 en sortie classique (push-pull)
while True: # Boucle infinie
led.on() # LED allumée
sleep_ms(500) # Attendre 500 ms
led.off() # LED éteinte
sleep_ms(500) # Attendre 500 ms
from pyb import Pin
pin = Pin("D8", Pin.OUT_PP) # En sortie "classique" (push-pull)
pin.on() # Etat haut (3,3 V)
#pin.off() # Etat bas (0 V)
class Motor(object):
'''
Controls a motor
'''
PWM_FREQ = 50.0
MIN_DUTY = 30.0
MAX_DUTY = 90.0
DIFF_DUTY = (MAX_DUTY - MIN_DUTY) / 100.0
def __init__(self, pwmPin, pwmTimer, pwmChannel, reversePin):
'''
Constructor
@param pwmPin: Pin where the PWM-signal comes from
@param pwmTimer: Timer to produce the PWM signal
@param pwmChannel: Channel of the timer
@param reversePin: Pin which controls the reverse signal
'''
self._throttle = 0.0
self._pwm = Pwm(pwmPin, pwmTimer, pwmChannel, Motor.PWM_FREQ)
self._reversePin = Pin(reversePin, Pin.OUT)
self._reversePin.off()
def cleanup(self):
'''
Finishes and releases the resources
'''
self.stop()
self._pwm.cleanup()
self._reversePin.off()
def setThrottle(self, throttle):
'''
Makes the motor spin
@param throttle: (-100..100) Percentage to spin the motor. This value can be negative, in that case, the motor spins reversed.
'''
if throttle < 0:
self.setAbsThrottle(-throttle, True)
else:
self.setAbsThrottle(throttle, False)
def setAbsThrottle(self, throttle, reverse):
'''
Sets the motor throttle as absolute value (negative values are considered as 0)
@param throttle: (0..100) Percentagle to spin the motor.
@param reverse: Indicates whether the motor spins forwards or backwards (reversed)
'''
if throttle < 0.0:
throttle = 0.0
elif throttle > 100.0:
throttle = 100.0
self._throttle = throttle
if self._throttle != 0:
if reverse:
self._reversePin.on()
else:
self._reversePin.off()
duty = Motor.MIN_DUTY + self._throttle * Motor.DIFF_DUTY
self._pwm.setDutyPerc(duty)
else:
self._reversePin.off()
self._pwm.setDutyPerc(0)
def getThrottle(self):
'''
@return: The current throttle
'''
return self._throttle
def isReversed(self):
'''
@return: Indicates whether the motor drives forwards or backwards
'''
return self._reversePin.value() == 1
def stop(self):
'''
Stops the motor
'''
self.setAbsThrottle(0, False)
class Esp8266(object):
'''
WiFi module
TODO: 20200326 DPM: This class is intended for having a single client. Another limitation is that any request
to the module should not be performed meanwhile it is in server mode. Further improvements must be done
to avoid these limitations.
A possible solution could be capturing all messages from the module in a IRQ-handler where the contents
could be processed to rise events or send data through queues or even change the status of the object.
'''
BYTES_ENCODING = "ascii"
OP_MODE_CLIENT = const(1)
OP_MODE_AP = const(2)
SECURITY_OPEN = const(0)
SECURITY_WEP = const(1)
SECURITY_WPA = const(2)
SECURITY_WPA2 = const(3)
SECURITY_WPA_WPA2 = const(4)
def __init__(self,
uartId,
enablePin=None,
readTimeout=1000,
baud=115200,
debug=False):
'''
Constructor
@param uartId: Id of the UART port.
@param enablePin: (default=None) Pin to handle the enable signal of the ESP8266 module.
If not provided here, this line should be provided using a different
way to the module in order to activate it.
@param readTimeout: Time to wait for any incoming data in milliseconds (default 1000)
@param baud: (default=115200) bit-rate of the communication
@param debug: (default=False) prints debug information on the REPL
'''
self._uart = UART(uartId, baud)
if enablePin != None:
self._enablePin = Pin(enablePin, Pin.OUT)
else:
self._enablePin = None
self._readTimeout = readTimeout
self._debug = debug
# Reset module
self.disable()
sleep_ms(500)
def start(self, txPower=10):
'''
Starts the module.
Enable the module and set the TX-power.
@param txPower: (default=10) the tx-power
'''
self.enable()
self.setTxPower(txPower)
def cleanup(self):
self.disable()
self._uart.deinit()
def enable(self):
'''
Enables the module, if enable pin was provided
'''
if self._enablePin != None:
self._enablePin.on()
sleep_ms(500)
if self._uart.any():
self._flushRx()
sleep_ms(500)
def disable(self):
'''
Disables the module, if enable pin was provided
'''
if self._enablePin != None:
self._enablePin.off()
def isPresent(self):
'''
Checks if the module is present. Just sends a "AT" command.
@return: True if it's present
'''
self._write("AT")
return self._isOk()
def reset(self, txPower=10):
'''
Resets the module
@param txPower: (default=10) The tx-power
'''
self._write("AT+RST")
sleep_ms(300)
assert self._isOk()
self.setTxPower(txPower)
def getVersion(self):
'''
Gets the firmware version
@return: array with the version information as follows:
[AT version, SDK version, compile time]
'''
version = []
self._write("AT+GMR")
data = self._readline()
while data != None and not data.startswith(
bytes("OK", Esp8266.BYTES_ENCODING)):
if not data.startswith(bytes("AT+GMR", Esp8266.BYTES_ENCODING)):
version.append(data.strip())
data = self._readline()
return version
def getOperatingMode(self):
'''
@return: The operating mode
@raise Exception: On non reading mode
'''
self._write("AT+CWMODE?")
data = self._readline()
while data != None and not data.startswith(
bytes("+CWMODE", Esp8266.BYTES_ENCODING)):
data = self._readline()
if data != None:
mode = int(
data.split(bytes(":", Esp8266.BYTES_ENCODING))[1].strip())
self._flushRx()
else:
raise Exception("Cannot read operating mode.")
return mode
def setOperatingMode(self, mode):
'''
@param mode: The operating mode. Modes can be added using the or-operator.
'''
self._write("AT+CWMODE={0}".format(mode))
assert self._isOk()
def join(self, ssid, passwd=None):
'''
Joins to a wireless network. Only when client mode is enabled.
@param ssid: Network name
@param passwd: Password (Optional for open networks). Max 64 bytes ASCII
@raise Exception: On join error.
Error codes:
1: connection timeout.
2: wrong password.
3: cannot find the target AP.
4: connection failed.
'''
if passwd != None and passwd != "":
self._write("AT+CWJAP_CUR=\"{0}\",\"{1}\"".format(ssid, passwd))
else:
self._write("AT+CWJAP_CUR=\"{0}\"".format(ssid))
data = self._readline() or ""
isError = data.startswith(bytes("FAIL", Esp8266.BYTES_ENCODING))
while not isError and not data.startswith(
bytes("OK", Esp8266.BYTES_ENCODING)):
sleep_ms(100)
data = self._readline() or ""
isError = data.startswith(bytes("FAIL", Esp8266.BYTES_ENCODING))
self._flushRx()
if isError:
raise Exception("Error: Cannot join to network.")
def joinedNetwork(self):
'''
Gets the name of the joined network where the module is currently
connected to, if any.
@return: Name of the network
'''
name = ""
self._write("AT+CWJAP?")
data = self._readline()
self._flushRx()
if data.startswith(bytes("+CWJAP", Esp8266.BYTES_ENCODING)):
networkData = data.split(bytes(":", Esp8266.BYTES_ENCODING))[1]
name = networkData.split(bytes(",", Esp8266.BYTES_ENCODING))[0]
return name
def discover(self):
'''
List available discovered networks. This list may not be exhaustive.
@return: List of networks
'''
pass
def disconnect(self):
'''
Disconnects the current network. Only when it is already joined.
'''
pass
def setAccessPointConfig(self,
ssid,
passwd=None,
channel=1,
security=SECURITY_OPEN):
'''
Sets up the AP configuration.
@param ssid: Name of the network
@param passwd: Password (not used for open AP)
@param channel: WiFi channel (default 1)
@param security: Security mode, like WEP, WPA or WPA2 (default open)
'''
if security != Esp8266.SECURITY_OPEN:
self._write("AT+CWSAP=\"{0}\",\"{1}\",{2},{3}".format(
ssid, passwd, channel, security))
else:
self._write("AT+CWSAP=\"{0}\",\"\",{1},{2}".format(
ssid, channel, security))
assert self._isOk()
def getAccessPointConfig(self):
'''
Queries the current access point configuration. This works when AP mode is
enabled.
'''
pass
def getAddresses(self):
'''
Gets the addresses and MACs of the module depending of the operating mode. It returns
addresses for the currently active modes.
Possible address-types are: APIP, APMAC, STAIP, STAMAC
@return: Dictionary of addresses and MACs with the format {address-type, address}
'''
addresses = {}
self._write("AT+CIFSR")
data = self._readline()
#TODO: It seems not reading the contents
while data != None and data.startswith(
bytes("+CIFSR", Esp8266.BYTES_ENCODING)):
addressInfo = data.strip().split(bytes(
":", Esp8266.BYTES_ENCODING))[1].split(
bytes(",", Esp8266.BYTES_ENCODING))
addresses[addressInfo[0]] = addressInfo[1]
return addresses
def getAddressByType(self, addrType):
'''
Get the module address of a concrete type, if enabled.
@param addrType: Possible address-types are: APIP, APMAC, STAIP, STAMAC
@return: The address or none if the type is not enabled
'''
address = b""
addresses = self.getAddresses()
apIpKey = bytes(addrType, Esp8266.BYTES_ENCODING)
if apIpKey in addresses.keys():
address = addresses[apIpKey]
return address
def getApIpAddress(self):
'''
@return: The IP address of the module's access point or empty if not enabled.
'''
return self.getAddressByType("APIP")
def setApIpAddress(self, ip, gateway, netmask="255.255.255.0"):
'''
Sets the IP address of the module's access point
@param ip: IP address
@param gateway: Network's gateway
@param netmask: (default="255.255.255.0") Network's mask of the IP addresses
'''
self._write("AT+CIPAP_CUR=\"{0}\",\"{1}\",\"{2}\"".format(
ip, gateway, netmask))
assert self._isOk()
def getStaIpAddress(self):
'''
@return: The IP address when it is connected to a station or
empty if not enabled
'''
return self.getAddressByType("STAIP")
def setStaIpAddress(self, ip, gateway, netmask="255.255.255.0"):
'''
Sets the IP address when it is connected to a station
@param ip: IP address
@param gateway: Network's gateway
@param netmask: (default="255.255.255.0") Network's mask for the IP addresses
'''
self._write("AT+CIPSTA_CUR=\"{0}\",\"{1}\",\"{2}\"".format(
ip, gateway, netmask))
#This command is returning b"O" instead of b"OK\r\n"
assert self._startsLineWith("O")
def initServer(self, connectionClass, extraObj=None, port=333):
'''
Initializes the socket server.
20200325 DPM: There is a limitation of the length of the received messages:
It can receive up to 53 bytes at once and therefore further bytes will be missed.
In instance, the client could send:
'123456789012345678901234567890123456789012345678901234567890' (70 bytes)
But it receives:
b'+IPD,0,70:12345678901234567890123456789012345678901234567890123'
The strange thing is, it knows that it should be 70 bytes, but it misses the
last 17 bytes, which won't came in any further input either.
@see: Connection class
@param connectionClass: (instance of Connection class) Class which implements connection actions
@param extraObj: (default=None) Extra object to pass to the ConnectionClass new instances
@param port: (default=333) Listening port
'''
self._write("AT+CIPMUX=1")
assert self._isOk()
self._write("AT+CIPSERVER=1,{0}".format(port))
assert self._isOk()
#TODO: 20200326 DPM: Enabling IRQ here makes any request to the module not working properly.
self._enableRxIrq()
self._connections = {}
self._connectionClass = connectionClass
self._extraObj = extraObj
def stopServer(self):
'''
Stops the socket server.
'''
self._disableRxIrq()
self._write("AT+CIPSERVER=0")
assert self._isOk()
def _send(self, clientId, data):
#TODO: 20200326 DPM: Disabling the IRQ during the send will miss any new client connection.
# Therefore a single client can be used safely for the moment.
#TODO: 20200326 DPM: Implement with a lock to avoid different coroutines sending at the same time.
# Otherwise the concurrent send requests could be messed.
self._disableRxIrq()
self._write("AT+CIPSEND={0},{1}".format(clientId, len(data)))
self._flushRx()
self._write(data)
line = self._readline()
while line != None and not line.startswith("SEND"):
line = self._readline()
self._enableRxIrq()
assert line != None and "OK" in line
def getTimeout(self):
'''
Gets the socket client disconnect timeout
(AT+CIPSTO?)
@return: The timeout as seconds
'''
pass
def setTimeout(self, timeout):
'''
Sets the socket client disconnect timeout
(AT+CIPSTO=nn)
@param timeout: The timeout as seconds
'''
pass
def setTxPower(self, value):
'''
Sets the RF power for transmission
@param value: [0..82]; unit 0.25 dBm
'''
self._write("AT+RFPOWER={0}".format(value))
assert self._isOk()
def _write(self, data):
if self._debug:
print("=> {0}".format(bytes(data, Esp8266.BYTES_ENCODING)))
self._uart.write("{0}\r\n".format(data))
sleep_ms(200)
def _readline(self):
startTicks = ticks_ms()
while self._uart.any() == 0 and ticks_diff(
ticks_ms(), startTicks) < self._readTimeout:
sleep_ms(50)
data = None
if self._uart.any() != 0:
data = self._uart.readline()
if self._debug:
if data:
print("<= {0}".format(data))
else:
print("UART timeout")
return data
def _flushRx(self):
'''
Flushes the RX-buffer
'''
if self._uart.any():
buff = self._uart.read()
if self._debug:
print("(flushed)<= {0}".format(buff))
def _startsLineWith(self, text, flush=True):
'''
Checks whether the next line starts with a text
@param text: Text to find
@param flush: Flushes the RX-buffer after checking
@return: True if the line starts with the text
'''
data = self._readline()
startsWith = data != None and data.startswith(
bytes(text, Esp8266.BYTES_ENCODING))
while data != None and not startsWith:
data = self._readline()
startsWith = data != None and data.startswith(
bytes(text, Esp8266.BYTES_ENCODING))
if flush:
self._flushRx()
return startsWith
def _isOk(self, flush=True):
'''
@param flush: Flushes the RX-buffer after checking
@return: True if OK return was found
'''
return self._startsLineWith("OK", flush)
def _isError(self, flush=True):
'''
@param flush: Flushes the RX-buffer after checking
@return: True if Error return was found
'''
return self._startsLineWith("ERROR", flush)
def _enableRxIrq(self):
self._uart.irq(trigger=UART.IRQ_RXIDLE, handler=self._onDataReceived)
def _disableRxIrq(self):
self._uart.irq(trigger=UART.IRQ_RXIDLE, handler=None)
def _onDataReceived(self, _):
line = None
while self._uart.any():
line = self._readline()
if line.startswith(b"+IPD"):
#Message from client
contents = line.split(b":", 1)
data = contents[0].split(b",")
clientId = int(data[1])
message = str(contents[1][:int(data[2])],
Esp8266.BYTES_ENCODING)
get_event_loop().create_task(
self._connections[clientId].onReceived(message))
else:
line = line.strip()
contents = line.split(b",", 1)
if len(contents) > 1:
if contents[1] == b"CONNECT":
#New client
clientId = int(contents[0])
self._connections[clientId] = self._connectionClass(
clientId, self, self._extraObj)
get_event_loop().create_task(
self._connections[clientId].onConnected())
elif contents[1] == b"CLOSED":
#Client gone
clientId = int(contents[0])
self._connections[clientId].onClose()
del self._connections[clientId]
from machine import I2C
from ht16k33_seg import Seg7x4
i2c = I2C(1)
aff = Seg7x4(i2c, address=0x70)
f = 750E3 # max = 750 kHz [84Mhz/4/(12+15) = 778 kHz]
nb = 100 # Nombre de points de mesure
pinE = Pin('A2', Pin.OUT) # Source du circuit RC
adc = ADC(Pin('A3')) # Activation du CAN
buf = array("h", nb * [0x7FFF]) # h = signed short (int 2 octets)
tim = Timer(6, freq=f) # Déclaration du timer
while True:
pinE.on() # E=Vcc (charge)
sleep_ms(100) # Attendre 100 ms
pinE.off() # E=0 (décharge)
adc.read_timed(buf, tim) # Mesures
f = tim.freq() # Fréquence réelle du timer
x = [i / f * 1E6 for i in range(nb)] # Tableau des fréquence (µs)
y = [log(u) for u in buf] # Tableau des ln(u)
a, b = linear_reg(y, x) # Regression linéaire
C = -a / 10 # Calcul de la capacité
h = round(1.713 * C - 17.90, 1)
aff.fill(0) # Efface l'affichage précédent
aff.text(str(h)) # Affichage
if d[1]:
ir_avg.append(d[0])
if d[3]:
red_avg.append(d[2])
ir_D = (sum(ir_avg) - max(ir_avg) - min(ir_avg)) // len(ir_avg)
red_D = (sum(red_avg) - max(red_avg) - min(red_avg)) // len(red_avg)
print('ir:',ir_D)
print('red:',red_D)
return [ir_D,red_D]
################################################################
alert = Pin("PE0",Pin.OUT)
alert.on()
time.sleep(1)
alert.off()
while 1:
try:
m = max30102.MAX30102(pin='PE1')
except:
print("init error and retry ..")
time.sleep(1)
[ir_D,red_D] = get(1000)
class RomiMotor:
"""
We reuse the same timer for all instances of the class, so this is the callback
of the class, which calls the handlers in each instance.
"""
@classmethod
def class_rpm_handler(cls, tim):
for h in cls.rpm_handlers:
h(tim)
# List of the rpm handlers of the instances, necessary because we share a single timer.
rpm_handlers = []
# The shared timer
rpmtimer = None
"""
Initialize a RomiMotor, connected either to the 'X' side or the 'Y' side of the Pyboard.
On either side (? is 'X' or 'Y'):
- PWM is on pin ?1
- DIR is on pin ?2
- SLEEP is on pin ?3
- ENCA is on pin ?4
- ENCB is on pin ?5
"""
def __init__(self, X=True):
if X:
self.pwmpin = Pin('X1', Pin.OUT_PP)
self.pwmtim = Timer(2, freq=5000)
self.pwm = self.pwmtim.channel(1, mode=Timer.PWM, pin=self.pwmpin)
self.pwm.pulse_width(0)
self.dir = Pin('X2', Pin.OUT_PP)
self.dir.off() # O = forward, 1 = reverse
self.sleep = Pin('X3', Pin.OUT_PP)
self.sleep.value(0) # 0 = sleep, 1 = active
self.enca = Pin('X4', Pin.IN, Pin.PULL_UP)
self.encb = Pin('X5', Pin.IN, Pin.PULL_UP)
else:
self.pwmpin = Pin('Y1', Pin.OUT_PP)
self.pwmtim = Timer(8, freq=5000)
self.pwm = self.pwmtim.channel(1, mode=Timer.PWM, pin=self.pwmpin)
self.pwm.pulse_width(0)
self.dir = Pin('Y2', Pin.OUT_PP)
self.dir.off() # O = forward, 1 = reverse
self.sleep = Pin('Y3', Pin.OUT_PP)
self.sleep.value(0) # 0 = sleep, 1 = active
self.enca = Pin('Y4', Pin.IN, Pin.PULL_UP)
self.encb = Pin('Y5', Pin.IN, Pin.PULL_UP)
self.pwmscale = (self.pwmtim.period() +
1) // 100 # scale factor for percent power
self.count_a = 0 # counter for impulses on the A output of the encoder
self.target_a = 0 # target value for the A counter (for controlled rotation)
self.count_b = 0 # counter for impulses on the B output of the encoder
self.time_a = 0 # last time we got an impulse on the A output of the encoder
self.time_b = 0 # last time we got an impulse on the B output of the encoder
self.elapsed_a_b = 0 # time elapsed between an impulse on A and an impulse on B
self.dirsensed = 0 # direction sensed through the phase of the A and B outputs
self.rpm = 0 # current speed in rotations per second
self.rpm_last_a = 0 # value of the A counter when we last computed the rpms
self.cruise_rpm = 0 # target value for the rpms
ExtInt(self.enca, ExtInt.IRQ_RISING, Pin.PULL_UP, self.enca_handler)
ExtInt(self.encb, ExtInt.IRQ_RISING, Pin.PULL_UP, self.encb_handler)
if RomiMotor.rpmtimer is None: # create only one shared timer for all instances
RomiMotor.rpmtimer = Timer(4)
RomiMotor.rpmtimer.init(freq=4,
callback=RomiMotor.class_rpm_handler)
RomiMotor.rpm_handlers.append(
self.rpm_handler) # register the handler for this instance
"""
Handler for interrupts caused by impulses on the A output of the encoder.
This is where we sense the rotation direction and adjust the throttle to
reach a target number of rotations of the wheel.
"""
def enca_handler(self, pin):
self.count_a += 1
self.time_a = pyb.millis()
if pyb.elapsed_millis(self.time_b) > self.elapsed_a_b:
self.dirsensed = -1 # A occurs before B
else:
self.dirsensed = 1 # B occurs before A
if self.target_a > 0: # If we have a target rotation
if self.count_a >= self.target_a:
self.pwm.pulse_width(
0
) # If we reached of exceeded the rotation, stop the motor
self.target_a = 0 # remove the target
elif (self.target_a - self.count_a) < 30:
self.pwm.pulse_width(
7 * self.pwmscale
) # If we are very close to the target, slow down a lot
elif (self.target_a - self.count_a) < 60:
self.pwm.pulse_width(
15 *
self.pwmscale) # If we are close to the target, slow down
"""
Handler for interrupts caused by impulses on the B output of the encoder.
"""
def encb_handler(self, pin):
self.count_b += 1
self.elapsed_a_b = pyb.elapsed_millis(
self.time_a) # Memorize the duration since the last A impulse
"""
This is the handler of the timer interrupts to compute the rpms
"""
def rpm_handler(self, tim):
self.rpm = 4 * (self.count_a - self.rpm_last_a) # The timer is at 4Hz
self.rpm_last_a = self.count_a # Memorize the number of impulses on A
if self.cruise_rpm != 0: # If we have an RPM target
# Add a correction to the PWM according to the difference in RPMs
delta = abs(self.rpm - self.cruise_rpm)
if delta < 100:
corr = delta // 40
elif delta < 500:
corr = delta // 20
else:
corr = delta // 10
if self.cruise_rpm < self.rpm:
self.pwm.pulse_width(
max(5 * self.pwmscale,
self.pwm.pulse_width() - self.pwmscale * corr))
else:
self.pwm.pulse_width(
min(100 * self.pwmscale,
self.pwm.pulse_width() + self.pwmscale * corr))
"""
Set the power of the motor in percents.
Positive values go forward, negative values go backward.
"""
def throttle(self, pct):
if pct is None:
return
if pct < 0:
self.dir.on()
pct = -pct
else:
self.dir.off()
self.pwm.pulse_width(pct * self.pwmscale)
self.sleep.on()
"""
Get the current power as a percentage of the max power.
The result is positive if the motor runs forward, negative if it runs backward.
"""
def getThrottle(self):
thr = self.pwm.pulse_width() // self.pwmscale
if self.dir.value() > 0:
thr = -thr
return thr
"""
Release the motor to let it rotate freely.
"""
def release(self, release=True):
if release:
self.sleep.off()
else:
self.sleep.on()
"""
Perform 'turns' rotations of the wheel at 'power' percents of the max power.
If 'turns' is positive, the wheel turns forward, if it is negative, it turns backward.
"""
def rotatewheel(self, turns, power=20):
if turns < 0:
sign = -1
turns = -turns
else:
sign = 1
self.count_a = 0
self.count_b = 0
self.target_a = int(360 * turns)
self.throttle(sign * power)
"""
Wait for the rotations requested by 'rotatewheel' to be done.
"""
def wait(self):
while self.count_a < self.target_a:
pass
"""
Set a target RPMs. The wheel turns in its current rotation direction,
'rpm' should be non negative.
"""
def cruise(self, rpm):
self.cruise_rpm = int(rpm * 60)
"""
Get the current RPMs. This is always non negative, regardless of the rotation direction.
"""
def get_rpms(self):
return self.rpm / 60
"""
Cancel all targets of rotation and RPM
"""
def clear(self):
self.target_a = 0
self.cruise_rpm = 0
"""
Stop the motor.
"""
def stop(self):
self.clear()
self.throttle(0)
