kopia lustrzana https://github.com/pimoroni/pimoroni-pico
Finished MP examples
rodzic
0efe210c4c
commit
56451bff30
|
@ -6,13 +6,10 @@
|
||||||
- [Motor Cluster](#motor-cluster)
|
- [Motor Cluster](#motor-cluster)
|
||||||
- [Motor Wave](#motor-wave)
|
- [Motor Wave](#motor-wave)
|
||||||
- [Stop Motors](#stop-motors)
|
- [Stop Motors](#stop-motors)
|
||||||
- [Encoder Examples](#encoder-examples)
|
|
||||||
- [Read Encoders](#read-encoders)
|
|
||||||
- [Motor Profiler](#motor-profiler)
|
|
||||||
- [Function Examples](#function-examples)
|
- [Function Examples](#function-examples)
|
||||||
- [Read Sensors](#read-sensors)
|
- [Read Sensors](#read-sensors)
|
||||||
- [Sensor Feedback](#sensor-feedback)
|
- [Read Encoders](#read-encoders)
|
||||||
- [Current Meter](#current-meter)
|
- [Motor Profiler](#motor-profiler)
|
||||||
- [LED Rainbow](#led-rainbow)
|
- [LED Rainbow](#led-rainbow)
|
||||||
- [Turn Off LEDs](#turn-off-leds)
|
- [Turn Off LEDs](#turn-off-leds)
|
||||||
- [Control Examples](#control-examples)
|
- [Control Examples](#control-examples)
|
||||||
|
@ -21,6 +18,7 @@
|
||||||
- [Position on Velocity Control](#position-on-velocity-control)
|
- [Position on Velocity Control](#position-on-velocity-control)
|
||||||
- [Reactive Encoder](#reactive-encoder)
|
- [Reactive Encoder](#reactive-encoder)
|
||||||
- [Quad Position Wave](#quad-position-wave)
|
- [Quad Position Wave](#quad-position-wave)
|
||||||
|
- [Quad Velocity Sequence](#quad-velocity-sequence)
|
||||||
- [Tuning Examples](#tuning-examples)
|
- [Tuning Examples](#tuning-examples)
|
||||||
- [Position Tuning](#position-tuning)
|
- [Position Tuning](#position-tuning)
|
||||||
- [Velocity Tuning](#velocity-tuning)
|
- [Velocity Tuning](#velocity-tuning)
|
||||||
|
@ -59,7 +57,13 @@ An example of applying a wave pattern to a group of motors and the LEDs.
|
||||||
A simple program that stops the motors.
|
A simple program that stops the motors.
|
||||||
|
|
||||||
|
|
||||||
## Encoder Examples
|
## Function Examples
|
||||||
|
|
||||||
|
### Read Sensors
|
||||||
|
[read_sensors.py](read_sensors.py)
|
||||||
|
|
||||||
|
Shows how to initialise and read the 2 external and 6 internal sensors of Motor 2040.
|
||||||
|
|
||||||
|
|
||||||
### Read Encoders
|
### Read Encoders
|
||||||
[read_encoders.py](read_encoders.py)
|
[read_encoders.py](read_encoders.py)
|
||||||
|
@ -74,26 +78,6 @@ A program that profiles the speed of a motor across its PWM
|
||||||
duty cycle range using the attached encoder for feedback.
|
duty cycle range using the attached encoder for feedback.
|
||||||
|
|
||||||
|
|
||||||
## Function Examples
|
|
||||||
|
|
||||||
### Read Sensors
|
|
||||||
[read_sensors.py](read_sensors.py)
|
|
||||||
TODO
|
|
||||||
Shows how to initialise and read the 6 external and 2 internal sensors of Motor 2040.
|
|
||||||
|
|
||||||
|
|
||||||
### Sensor Feedback
|
|
||||||
[sensor_feedback.py](sensor_feedback.py)
|
|
||||||
TODO
|
|
||||||
Show how to read the 6 external sensors and display their values on the neighbouring LEDs.
|
|
||||||
|
|
||||||
|
|
||||||
### Current Meter
|
|
||||||
[current_meter.py](current_meter.py)
|
|
||||||
TODO
|
|
||||||
An example of how to use Motor 2040's current measuring ability and display the value on the onboard LED bar.
|
|
||||||
|
|
||||||
|
|
||||||
### LED Rainbow
|
### LED Rainbow
|
||||||
[led_rainbow.py](led_rainbow.py)
|
[led_rainbow.py](led_rainbow.py)
|
||||||
|
|
||||||
|
@ -135,7 +119,12 @@ A demonstration of how a motor with an encoder can be used as a programmable rot
|
||||||
### Quad Position Wave
|
### Quad Position Wave
|
||||||
[quad_position_wave.py](quad_position_wave.py)
|
[quad_position_wave.py](quad_position_wave.py)
|
||||||
|
|
||||||
An demonstration of driving all four of Motor 2040's motor outputs between positions, with the help of their attached encoders and PID control.
|
A demonstration of driving all four of Motor 2040's motor outputs between positions, with the help of their attached encoders and PID control.
|
||||||
|
|
||||||
|
### Quad Velocity Sequence
|
||||||
|
[quad_velocity_sequence.py](quad_velocity_sequence.py)
|
||||||
|
|
||||||
|
A demonstration of driving all four of Motor 2040's motor outputs through a sequence of velocities, with the help of their attached encoders and PID control.
|
||||||
|
|
||||||
|
|
||||||
## Tuning Examples
|
## Tuning Examples
|
||||||
|
|
|
@ -1,90 +0,0 @@
|
||||||
import gc
|
|
||||||
import time
|
|
||||||
from machine import Pin
|
|
||||||
from pimoroni import Analog, AnalogMux, Button
|
|
||||||
from plasma import WS2812
|
|
||||||
from servo import ServoCluster, servo2040
|
|
||||||
|
|
||||||
"""
|
|
||||||
An example of how to use Servo 2040's current measuring
|
|
||||||
ability and display the value on the onboard LED bar.
|
|
||||||
|
|
||||||
Press "Boot" to exit the program.
|
|
||||||
|
|
||||||
NOTE: ServoCluster and Plasma WS2812 use the RP2040's PIO system,
|
|
||||||
and as such may have problems when running code multiple times.
|
|
||||||
If you encounter issues, try resetting your board.
|
|
||||||
"""
|
|
||||||
|
|
||||||
BRIGHTNESS = 0.4 # The brightness of the LEDs
|
|
||||||
UPDATES = 50 # How many times to update LEDs and Servos per second
|
|
||||||
MAX_CURRENT = 3.0 # The maximum current, in amps, to show on the meter
|
|
||||||
SAMPLES = 50 # The number of current measurements to take per reading
|
|
||||||
TIME_BETWEEN = 0.001 # The time between each current measurement
|
|
||||||
|
|
||||||
# Free up hardware resources ahead of creating a new ServoCluster
|
|
||||||
gc.collect()
|
|
||||||
|
|
||||||
# Create a servo cluster for pins 0 to 7, using PIO 0 and State Machine 0
|
|
||||||
START_PIN = servo2040.SERVO_1
|
|
||||||
END_PIN = servo2040.SERVO_8
|
|
||||||
servos = ServoCluster(pio=0, sm=0, pins=list(range(START_PIN, END_PIN + 1)))
|
|
||||||
|
|
||||||
# Set up the shared analog inputs
|
|
||||||
cur_adc = Analog(servo2040.SHARED_ADC, servo2040.CURRENT_GAIN,
|
|
||||||
servo2040.SHUNT_RESISTOR, servo2040.CURRENT_OFFSET)
|
|
||||||
|
|
||||||
# Set up the analog multiplexer, including the pin for controlling pull-up/pull-down
|
|
||||||
mux = AnalogMux(servo2040.ADC_ADDR_0, servo2040.ADC_ADDR_1, servo2040.ADC_ADDR_2,
|
|
||||||
muxed_pin=Pin(servo2040.SHARED_ADC))
|
|
||||||
|
|
||||||
# Create the LED bar, using PIO 1 and State Machine 0
|
|
||||||
led_bar = WS2812(servo2040.NUM_LEDS, 1, 0, servo2040.LED_DATA)
|
|
||||||
|
|
||||||
# Create the user button
|
|
||||||
user_sw = Button(servo2040.USER_SW)
|
|
||||||
|
|
||||||
# Start updating the LED bar
|
|
||||||
led_bar.start()
|
|
||||||
|
|
||||||
# Enable all servos (this puts them at the middle).
|
|
||||||
# The servos are not going to be moved, but are activated to give a current draw
|
|
||||||
servos.enable_all()
|
|
||||||
|
|
||||||
# Read sensors until the user button is pressed
|
|
||||||
while user_sw.raw() is not True:
|
|
||||||
|
|
||||||
# Select the current sense
|
|
||||||
mux.select(servo2040.CURRENT_SENSE_ADDR)
|
|
||||||
|
|
||||||
# Read the current sense several times and average the result
|
|
||||||
current = 0
|
|
||||||
for i in range(SAMPLES):
|
|
||||||
current += cur_adc.read_current()
|
|
||||||
time.sleep(TIME_BETWEEN)
|
|
||||||
current /= SAMPLES
|
|
||||||
|
|
||||||
# Print out the current sense value
|
|
||||||
print("Current =", round(current, 4))
|
|
||||||
|
|
||||||
# Convert the current to a percentage of the maximum we want to show
|
|
||||||
percent = (current / MAX_CURRENT)
|
|
||||||
|
|
||||||
# Update all the LEDs
|
|
||||||
for i in range(servo2040.NUM_LEDS):
|
|
||||||
# Calculate the LED's hue, with Red for high currents and Green for low
|
|
||||||
hue = (1.0 - i / (servo2040.NUM_LEDS - 1)) * 0.333
|
|
||||||
|
|
||||||
# Calculate the current level the LED represents
|
|
||||||
level = (i + 0.5) / servo2040.NUM_LEDS
|
|
||||||
# If the percent is above the level, light the LED, otherwise turn it off
|
|
||||||
if percent >= level:
|
|
||||||
led_bar.set_hsv(i, hue, 1.0, BRIGHTNESS)
|
|
||||||
else:
|
|
||||||
led_bar.set_hsv(i, hue, 1.0, 0.0)
|
|
||||||
|
|
||||||
# Disable the servos
|
|
||||||
servos.disable_all()
|
|
||||||
|
|
||||||
# Turn off the LED bar
|
|
||||||
led_bar.clear()
|
|
|
@ -7,7 +7,7 @@ from encoder import Encoder, MMME_CPR
|
||||||
from pimoroni import Button, PID, REVERSED_DIR
|
from pimoroni import Button, PID, REVERSED_DIR
|
||||||
|
|
||||||
"""
|
"""
|
||||||
An demonstration of driving all four of Motor 2040's motor outputs between
|
A demonstration of driving all four of Motor 2040's motor outputs between
|
||||||
positions, with the help of their attached encoders and PID control.
|
positions, with the help of their attached encoders and PID control.
|
||||||
|
|
||||||
Press "Boot" to exit the program.
|
Press "Boot" to exit the program.
|
||||||
|
|
|
@ -0,0 +1,162 @@
|
||||||
|
import gc
|
||||||
|
import time
|
||||||
|
from motor import Motor, motor2040
|
||||||
|
from encoder import Encoder, MMME_CPR
|
||||||
|
from pimoroni import Button, PID, REVERSED_DIR
|
||||||
|
|
||||||
|
"""
|
||||||
|
A demonstration of driving all four of Motor 2040's motor outputs through a
|
||||||
|
sequence of velocities, with the help of their attached encoders and PID control.
|
||||||
|
|
||||||
|
Press "Boot" to exit the program.
|
||||||
|
"""
|
||||||
|
|
||||||
|
GEAR_RATIO = 50 # The gear ratio of the motors
|
||||||
|
COUNTS_PER_REV = MMME_CPR * GEAR_RATIO # The counts per revolution of each motor's output shaft
|
||||||
|
|
||||||
|
SPEED_SCALE = 5.4 # The scaling to apply to each motor's speed to match its real-world speed
|
||||||
|
|
||||||
|
UPDATES = 100 # How many times to update the motor per second
|
||||||
|
UPDATE_RATE = 1 / UPDATES
|
||||||
|
TIME_FOR_EACH_MOVE = 2 # The time to travel between each value
|
||||||
|
UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES
|
||||||
|
PRINT_DIVIDER = 4 # How many of the updates should be printed (i.e. 2 would be every other update)
|
||||||
|
|
||||||
|
# PID values
|
||||||
|
VEL_KP = 30.0 # Velocity proportional (P) gain
|
||||||
|
VEL_KI = 0.0 # Velocity integral (I) gain
|
||||||
|
VEL_KD = 0.4 # Velocity derivative (D) gain
|
||||||
|
|
||||||
|
|
||||||
|
# Free up hardware resources ahead of creating a new Encoder
|
||||||
|
gc.collect()
|
||||||
|
|
||||||
|
# Create a list of motors with a given speed scale
|
||||||
|
MOTOR_PINS = [motor2040.MOTOR_A, motor2040.MOTOR_B, motor2040.MOTOR_C, motor2040.MOTOR_D]
|
||||||
|
motors = [Motor(pins, speed_scale=SPEED_SCALE) for pins in MOTOR_PINS]
|
||||||
|
|
||||||
|
# Create a list of encoders, using PIO 0, with the given counts per revolution
|
||||||
|
ENCODER_PINS = [motor2040.ENCODER_A, motor2040.ENCODER_B, motor2040.ENCODER_C, motor2040.ENCODER_D]
|
||||||
|
ENCODER_NAMES = ["RR", "RL", "FL", "FR"]
|
||||||
|
encoders = [Encoder(0, i, ENCODER_PINS[i], counts_per_rev=COUNTS_PER_REV, count_microsteps=True) for i in range(motor2040.NUM_MOTORS)]
|
||||||
|
|
||||||
|
# Wheel friendly names
|
||||||
|
FL = 2
|
||||||
|
FR = 3
|
||||||
|
RL = 1
|
||||||
|
RR = 0
|
||||||
|
|
||||||
|
# Reverse the direction of the B and D motors and encoders
|
||||||
|
motors[FL].direction(REVERSED_DIR)
|
||||||
|
motors[RL].direction(REVERSED_DIR)
|
||||||
|
encoders[FL].direction(REVERSED_DIR)
|
||||||
|
encoders[RL].direction(REVERSED_DIR)
|
||||||
|
|
||||||
|
# Create the user button
|
||||||
|
user_sw = Button(motor2040.USER_SW)
|
||||||
|
|
||||||
|
# Create PID objects for position control
|
||||||
|
vel_pids = [PID(VEL_KP, VEL_KI, VEL_KD, UPDATE_RATE) for i in range(motor2040.NUM_MOTORS)]
|
||||||
|
|
||||||
|
# Enable the motor to get started
|
||||||
|
for m in motors:
|
||||||
|
m.enable()
|
||||||
|
|
||||||
|
|
||||||
|
update = 0
|
||||||
|
print_count = 0
|
||||||
|
|
||||||
|
|
||||||
|
# Helper functions for driving in common directions
|
||||||
|
def drive_forward(speed):
|
||||||
|
vel_pids[FL].setpoint = speed
|
||||||
|
vel_pids[FR].setpoint = speed
|
||||||
|
vel_pids[RL].setpoint = speed
|
||||||
|
vel_pids[RR].setpoint = speed
|
||||||
|
|
||||||
|
|
||||||
|
def turn_right(speed):
|
||||||
|
vel_pids[FL].setpoint = speed
|
||||||
|
vel_pids[FR].setpoint = -speed
|
||||||
|
vel_pids[RL].setpoint = speed
|
||||||
|
vel_pids[RR].setpoint = -speed
|
||||||
|
|
||||||
|
|
||||||
|
def strafe_right(speed):
|
||||||
|
vel_pids[FL].setpoint = speed
|
||||||
|
vel_pids[FR].setpoint = -speed
|
||||||
|
vel_pids[RL].setpoint = -speed
|
||||||
|
vel_pids[RR].setpoint = speed
|
||||||
|
|
||||||
|
|
||||||
|
def stop():
|
||||||
|
vel_pids[FL].setpoint = 0
|
||||||
|
vel_pids[FR].setpoint = 0
|
||||||
|
vel_pids[RL].setpoint = 0
|
||||||
|
vel_pids[RR].setpoint = 0
|
||||||
|
|
||||||
|
|
||||||
|
sequence = 0
|
||||||
|
|
||||||
|
captures = [None] * motor2040.NUM_MOTORS
|
||||||
|
|
||||||
|
# Continually move the motor until the user button is pressed
|
||||||
|
while user_sw.raw() is not True:
|
||||||
|
|
||||||
|
# Capture the state of all the encoders
|
||||||
|
for i in range(motor2040.NUM_MOTORS):
|
||||||
|
captures[i] = encoders[i].capture()
|
||||||
|
|
||||||
|
# Calculate how far along this movement to be
|
||||||
|
percent_along = min(update / UPDATES_PER_MOVE, 1.0)
|
||||||
|
|
||||||
|
for i in range(motor2040.NUM_MOTORS):
|
||||||
|
# Calculate the acceleration to apply to the motor to move it closer to the velocity setpoint
|
||||||
|
accel = vel_pids[i].calculate(captures[i].revolutions_per_second)
|
||||||
|
|
||||||
|
# Accelerate or decelerate the motor
|
||||||
|
motors[i].speed(motors[i].speed() + (accel * UPDATE_RATE))
|
||||||
|
|
||||||
|
# Print out the current motor values, but only on every multiple
|
||||||
|
if print_count == 0:
|
||||||
|
for i in range(len(motors)):
|
||||||
|
print(ENCODER_NAMES[i], "=", captures[i].revolutions_per_second, end=", ")
|
||||||
|
print()
|
||||||
|
|
||||||
|
# Increment the print count, and wrap it
|
||||||
|
print_count = (print_count + 1) % PRINT_DIVIDER
|
||||||
|
|
||||||
|
update += 1 # Move along in time
|
||||||
|
|
||||||
|
# Have we reached the end of this movement?
|
||||||
|
if update >= UPDATES_PER_MOVE:
|
||||||
|
update = 0 # Reset the counter
|
||||||
|
|
||||||
|
# Move on to the next part of the sequence
|
||||||
|
sequence += 1
|
||||||
|
|
||||||
|
# Loop the sequence back around
|
||||||
|
if sequence >= 7:
|
||||||
|
sequence = 0
|
||||||
|
|
||||||
|
# Set the motor speeds, based on the sequence
|
||||||
|
if sequence == 0:
|
||||||
|
drive_forward(1.0)
|
||||||
|
elif sequence == 1:
|
||||||
|
drive_forward(-1.0)
|
||||||
|
elif sequence == 2:
|
||||||
|
turn_right(1.0)
|
||||||
|
elif sequence == 3:
|
||||||
|
turn_right(-1.0)
|
||||||
|
elif sequence == 4:
|
||||||
|
strafe_right(1.0)
|
||||||
|
elif sequence == 5:
|
||||||
|
strafe_right(-1.0)
|
||||||
|
elif sequence == 6:
|
||||||
|
stop()
|
||||||
|
|
||||||
|
time.sleep(UPDATE_RATE)
|
||||||
|
|
||||||
|
# Stop all the motors
|
||||||
|
for m in motors:
|
||||||
|
m.disable()
|
|
@ -1,32 +1,35 @@
|
||||||
import time
|
import time
|
||||||
from machine import Pin
|
from machine import Pin
|
||||||
from pimoroni import Analog, AnalogMux, Button
|
from pimoroni import Analog, AnalogMux, Button
|
||||||
from servo import servo2040
|
from motor import motor2040
|
||||||
|
|
||||||
"""
|
"""
|
||||||
Shows how to initialise and read the 6 external
|
Shows how to initialise and read the 2 external
|
||||||
and 2 internal sensors of Servo 2040.
|
and 6 internal sensors of Motor 2040.
|
||||||
|
|
||||||
Press "Boot" to exit the program.
|
Press "Boot" to exit the program.
|
||||||
"""
|
"""
|
||||||
|
|
||||||
# Set up the shared analog inputs
|
# Set up the shared analog inputs
|
||||||
sen_adc = Analog(servo2040.SHARED_ADC)
|
sen_adc = Analog(motor2040.SHARED_ADC)
|
||||||
vol_adc = Analog(servo2040.SHARED_ADC, servo2040.VOLTAGE_GAIN)
|
vol_adc = Analog(motor2040.SHARED_ADC, motor2040.VOLTAGE_GAIN)
|
||||||
cur_adc = Analog(servo2040.SHARED_ADC, servo2040.CURRENT_GAIN,
|
cur_adc = Analog(motor2040.SHARED_ADC, motor2040.CURRENT_GAIN,
|
||||||
servo2040.SHUNT_RESISTOR, servo2040.CURRENT_OFFSET)
|
motor2040.SHUNT_RESISTOR, motor2040.CURRENT_OFFSET)
|
||||||
|
|
||||||
# Set up the analog multiplexer, including the pin for controlling pull-up/pull-down
|
# Set up the analog multiplexer, including the pin for controlling pull-up/pull-down
|
||||||
mux = AnalogMux(servo2040.ADC_ADDR_0, servo2040.ADC_ADDR_1, servo2040.ADC_ADDR_2,
|
mux = AnalogMux(motor2040.ADC_ADDR_0, motor2040.ADC_ADDR_1, motor2040.ADC_ADDR_2,
|
||||||
muxed_pin=Pin(servo2040.SHARED_ADC))
|
muxed_pin=Pin(motor2040.SHARED_ADC))
|
||||||
|
|
||||||
# Set up the sensor addresses and have them pulled down by default
|
# Set up the sensor addresses and have them pulled down by default
|
||||||
sensor_addrs = list(range(servo2040.SENSOR_1_ADDR, servo2040.SENSOR_6_ADDR + 1))
|
sensor_addrs = list(range(motor2040.SENSOR_1_ADDR, motor2040.SENSOR_2_ADDR + 1))
|
||||||
for addr in sensor_addrs:
|
for addr in sensor_addrs:
|
||||||
mux.configure_pull(addr, Pin.PULL_DOWN)
|
mux.configure_pull(addr, Pin.PULL_DOWN)
|
||||||
|
|
||||||
|
# Set up the pull-up for the fault sense
|
||||||
|
mux.configure_pull(motor2040.FAULT_SENSE_ADDR, Pin.PULL_UP)
|
||||||
|
|
||||||
# Create the user button
|
# Create the user button
|
||||||
user_sw = Button(servo2040.USER_SW)
|
user_sw = Button(motor2040.USER_SW)
|
||||||
|
|
||||||
|
|
||||||
# Read sensors until the user button is pressed
|
# Read sensors until the user button is pressed
|
||||||
|
@ -38,11 +41,16 @@ while user_sw.raw() is not True:
|
||||||
print("S", i + 1, " = ", round(sen_adc.read_voltage(), 3), sep="", end=", ")
|
print("S", i + 1, " = ", round(sen_adc.read_voltage(), 3), sep="", end=", ")
|
||||||
|
|
||||||
# Read the voltage sense and print the value
|
# Read the voltage sense and print the value
|
||||||
mux.select(servo2040.VOLTAGE_SENSE_ADDR)
|
mux.select(motor2040.VOLTAGE_SENSE_ADDR)
|
||||||
print("Voltage =", round(vol_adc.read_voltage(), 4), end=", ")
|
print("Voltage =", round(vol_adc.read_voltage(), 4), end=", ")
|
||||||
|
|
||||||
# Read the current sense and print the value
|
# Read the current sense's of each motor and print the value
|
||||||
mux.select(servo2040.CURRENT_SENSE_ADDR)
|
for i in range(motor2040.NUM_MOTORS):
|
||||||
print("Current =", round(cur_adc.read_current(), 4))
|
mux.select(i + motor2040.CURRENT_SENSE_A_ADDR)
|
||||||
|
print("C", i + 1, "=", cur_adc.read_current(), sep="", end=", ")
|
||||||
|
|
||||||
|
# Read the fault sense and print the value
|
||||||
|
mux.select(motor2040.FAULT_SENSE_ADDR)
|
||||||
|
print("Fault =", not mux.read())
|
||||||
|
|
||||||
time.sleep(0.5)
|
time.sleep(0.5)
|
||||||
|
|
|
@ -1,58 +0,0 @@
|
||||||
import time
|
|
||||||
from machine import Pin
|
|
||||||
from pimoroni import Analog, AnalogMux, Button
|
|
||||||
from plasma import WS2812
|
|
||||||
from servo import servo2040
|
|
||||||
|
|
||||||
"""
|
|
||||||
Show how to read the 6 external sensors and
|
|
||||||
display their values on the neighbouring LEDs.
|
|
||||||
|
|
||||||
Press "Boot" to exit the program.
|
|
||||||
|
|
||||||
NOTE: Plasma WS2812 uses the RP2040's PIO system, and as
|
|
||||||
such may have problems when running code multiple times.
|
|
||||||
If you encounter issues, try resetting your board.
|
|
||||||
"""
|
|
||||||
|
|
||||||
BRIGHTNESS = 0.4 # The brightness of the LEDs
|
|
||||||
UPDATES = 50 # How many times to update LEDs and Servos per second
|
|
||||||
|
|
||||||
# Set up the shared analog inputs
|
|
||||||
sen_adc = Analog(servo2040.SHARED_ADC)
|
|
||||||
|
|
||||||
# Set up the analog multiplexer, including the pin for controlling pull-up/pull-down
|
|
||||||
mux = AnalogMux(servo2040.ADC_ADDR_0, servo2040.ADC_ADDR_1, servo2040.ADC_ADDR_2,
|
|
||||||
muxed_pin=Pin(servo2040.SHARED_ADC))
|
|
||||||
|
|
||||||
# Set up the sensor addresses and have them pulled down by default
|
|
||||||
sensor_addrs = list(range(servo2040.SENSOR_1_ADDR, servo2040.SENSOR_6_ADDR + 1))
|
|
||||||
for addr in sensor_addrs:
|
|
||||||
mux.configure_pull(addr, Pin.PULL_DOWN)
|
|
||||||
|
|
||||||
# Create the LED bar, using PIO 1 and State Machine 0
|
|
||||||
led_bar = WS2812(servo2040.NUM_LEDS, 1, 0, servo2040.LED_DATA)
|
|
||||||
|
|
||||||
# Create the user button
|
|
||||||
user_sw = Button(servo2040.USER_SW)
|
|
||||||
|
|
||||||
# Start updating the LED bar
|
|
||||||
led_bar.start()
|
|
||||||
|
|
||||||
|
|
||||||
# Read sensors until the user button is pressed
|
|
||||||
while user_sw.raw() is not True:
|
|
||||||
|
|
||||||
# Read each sensor in turn and print its voltage
|
|
||||||
for i in range(len(sensor_addrs)):
|
|
||||||
mux.select(sensor_addrs[i])
|
|
||||||
sensor_voltage = sen_adc.read_voltage()
|
|
||||||
|
|
||||||
# Calculate the LED's hue, with Green for high voltages and Blue for low
|
|
||||||
hue = (2.0 - (sensor_voltage / 3.3)) * 0.333
|
|
||||||
led_bar.set_hsv(i, hue, 1.0, BRIGHTNESS)
|
|
||||||
|
|
||||||
print("S", i + 1, " = ", round(sensor_voltage, 3), sep="", end=", ")
|
|
||||||
print()
|
|
||||||
|
|
||||||
time.sleep(1.0 / UPDATES)
|
|
|
@ -177,10 +177,9 @@ typedef struct _mp_obj_float_t {
|
||||||
mp_obj_base_t base;
|
mp_obj_base_t base;
|
||||||
mp_float_t value;
|
mp_float_t value;
|
||||||
} mp_obj_float_t;
|
} mp_obj_float_t;
|
||||||
//TODO confirm below numbers are correct
|
|
||||||
mp_obj_float_t motor2040_shunt_resistor = {{&mp_type_float}, 0.47f};
|
mp_obj_float_t motor2040_shunt_resistor = {{&mp_type_float}, 0.47f};
|
||||||
mp_obj_float_t motor2040_voltage_gain = {{&mp_type_float}, 3.9f / 13.9f};
|
mp_obj_float_t motor2040_voltage_gain = {{&mp_type_float}, 3.9f / 13.9f};
|
||||||
mp_obj_float_t motor2040_current_offset = {{&mp_type_float}, -0.02f};
|
mp_obj_float_t motor2040_current_offset = {{&mp_type_float}, -0.005f};
|
||||||
|
|
||||||
|
|
||||||
/***** Globals Table *****/
|
/***** Globals Table *****/
|
||||||
|
@ -231,7 +230,7 @@ STATIC const mp_rom_map_elem_t motor2040_globals_table[] = {
|
||||||
{ MP_ROM_QSTR(MP_QSTR_SENSOR_2_ADDR), MP_ROM_INT(0b111) },
|
{ MP_ROM_QSTR(MP_QSTR_SENSOR_2_ADDR), MP_ROM_INT(0b111) },
|
||||||
{ MP_ROM_QSTR(MP_QSTR_NUM_SENSORS), MP_ROM_INT(2) },
|
{ MP_ROM_QSTR(MP_QSTR_NUM_SENSORS), MP_ROM_INT(2) },
|
||||||
{ MP_ROM_QSTR(MP_QSTR_SHUNT_RESISTOR), MP_ROM_PTR(&motor2040_shunt_resistor) },
|
{ MP_ROM_QSTR(MP_QSTR_SHUNT_RESISTOR), MP_ROM_PTR(&motor2040_shunt_resistor) },
|
||||||
{ MP_ROM_QSTR(MP_QSTR_CURRENT_GAIN), MP_ROM_INT(5) },
|
{ MP_ROM_QSTR(MP_QSTR_CURRENT_GAIN), MP_ROM_INT(1) },
|
||||||
{ MP_ROM_QSTR(MP_QSTR_VOLTAGE_GAIN), MP_ROM_PTR(&motor2040_voltage_gain) },
|
{ MP_ROM_QSTR(MP_QSTR_VOLTAGE_GAIN), MP_ROM_PTR(&motor2040_voltage_gain) },
|
||||||
{ MP_ROM_QSTR(MP_QSTR_CURRENT_OFFSET), MP_ROM_PTR(&motor2040_current_offset) },
|
{ MP_ROM_QSTR(MP_QSTR_CURRENT_OFFSET), MP_ROM_PTR(&motor2040_current_offset) },
|
||||||
};
|
};
|
||||||
|
|
|
@ -79,6 +79,12 @@ class AnalogMux:
|
||||||
else:
|
else:
|
||||||
self.pulls[address] = pull
|
self.pulls[address] = pull
|
||||||
|
|
||||||
|
def read(self):
|
||||||
|
if self.muxed_pin is not None:
|
||||||
|
return self.muxed_pin.value()
|
||||||
|
else:
|
||||||
|
raise RuntimeError("there is no muxed pin assigned to this mux")
|
||||||
|
|
||||||
|
|
||||||
class Button:
|
class Button:
|
||||||
def __init__(self, button, invert=True, repeat_time=200, hold_time=1000):
|
def __init__(self, button, invert=True, repeat_time=200, hold_time=1000):
|
||||||
|
|
Ładowanie…
Reference in New Issue