Lunch-Duty CNC Machine
This week my group created a machine that cleans tables automatically. See the group page here. Below is my personal contribution.
VL53L1X Sensor
I found this video from this forum which I'll use to talk to the VL53L1X PCB1540 sensor with MicroPython.
from enum import Enum
class RAG_POSITION(Enum):
DOWN = 1
UP = 2
class RAG_ROTATION(Enum):
SPINNING = 3
IDLE = 4
class DIRECTION(Enum):
FORWARD = True
BACK = False
CLEANING_DIRECTION = DIRECTION.FORWARD
def start_condition(): # -> bool
"""
determines whether the on button/bluetooth start connection/etc has been pressed
"""
pass
def wait_for_startup(): # -> void
while True:
if start_condition():
main_flow()
def set_rag_position(rag_pos): # -> void
"""
- moves rags to position RAG_POSITION.UP or RAG_POSITION.DOWN
- doesn't end the function until movement complete
"""
pass
def set_rag_rotation(rag_rot): # -> void
"""
- moves rags to position RAG_ROTATION.SPINNING or RAG_ROTATION.IDLE
- doesn't end the function until state achieved
"""
pass
def hit_edge(sensor_dir): # -> bool
"""
checks the the FORWARD or BACKWARD sensor (depending on `sensor_dir`) senses a table, and returns False if there's a table and True if not
"""
pass
def move_until_edge(current_dir): # -> void
"""
- begins wheel movement in `current_dir`
- waits for sensors to indicate the end of a table
- returns opposite direction
"""
while not hit_edge(current_dir): # don't have to worry about the current edge since it's a different sensor
delay(0.05)
"""
- stops wheel movement
"""
return not current_dir
def main_flow():
current_direction = CLEANING_DIRECTION
set_rag_position(RAG_POSITION.DOWN)
set_rag_rotation(RAG_ROTATION.SPINNING)
current_direction = move_until_edge(current_direction)
set_rag_rotation(RAG_ROTATION.IDLE)
set_rag_position(RAG_POSITION.UP)
move_until_edge(current_direction)
if __name__ == "__main__":
while True:
wait_for_startup()
I also had an idea that an emergency stop button would probably be a good idea.
Networking With The Sensor
I soldered the sensor.
Then I asked ChatGPT 4.0 to point me to some resources.
Prompt
Point me to some resources to control a PCB1540 VL53L1X sensor with a raspberry pi pico w using micropython
Response
The VL53L1X is a Time-of-Flight (ToF) ranging sensor that uses STMicroelectronics' patented FlightSense technology. It can be used with a Raspberry Pi Pico to measure distance accurately. Here are some resources to help you get started with controlling a PCB1540 VL53L1X sensor using MicroPython on a Raspberry Pi Pico:
MicroPython for Raspberry Pi Pico:
To get started with MicroPython on your Raspberry Pi Pico, visit the official documentation:
https://datasheets.raspberrypi.org/pico/raspberry-pi-pico-python-sdk.pdf
VL53L1X Datasheet:
To understand the VL53L1X sensor's capabilities and specifications, refer to its datasheet:
https://www.st.com/resource/en/datasheet/vl53l1x.pdf
MicroPython VL53L1X library:
You'll need a MicroPython library for the VL53L1X sensor. Here is a library you can use:
https://github.com/pimoroni/pimoroni-pico/blob/main/micropython/modules/pimoroni_pico/vl53l1x.py
Download the vl53l1x.py file and copy it to your Raspberry Pi Pico's storage.
Wiring:
Connect the VL53L1X sensor to the Raspberry Pi Pico as follows:
VCC to 3.3V
GND to GND
SDA to GPIO0 (I2C0_SDA)
SCL to GPIO1 (I2C0_SCL)
Example code:
After connecting the sensor and copying the library, you can use the following example code to get started:
Copy code
import machine
import utime
from vl53l1x import VL53L1X
# Configure I2C
i2c = machine.I2C(0, scl=machine.Pin(1), sda=machine.Pin(0), freq=400000)
# Initialize the VL53L1X sensor
sensor = VL53L1X(i2c)
# Set the distance mode (1=Short, 2=Medium, 3=Long)
sensor.set_distance_mode(2)
while True:
# Get the distance measurement
distance = sensor.get_distance()
print("Distance:", distance, "mm")
# Wait for a second before taking the next measurement
utime.sleep(1)
This example initializes the I2C interface, sets up the VL53L1X sensor, and takes distance measurements every second, printing the results.
Remember to always consult the official documentation for the Raspberry Pi Pico and the VL53L1X sensor for the most up-to-date and accurate information. Good luck with your project!
I tried doing this, based off of page 4 of this datasheet and this Raspberry Pi Pico W pinout sheet, connecting 6 on the breakout board to 3V+, 5 to GND, 2 to SDA (GPIO 0), 4 to SCL (GPIO 1). It started smoking, and we realized after testing with the multimeter that all of these board had continuity between the supposed VCC and GND.
I also realized that, in the future, we'll need to calibrate the sensor values for distances on and off the table.
I realized that maybe the weird symbols on the pinout sheet for the breakout board meant that 7 and 8 are actually power and ground, and these pins didn't have continuity. I plugged it in (VBUS instead of 3V+) this time since Merritt Backerman taught me that is 5V+, as this board requires.
I installed MicroPython for the Pico W in Thonny on the Pico (holding the bootloader button as I plugged it in).
Then I installed the vl53l1x library at Tools > Manage packages....
I got a warning that it doesn't look like MicroPython/CircuitPython but installed it anyway since the page said Python.
I got this error.
install VL53L1X
Preparing working environment ...
Collecting pip==22.2.2
Using cached pip-22.2.2-py3-none-any.whl (2.0 MB)
Collecting setuptools==65.4.1
Downloading setuptools-65.4.1-py3-none-any.whl (1.2 MB)
---------------------------------------- 1.2/1.2 MB 4.1 MB/s eta 0:00:00
Collecting wheel==0.38.4
Using cached wheel-0.38.4-py3-none-any.whl (36 kB)
Installing collected packages: wheel, setuptools, pip
Attempting uninstall: setuptools
Found existing installation: setuptools 65.5.0
Uninstalling setuptools-65.5.0:
Successfully uninstalled setuptools-65.5.0
Attempting uninstall: pip
Found existing installation: pip 22.3.1
Uninstalling pip-22.3.1:
Successfully uninstalled pip-22.3.1
Successfully installed pip-22.2.2 setuptools-65.4.1 wheel-0.38.4
Looking in indexes: http://127.0.0.1:36628
127.0.0.1 - - [29/Mar/2023 18:30:26] "GET /vl53l1x/ HTTP/1.1" 200 -
Collecting VL53L1X
127.0.0.1 - - [29/Mar/2023 18:30:26] "GET /vl53l1x/VL53L1X-0.0.5.tar.gz/ HTTP/1.1" 200 -
Downloading http://127.0.0.1:36628/vl53l1x/VL53L1X-0.0.5.tar.gz/
- 182.3 kB ? 0:00:00
Installing build dependencies: started
127.0.0.1 - - [29/Mar/2023 18:30:27] "GET /setuptools/ HTTP/1.1" 200 -
127.0.0.1 - - [29/Mar/2023 18:30:28] "GET /setuptools/setuptools-67.6.1-py3-none-any.whl/ HTTP/1.1" 200 -
127.0.0.1 - - [29/Mar/2023 18:30:28] "GET /wheel/ HTTP/1.1" 200 -
127.0.0.1 - - [29/Mar/2023 18:30:28] "GET /wheel/wheel-0.40.0-py3-none-any.whl/ HTTP/1.1" 200 -
127.0.0.1 - - [29/Mar/2023 18:30:31] "GET /pip/ HTTP/1.1" 200 -
Installing build dependencies: finished with status 'done'
Getting requirements to build wheel: started
Getting requirements to build wheel: finished with status 'done'
Preparing metadata (pyproject.toml): started
Preparing metadata (pyproject.toml): finished with status 'done'
127.0.0.1 - - [29/Mar/2023 18:30:33] "GET /smbus2/ HTTP/1.1" 200 -
Collecting smbus2
127.0.0.1 - - [29/Mar/2023 18:30:33] "GET /smbus2/smbus2-0.4.2-py2.py3-none-any.whl/ HTTP/1.1" 200 -
Downloading http://127.0.0.1:36628/smbus2/smbus2-0.4.2-py2.py3-none-any.whl/
- 11.5 kB ? 0:00:00
Building wheels for collected packages: VL53L1X
Building wheel for VL53L1X (pyproject.toml): started
Building wheel for VL53L1X (pyproject.toml): finished with status 'error'
error: subprocess-exited-with-error
Building wheel for VL53L1X (pyproject.toml) did not run successfully.
exit code: 1
[9 lines of output]
running bdist_wheel
running build
running build_py
creating build
creating build\lib.win-amd64-cpython-310
copying python\VL53L1X.py -> build\lib.win-amd64-cpython-310
running build_ext
building 'vl53l1x_python' extension
error: Microsoft Visual C++ 14.0 or greater is required. Get it with "Microsoft C++ Build Tools": https://visualstudio.microsoft.com/visual-cpp-build-tools/
[end of output]
note: This error originates from a subprocess, and is likely not a problem with pip.
ERROR: Failed building wheel for VL53L1X
Failed to build VL53L1X
ERROR: Could not build wheels for VL53L1X, which is required to install pyproject.toml-based projects
Error Command '['C:\\Users\\adams\\AppData\\Local\\pipkin\\cache\\workspaces\\62544052081391d01bd1e406413cd91d\\Scripts\\python.exe', '-I', '-m', 'pip', '--no-color', '--disable-pip-version-check', '--trusted-host', '127.0.0.1', 'install', '--no-compile', '--use-pep517', '--upgrade-strategy', 'only-if-needed', 'VL53L1X', '--index-url', 'http://127.0.0.1:36628']' returned non-zero exit status 1.
I deleted ChatGPT's code.
import machine
import utime
from vl53l1x import VL53L1X
# Configure I2C
i2c = machine.I2C(0, scl=machine.Pin(1), sda=machine.Pin(0), freq=400000)
# Initialize the VL53L1X sensor
sensor = VL53L1X(i2c)
# Set the distance mode (1=Short, 2=Medium, 3=Long)
sensor.set_distance_mode(2)
while True:
# Get the distance measurement
distance = sensor.get_distance()
print("Distance:", distance, "mm")
# Wait for a second before taking the next measurement
utime.sleep(1)
And used this tutorial's code based on their website but using the wiring I described. I got this error.
MPY: soft reboot
Traceback (most recent call last):
File "main.py", line 4, in <module>
File "PiicoDev_VL53L1X.py", line 111, in __init__
File "PiicoDev_VL53L1X.py", line 135, in reset
File "PiicoDev_VL53L1X.py", line 124, in writeReg
OSError: [Errno 5] EIO
MicroPython v1.19.1-994-ga4672149b on 2023-03-29; Raspberry Pi Pico W with RP2040
Type "help()" for more information.
Decided to take a break and try to do it with an Arduino Uno, since I learned last week that I need to get a basic tutorial to work.
Following this tutorial I downloaded this repo as a ZIP file and included as a ZIP library in the Arduino IDE. I then restarted the Arduino IDE and saw the example!
I was just getting this output, though.
VL53L1X Qwiic Test
v�
Realized Baud Rate in the Serial Monitor wasn't 115200 like the program said.
This pinout sheet should A5 and A4 weren't the right pins, so I changed them. I also realized I had been connecting to the wrong end of the sensor board. Merritt Backerman sent me this library and I used the Continuous example, which worked finally (with the sensor that was smoking before)!!!
I also had a big learning that, oftentime finding code examples for MicroPython is difficult, and instead searching for CircuitPython can lead to better results.
Considering the amount of issues using the Pico, I decided to use the Arduino Uno.
I followed this tutorial for a wiring setup and learned my driver is called a ULN2003 driver board.
I used this code, which worked!
//Includes the Arduino Stepper Library
#include <Stepper.h>
// Defines the number of steps per rotation
const int stepsPerRevolution = 2038;
// Creates an instance of stepper class
// Pins entered in sequence IN1-IN3-IN2-IN4 for proper step sequence
Stepper myStepper = Stepper(stepsPerRevolution, 8, 10, 9, 11);
void setup() {
// Nothing to do (Stepper Library sets pins as outputs)
}
void loop() {
// Rotate CW slowly at 5 RPM
myStepper.setSpeed(5);
myStepper.step(stepsPerRevolution);
delay(1000);
// Rotate CCW quickly at 10 RPM
myStepper.setSpeed(10);
myStepper.step(-stepsPerRevolution);
delay(1000);
}
I added the program together to the following which stops the motor when there is an object close to the sensor, whick works!
//Includes the Arduino Stepper Library
#include <Stepper.h>
// Defines the number of steps per rotation
const int stepsPerRevolution = 2038;
// Creates an instance of stepper class
// Pins entered in sequence IN1-IN3-IN2-IN4 for proper step sequence
Stepper myStepper = Stepper(stepsPerRevolution, 8, 10, 9, 11);
/*
This example shows how to take simple range measurements with the VL53L1X. The
range readings are in units of mm.
*/
#include <Wire.h>
#include <VL53L1X.h>
VL53L1X sensor;
void setup()
{
while (!Serial) {}
Serial.begin(115200);
Wire.begin();
Wire.setClock(400000); // use 400 kHz I2C
sensor.setTimeout(500);
if (!sensor.init())
{
Serial.println("Failed to detect and initialize sensor!");
while (1);
}
// Use long distance mode and allow up to 50000 us (50 ms) for a measurement.
// You can change these settings to adjust the performance of the sensor, but
// the minimum timing budget is 20 ms for short distance mode and 33 ms for
// medium and long distance modes. See the VL53L1X datasheet for more
// information on range and timing limits.
sensor.setDistanceMode(VL53L1X::Long);
sensor.setMeasurementTimingBudget(50000);
// Start continuous readings at a rate of one measurement every 50 ms (the
// inter-measurement period). This period should be at least as long as the
// timing budget.
sensor.startContinuous(50);
// Nothing to do (Stepper Library sets pins as outputs)
}
void loop() {
if (sensor.read() < 100) return;
Serial.println(sensor.read());
// Rotate CW slowly at 5 RPM
myStepper.setSpeed(10);
myStepper.step(100);
}
I also got DC motors to work with a driver. Here's the final code.
#include <SparkFun_TB6612.h>
// Pins for all inputs, keep in mind the PWM defines must be on PWM pins
// the default pins listed are the ones used on the Redbot (ROB-12097) with
// the exception of STBY which the Redbot controls with a physical switch
#define AIN1 2
#define BIN1 7
#define AIN2 4
#define BIN2 8
#define PWMA 5
#define PWMB 6
#define STBY 9
// these constants are used to allow you to make your motor configuration
// line up with function names like forward. Value can be 1 or -1
const int offsetA = 1;
const int offsetB = 1;
// Initializing motors. The library will allow you to initialize as many
// motors as you have memory for. If you are using functions like forward
// that take 2 motors as arguements you can either write new functions or
// call the function more than once.
Motor motor1 = Motor(AIN1, AIN2, PWMA, offsetA, STBY);
Motor motor2 = Motor(BIN1, BIN2, PWMB, offsetB, STBY);
//Includes the Arduino Stepper Library
#include <Stepper.h>
// Defines the number of steps per rotation
const int stepsPerRevolution = 2038;
// Creates an instance of stepper class
// Pins entered in sequence IN1-IN3-IN2-IN4 for proper step sequence
Stepper myStepper = Stepper(stepsPerRevolution, 8, 10, 9, 11);
/*
This example shows how to take simple range measurements with the VL53L1X. The
range readings are in units of mm.
*/
#include <Wire.h>
#include <VL53L1X.h>
VL53L1X sensor;
void setup()
{
while (!Serial) {}
Serial.begin(115200);
Wire.begin();
Wire.setClock(400000); // use 400 kHz I2C
sensor.setTimeout(500);
if (!sensor.init())
{
Serial.println("Failed to detect and initialize sensor!");
while (1);
}
// Use long distance mode and allow up to 50000 us (50 ms) for a measurement.
// You can change these settings to adjust the performance of the sensor, but
// the minimum timing budget is 20 ms for short distance mode and 33 ms for
// medium and long distance modes. See the VL53L1X datasheet for more
// information on range and timing limits.
sensor.setDistanceMode(VL53L1X::Long);
sensor.setMeasurementTimingBudget(50000);
// Start continuous readings at a rate of one measurement every 50 ms (the
// inter-measurement period). This period should be at least as long as the
// timing budget.
sensor.startContinuous(50);
// Nothing to do (Stepper Library sets pins as outputs)
}
void loop() {
forward(motor1, motor2, 150);
while (sensor.read() < 100) {
Serial.print("going "); Serial.println(sensor.read());
}
brake(motor1, motor2);
while (sensor.read() > 100) {
Serial.print("waiting "); Serial.println(sensor.read());
}
//Serial.println(sensor.read());
// Rotate CW slowly at 5 RPM
/*myStepper.setSpeed(10);
myStepper.step(100);*/
}
I skimmed this and this as well as installed this Arduino library.
- SCL arduino - 2 sensor
- SDA arduino - 4 sensor
- 5V arduino - 5 sensor
-
GND arduino - 6 sensor
-
GND - dc motors' grounds, power supply ground
- BO1 - dc motor 1 power
- AO1 - dc motor 2 power
- MV - power supply voltage
- PWMA - 5
- AI2 - 4
- AI1 - 2
- STBY - 9
- BI1 - 7
- BI2 - 8
- PWMB - 6
Stepper Motor
Following this tutorial I wired the L298n Driver and Nema-17 stepper as follows.
I knew that the two adjacent wires on the stepper were of the same phase because I stuck in LED between then and turned the motor, which caused the LED to turn on.
Arduino 1-4 - Driver int1-int4 Arduino gnd - power supply gnd - driver gnd (middle of three screw-in-pins) Driver two - two on each side of the four of the stepper motor 5V (right of three) driver - 5v arduino (if arduino not plugged in) 12V drivr (left of three) - 12v power supply arduino A0 - either GND or 3V or 5V arduino to change speed
// Include the Arduino Stepper Library
#include <Stepper.h>
// Define Constants
// Number of steps per output rotation
const int STEPS_PER_REV = 200;
const int SPEED_CONTROL = A0;
// The pins used are 1,2,3,4
// Connected to L298N Motor Driver In1, In2, In3, In4
// Pins entered in sequence 1-2-3-4 for proper step sequencing
Stepper stepper_NEMA17(STEPS_PER_REV, 1, 2, 3, 4);
void setup() {
Serial.begin(9600);
// nothing to do inside the void setup
}
void loop() {
// read the sensor value:
int sensorReading = analogRead(SPEED_CONTROL);
// map it to a range from 0 to 100:
int motorSpeed = map(sensorReading, 0, 1023, 0, 100);
// set the motor speed:
if (motorSpeed > 0) {
Serial.println(motorSpeed);
stepper_NEMA17.setSpeed(motorSpeed);
// step 1/100 of a revolution:
stepper_NEMA17.step(STEPS_PER_REV / 100);
}
}
The motor would vibrate weirdly and not turn sometimes (when I was adjusting both grounds connecting into the screw-in-pin) and other times did nothing. I tried hooking it up directly to the Arduino with the Arduino Stepper example sketch, but it just vibrated when it was supposed to be turning (I confirmed the steps per revolution is 200 here).
I connected the motor pins to pins 8-11 on the Arduino.
/*
Stepper Motor Control - one revolution
This program drives a unipolar or bipolar stepper motor.
The motor is attached to digital pins 8 - 11 of the Arduino.
The motor should revolve one revolution in one direction, then
one revolution in the other direction.
Created 11 Mar. 2007
Modified 30 Nov. 2009
by Tom Igoe
*/
#include <Stepper.h>
const int stepsPerRevolution = 200; // change this to fit the number of steps per revolution
// for your motor
// initialize the stepper library on pins 8 through 11:
Stepper myStepper(stepsPerRevolution, 8, 9, 10, 11);
void setup() {
// set the speed at 60 rpm:
myStepper.setSpeed(60);
// initialize the serial port:
Serial.begin(9600);
}
void loop() {
// step one revolution in one direction:
Serial.println("clockwise");
//myStepper.step(stepsPerRevolution);
delay(500);
// step one revolution in the other direction:
Serial.println("counterclockwise");
//myStepper.step(-stepsPerRevolution);
delay(500);
}
It just vibrated again.
I tried using the ULN2003 Stepper with this tutorial and it worked, so I believe that I simply have a bad stepper motor.
I tried using a smaller stepper motor, which worked, leading me to believe there is a problem with the stepper (later it turns out it kind of worked, so I'm not sure whether this was truly an issue).
4-Stepper Driver
Mr. Dubick told us it would be a better idea to use a four-stepper driver arduino shield.
Worked first try! 12V power supply, connect ENABLE to GND and connected the servo to one of the axis. Followed this tutorial.
Here's the code.
// CNC Shield Stepper Control Demo
// Superb Tech
// www.youtube.com/superbtech
const int StepX = 2;
const int DirX = 5;
const int StepY = 3;
const int DirY = 6;
const int StepZ = 4;
const int DirZ = 7;
void setup() {
pinMode(StepX,OUTPUT);
pinMode(DirX,OUTPUT);
pinMode(StepY,OUTPUT);
pinMode(DirY,OUTPUT);
pinMode(StepZ,OUTPUT);
pinMode( DirZ,OUTPUT);
}
void loop() {
digitalWrite(DirX, HIGH); // set direction, HIGH for clockwise, LOW for anticlockwise
digitalWrite(DirY, HIGH);
digitalWrite(DirZ, HIGH);
for(int x = 0; x<200; x++) { // loop for 200 steps
digitalWrite(StepX,HIGH);
delayMicroseconds(500);
digitalWrite(StepX,LOW);
delayMicroseconds(500);
}
delay(1000); // delay for 1 second
for(int x = 0; x<200; x++) { // loop for 200 steps
digitalWrite(StepY,HIGH);
delayMicroseconds(500);
digitalWrite(StepY,LOW);
delayMicroseconds(500);
}
delay(1000); // delay for 1 second
for(int x = 0; x<200; x++) { // loop for 200 steps
digitalWrite(StepZ,HIGH);
delayMicroseconds(500);
digitalWrite(StepZ,LOW);
delayMicroseconds(500);
}
delay(1000); // delay for 1 second
}
I plugged it in to all the other motor drivers which worked, except for the red "A" axis driver.
When I ran the following code from this website, the driver for the A-axis got very hot and the motor stopped working for the other axes. I also had to install the AccelStepper library from the Arduino Library Manager.
#include <AccelStepper.h>
// Voor de Arduino Uno + CNC shield V3
#define MOTOR_X_ENABLE_PIN 8
#define MOTOR_X_STEP_PIN 2
#define MOTOR_X_DIR_PIN 5
#define MOTOR_Y_ENABLE_PIN 8
#define MOTOR_Y_STEP_PIN 3
#define MOTOR_Y_DIR_PIN 6
#define MOTOR_Z_ENABLE_PIN 8
#define MOTOR_Z_STEP_PIN 4
#define MOTOR_Z_DIR_PIN 7
#define MOTOR_A_ENABLE_PIN 8
#define MOTOR_A_STEP_PIN 12
#define MOTOR_A_DIR_PIN 13
AccelStepper motorX(1, MOTOR_X_STEP_PIN, MOTOR_X_DIR_PIN);
AccelStepper motorY(1, MOTOR_Y_STEP_PIN, MOTOR_Y_DIR_PIN);
AccelStepper motorZ(1, MOTOR_Z_STEP_PIN, MOTOR_Z_DIR_PIN);
AccelStepper motorA(1, MOTOR_A_STEP_PIN, MOTOR_A_DIR_PIN);
void setup()
{
pinMode(MOTOR_X_ENABLE_PIN, OUTPUT);
pinMode(MOTOR_Y_ENABLE_PIN, OUTPUT);
pinMode(MOTOR_Z_ENABLE_PIN, OUTPUT);
pinMode(MOTOR_A_ENABLE_PIN, OUTPUT);
motorX.setEnablePin(MOTOR_X_ENABLE_PIN);
motorX.setPinsInverted(false, false, true);
motorX.setAcceleration(100);
//motorX.setMaxSpeed(100);
//motorX.setSpeed(100);
motorX.enableOutputs();
motorY.setEnablePin(MOTOR_Y_ENABLE_PIN);
motorY.setPinsInverted(false, false, true);
motorY.setAcceleration(100);
//motorY.setMaxSpeed(100);
//motorY.setSpeed(100);
motorY.enableOutputs();
motorZ.setEnablePin(MOTOR_Z_ENABLE_PIN);
motorZ.setPinsInverted(false, false, true);
motorZ.setAcceleration(100);
//motorZ.setMaxSpeed(100);
//motorZ.setSpeed(100);
motorZ.enableOutputs();
motorA.setEnablePin(MOTOR_Z_ENABLE_PIN);
motorA.setPinsInverted(false, false, true);
motorA.setAcceleration(100);
//motorA.setMaxSpeed(100);
//motorA.setSpeed(100);
motorA.enableOutputs();
}
void loop()
{
motorX.move(3000);
motorX.run();
motorY.move(3000);
motorY.run();
motorZ.move(3000);
motorZ.run();
motorA.move(3000);
motorA.run();
}
I tried with a different motor from wires we took apart from an old 3d-printer, and it worked!
This video revealed it's a clone axis, which it perfect for our application!
After connecting the two blue wires and the two yellow wires next to y-axis and plugging in the motor I thought burnt out and the working motor, it worked! The motion on the previously-really-hot motor was choppy but consistent, and the other motor vibrated at times when it wasn't moving, but the movement was consistent. The motors and drivers also got fairly hot. I asked Mr. Nelson for advice, who suggested that I learn how to tune the current of a stepper motor and add heat sinks to the drivers. He also revealed that the H-bridge controlers have small potentiometers than can change the current going to the motors. I never ended up using this, but it would be very helpful for a project that requires more precision with current in the future. Although, I did add the heat sinks.
Next I tried plugging the Time-of-Flight sensor into the SDA, SCL, 5V, and GND pins on the shield, but it didn't work. I tried taking off the shield and wiring it directly to the Arduino, which worked.
const int StepX = 2;
const int DirX = 5;
const int StepY = 3;
const int DirY = 6;
const int StepZ = 4;
const int DirZ = 7;
#include <VL53L1X.h>
VL53L1X sensor;
void setup() {
Serial.begin(115200);
pinMode(StepX,OUTPUT);
pinMode(DirX,OUTPUT);
pinMode(StepY,OUTPUT);
pinMode(DirY,OUTPUT);
pinMode(StepZ,OUTPUT);
pinMode( DirZ,OUTPUT);
Wire.begin();
Wire.setClock(400000); // use 400 kHz I2C
sensor.setTimeout(500);
if (!sensor.init())
{
Serial.println("Failed to detect and initialize sensor!");
while (1);
}
// Use long distance mode and allow up to 50000 us (50 ms) for a measurement.
// You can change these settings to adjust the performance of the sensor, but
// the minimum timing budget is 20 ms for short distance mode and 33 ms for
// medium and long distance modes. See the VL53L1X datasheet for more
// information on range and timing limits.
sensor.setDistanceMode(VL53L1X::Long);
sensor.setMeasurementTimingBudget(50000);
// Start continuous readings at a rate of one measurement every 50 ms (the
// inter-measurement period). This period should be at least as long as the
// timing budget.
sensor.startContinuous(50);
}
void loop() {
Serial.println(sensor.read());
digitalWrite(DirX, HIGH); // set direction, HIGH for clockwise, LOW for anticlockwise
digitalWrite(DirY, HIGH);
digitalWrite(DirZ, HIGH);
for(int x = 0; x<200; x++) { // loop for 200 steps
digitalWrite(StepX,HIGH);
delayMicroseconds(500);
digitalWrite(StepX,LOW);
delayMicroseconds(500);
}
delay(1000); // delay for 1 second
Serial.println("Turning Y");
for(int x = 0; x<1000; x++) { // loop for 200 steps
digitalWrite(StepY,HIGH);
delayMicroseconds(500);
digitalWrite(StepY,LOW);
delayMicroseconds(500);
}
delay(1000); // delay for 1 second
for(int x = 0; x<200; x++) { // loop for 200 steps
digitalWrite(StepZ,HIGH);
delayMicroseconds(500);
digitalWrite(StepZ,LOW);
delayMicroseconds(500);
}
delay(1000); // delay for 1 second
}
Next, I realized that the SDA and SCL pins for communication with the sensor weren't working because they're connected to A5 and Pin 1 on the Arduino, and there aren't actually and pins from the shield that connect to the SDA and SCL pins of the Arduino!
So, I grabbed two wires that would fit under their shield, which worked extremely well!
I then created a new Arduino file and noticed that I could copy the INO file in the project folder and then could include that code using the #include<OTHER_FILE_NAME.cpp> command.
Basically I needed to change the structure to get that to work (see final working example). The sensor started not working, then I realized I switched to a different one but the header were upside down. Then it started working sometimes but something just getting stuck without writing anything on sensor.init(). I printed something before that and it only printed out half of that thing. I realized it could be that the serial monitor had something wrong so I tried hooking it up to a motor again, which required me to hold the ground to the ground on the middle of the Arduino so the shield could fit on. Now I got it to print out if after I compiled I unplugged then plugged into a different port, but this worked inconsistently. Finally I went back to a different ground and used the SparkFun_VL53L1X.h library instead because this document revealed that the other library might actually require two more pins to be corrected. Now it worked, and I integrated the example code from the library. (I also fed it 3.3v instead of 5v, but then later Merritt Backerman revealed to me that this is incorrect and it should be 5V).
Library example code:
/*
Reading distance from the laser based VL53L1X
By: Nathan Seidle
SparkFun Electronics
Date: April 4th, 2018
License: This code is public domain but you buy me a beer if you use this and we meet someday (Beerware license).
SparkFun labored with love to create this code. Feel like supporting open source hardware?
Buy a board from SparkFun! https://www.sparkfun.com/products/14667
This example prints the distance to an object.
Are you getting weird readings? Be sure the vacuum tape has been removed from the sensor.
*/
#include <Wire.h>
#include "SparkFun_VL53L1X.h" //Click here to get the library: http://librarymanager/All#SparkFun_VL53L1X
//Optional interrupt and shutdown pins.
#define SHUTDOWN_PIN 2
#define INTERRUPT_PIN 3
SFEVL53L1X distanceSensor;
//Uncomment the following line to use the optional shutdown and interrupt pins.
//SFEVL53L1X distanceSensor(Wire, SHUTDOWN_PIN, INTERRUPT_PIN);
void setup(void)
{
Wire.begin();
Serial.begin(115200);
Serial.println("VL53L1X Qwiic Test");
if (distanceSensor.begin() != 0) //Begin returns 0 on a good init
{
Serial.println("Sensor failed to begin. Please check wiring. Freezing...");
while (1)
;
}
Serial.println("Sensor online!");
}
void loop(void)
{
distanceSensor.startRanging(); //Write configuration bytes to initiate measurement
while (!distanceSensor.checkForDataReady())
{
delay(1);
}
int distance = distanceSensor.getDistance(); //Get the result of the measurement from the sensor
distanceSensor.clearInterrupt();
distanceSensor.stopRanging();
Serial.print("Distance(mm): ");
Serial.print(distance);
float distanceInches = distance * 0.0393701;
float distanceFeet = distanceInches / 12.0;
Serial.print("\tDistance(ft): ");
Serial.print(distanceFeet, 2);
Serial.println();
}
Here's the final, integrated code for the sensor.
FINAL_PROGRAM.ino
#include "sensor.h"
#include "serial.h"
#include "stepper.h"
void setup() {
InitializeSerial();
InitializeSensorCommunicationI2C();
}
void loop() {
float mm;
float in;
float ft;
SensorVal(mm, in, ft);
Serial.println(mm);
}
sensor.h
#include <Wire.h>
#include "SparkFun_VL53L1X.h"
void InitializeSensorCommunicationI2C();
void SensorVal(float&, float&, float&); // mm, in, ft
sensor.cpp
#include "sensor.h"
SFEVL53L1X sensor;
/*
Reading distance from the laser based VL53L1X
By: Nathan Seidle
SparkFun Electronics
Date: April 4th, 2018
License: This code is public domain but you buy me a beer if you use this and we meet someday (Beerware license).
SparkFun labored with love to create this code. Feel like supporting open source hardware?
Buy a board from SparkFun! https://www.sparkfun.com/products/14667
This example prints the distance to an object.
Are you getting weird readings? Be sure the vacuum tape has been removed from the sensor.
*/
//Uncomment the following line to use the optional shutdown and interrupt pins.
//SFEVL53L1X distanceSensor(Wire, SHUTDOWN_PIN, INTERRUPT_PIN);
void SensorVal(float &mm, float &in, float &ft) {
sensor.startRanging(); //Write configuration bytes to initiate measurement
while (!sensor.checkForDataReady())
{
delay(1);
}
int distance = sensor.getDistance(); //Get the result of the measurement from the sensor
sensor.clearInterrupt();
sensor.stopRanging();
float distanceMm = distance;
float distanceInches = distance * 0.0393701;
float distanceFeet = distanceInches / 12.0;
mm = distanceMm;
in = distanceInches;
ft = distanceFeet;
}
void InitializeSensorCommunicationI2C() {
Wire.begin();
Serial.begin(115200);
Serial.println("VL53L1X Qwiic Test");
if (sensor.begin() != 0) //Begin returns 0 on a good init
{
Serial.println("Sensor failed to begin. Please check wiring. Freezing...");
while (1)
;
}
Serial.println("Sensor online!");
}
serial.h
void InitializeSerial();
serial.cpp
#include<Arduino.h>
#include "serial.h"
void InitializeSerial() {
Serial.begin(115200);
}
stepper.h
void setupf();
void loopf();
stepper.cpp
#include "stepper.h"
#include "sensor.h"
const int StepX = 2;
const int DirX = 5;
const int StepY = 3;
const int DirY = 6;
const int StepZ = 4;
const int DirZ = 7;
void setupf() {
pinMode(StepX,OUTPUT);
pinMode(DirX,OUTPUT);
pinMode(StepY,OUTPUT);
pinMode(DirY,OUTPUT);
pinMode(StepZ,OUTPUT);
pinMode( DirZ,OUTPUT);
}
void loopf() {
digitalWrite(DirX, HIGH); // set direction, HIGH for clockwise, LOW for anticlockwise
digitalWrite(DirY, HIGH);
digitalWrite(DirZ, HIGH);
if (SensorVal() < 100) return;
for(int x = 0; x<200; x++) { // loop for 200 steps
digitalWrite(StepX,HIGH);
delayMicroseconds(500);
digitalWrite(StepX,LOW);
delayMicroseconds(500);
}
delay(1000); // delay for 1 second
Serial.println("Turning Y");
for(int x = 0; x<1000; x++) { // loop for 200 steps
digitalWrite(StepY,HIGH);
delayMicroseconds(500);
digitalWrite(StepY,LOW);
delayMicroseconds(500);
}
delay(1000); // delay for 1 second
for(int x = 0; x<200; x++) { // loop for 200 steps
digitalWrite(StepZ,HIGH);
delayMicroseconds(500);
digitalWrite(StepZ,LOW);
delayMicroseconds(500);
}
delay(1000); // delay for 1 second
}
This source helped me when I realized using tuples in an Arduino environment will be hard.
FINAL_PROGRAM.ino
#include "sensor.h"
#include "serial.h"
#include "stepper.h"
void setup() {
InitializeSerial();
InitializeSensorCommunicationI2C();
}
void loop() {
loopff();
}
sensor.h
#include <Wire.h>
#include "SparkFun_VL53L1X.h"
void InitializeSensorCommunicationI2C();
void loopff();
sensor.cpp
#include "sensor.h"
#define SHUTDOWN_PIN 2
#define INTERRUPT_PIN 3
SFEVL53L1X sensor;
void loopff() {
sensor.startRanging(); //Write configuration bytes to initiate measurement
while (!sensor.checkForDataReady())
{
delay(1);
}
int distance = sensor.getDistance(); //Get the result of the measurement from the sensor
sensor.clearInterrupt();
sensor.stopRanging();
Serial.print("Distance(mm): ");
Serial.print(distance);
float distanceInches = distance * 0.0393701;
float distanceFeet = distanceInches / 12.0;
Serial.print("\tDistance(ft): ");
Serial.print(distanceFeet, 2);
Serial.println();
}
void InitializeSensorCommunicationI2C() {
Wire.begin();
Serial.begin(115200);
Serial.println("VL53L1X Qwiic Test");
if (sensor.begin() != 0) //Begin returns 0 on a good init
{
Serial.println("Sensor failed to begin. Please check wiring. Freezing...");
while (1)
;
}
Serial.println("Sensor online!");
}
serial.h
void InitializeSerial();
serial.cpp
#include<Arduino.h>
#include "serial.h"
void InitializeSerial() {
Serial.begin(115200);
}
stepper.h
void setupf();
void loopf();
stepper.cpp
#include "stepper.h"
#include "sensor.h"
const int StepX = 2;
const int DirX = 5;
const int StepY = 3;
const int DirY = 6;
const int StepZ = 4;
const int DirZ = 7;
void setupf() {
pinMode(StepX,OUTPUT);
pinMode(DirX,OUTPUT);
pinMode(StepY,OUTPUT);
pinMode(DirY,OUTPUT);
pinMode(StepZ,OUTPUT);
pinMode( DirZ,OUTPUT);
}
void loopf() {
digitalWrite(DirX, HIGH); // set direction, HIGH for clockwise, LOW for anticlockwise
digitalWrite(DirY, HIGH);
digitalWrite(DirZ, HIGH);
if (SensorVal() < 100) return;
for(int x = 0; x<200; x++) { // loop for 200 steps
digitalWrite(StepX,HIGH);
delayMicroseconds(500);
digitalWrite(StepX,LOW);
delayMicroseconds(500);
}
delay(1000); // delay for 1 second
Serial.println("Turning Y");
for(int x = 0; x<1000; x++) { // loop for 200 steps
digitalWrite(StepY,HIGH);
delayMicroseconds(500);
digitalWrite(StepY,LOW);
delayMicroseconds(500);
}
delay(1000); // delay for 1 second
for(int x = 0; x<200; x++) { // loop for 200 steps
digitalWrite(StepZ,HIGH);
delayMicroseconds(500);
digitalWrite(StepZ,LOW);
delayMicroseconds(500);
}
delay(1000); // delay for 1 second
}
This source taught me that 200 steps is one revolution.
C:\Users\adams\AppData\Local\Temp\arduino-sketch-F69D2D2183674197C8C30C5B05CC1A93\sketch
FINAL_PROGRAM.ino
#include "sensor.h"
#include "serial.h"
#include "stepper.h"
#include "logic_lib.h"
const int CALIBRATION_BUTTON_PIN = -1; // TO DO DO THIS //
const int START_BUTTON_PIN = -1; // TO DO DO THIS //
const bool SKIP_CALIBRATION = false; // TO DO DO THIS //
const bool DOWN_ROTATION_Z = true; // TO DO DO THIS //
int currentRotation;
int BOTTOM_ROTATION;
int TOP_ROTATION;
void setup() {
InitializeSerial();
InitializeSensorCommunicationI2C();
InitializeSteppers();
InitializeLogic(START_BUTTON_PIN);
if (SKIP_CALIBRATION) return;
Calibrate(CALIBRATION_BUTTON_PIN, DOWN_ROTATION_Z, BOTTOM_ROTATION, TOP_ROTATION, currentRotation);
}
void loop() {
WaitForStartup(START_BUTTON_PIN, currentRotation, BOTTOM_ROTATION, TOP_ROTATION, DOWN_ROTATION_Z);
}
void Calibrate(int calibrationButtonPin, bool downRotation, int &_bottomRot, int &_topRot, int &_currentRot) {
pinMode(calibrationButtonPin, INPUT); // CHECK AND SEE IF PULLUP OR PULLDOWN
while (digitalRead(calibrationButtonPin) == LOW) { // assumes x and y are the two z motors and z is the wheel motor
TurnXYOpposite(180, downRotation);
delay(500);
}
int bottomRot = 0;
int topRot;
int currentRot = 0;
while (digitalRead(calibrationButtonPin) == LOW) { // assumes x and y are the two z motors and z is the wheel motor
TurnXYOpposite(180, !downRotation);
currentRot += 180; // going up = positive angle
delay(500);
}
topRot = currentRot;
_bottomRot = bottomRot;
_topRot = topRot;
_currentRot = currentRot;
}
sensor.h
#include <Wire.h>
#include "SparkFun_VL53L1X.h"
void InitializeSensorCommunicationI2C();
void SensorVal(float&, float&, float&, bool); // mm, in, ft
sensor.cpp
#include "sensor.h"
SFEVL53L1X sensor;
/*
Reading distance from the laser based VL53L1X
By: Nathan Seidle
SparkFun Electronics
Date: April 4th, 2018
License: This code is public domain but you buy me a beer if you use this and we meet someday (Beerware license).
SparkFun labored with love to create this code. Feel like supporting open source hardware?
Buy a board from SparkFun! https://www.sparkfun.com/products/14667
This example prints the distance to an object.
Are you getting weird readings? Be sure the vacuum tape has been removed from the sensor.
*/
//Uncomment the following line to use the optional shutdown and interrupt pins.
//SFEVL53L1X distanceSensor(Wire, SHUTDOWN_PIN, INTERRUPT_PIN);
void SensorVal(float &mm, float &in, float &ft, bool forwardSensor) { // AS OF NOW FORWARD SENSOR DOESN'T DO ANYTHING
sensor.startRanging(); //Write configuration bytes to initiate measurement
while (!sensor.checkForDataReady())
{
delay(1);
}
int distance = sensor.getDistance(); //Get the result of the measurement from the sensor
sensor.clearInterrupt();
sensor.stopRanging();
float distanceMm = distance;
float distanceInches = distance * 0.0393701;
float distanceFeet = distanceInches / 12.0;
mm = distanceMm;
in = distanceInches;
ft = distanceFeet;
}
void InitializeSensorCommunicationI2C() {
Wire.begin();
Serial.begin(115200);
Serial.println("VL53L1X Qwiic Test");
if (sensor.begin() != 0) //Begin returns 0 on a good init
{
Serial.println("Sensor failed to begin. Please check wiring. Freezing...");
while (1)
;
}
Serial.println("Sensor online!");
}
serial.h
void InitializeSerial();
serial.cpp
#include<Arduino.h>
#include "serial.h"
void InitializeSerial() {
Serial.begin(115200);
}
stepper.h
void InitializeSteppers();
void TurnAxis(float, int, int, bool);
void TurnX(float, bool); // angle in degrees
void TurnY(float, bool); // angle in degrees
void TurnXYOpposite(float, bool);
void TurnZ(float, bool); // angle in degrees
stepper.cpp
#include "stepper.h"
#include "sensor.h"
const int StepX = 2;
const int DirX = 5;
const int StepY = 3;
const int DirY = 6;
const int StepZ = 4;
const int DirZ = 7;
void InitializeSteppers() {
pinMode(StepX,OUTPUT);
pinMode(DirX,OUTPUT);
pinMode(StepY,OUTPUT);
pinMode(DirY,OUTPUT);
pinMode(StepZ,OUTPUT);
pinMode( DirZ,OUTPUT);
}
void TurnAxis(float angle, int stepVar, int dirVar, bool clockwise) {
auto s = clockwise ? HIGH : LOW;
digitalWrite(dirVar, s);
int steps = 200 * (angle / 360);
for(int i = 0; i < steps; i++) {
digitalWrite(stepVar,HIGH);
delayMicroseconds(500);
digitalWrite(stepVar,LOW);
delayMicroseconds(500);
}
}
void TurnX(float angle, bool clockwise) {TurnAxis(angle, StepX, DirX, clockwise);}
void TurnY(float angle, bool clockwise) {TurnAxis(angle, StepY, DirY, clockwise);}
void TurnXYOpposite(float angle, bool xClockwise) {
digitalWrite(DirX, xClockwise);
digitalWrite(DirY, !xClockwise);
int steps = 200 * (angle / 360);
for(int i = 0; i < steps; i++) {
digitalWrite(DirX,HIGH);
digitalWrite(DirY,HIGH);
delayMicroseconds(500);
digitalWrite(DirX,LOW);
digitalWrite(DirY,LOW);
delayMicroseconds(500);
}
}
void TurnZ(float angle, bool clockwise) {TurnAxis(angle, StepZ, DirZ, clockwise);}
logic_lib.h
#include<Arduino.h>
enum RAG_POSITION {
DOWN = 1,
UP = 2
};
enum DIRECTION {
FORWARD = true,
BACK = false
};
void InitializeLogic(int);
void Calibrate();
void WaitForStartup(int, int&, int, int, bool);
bool StartCondition(int);
void SetRagPosition(int, int&, int, int, bool); // first is really RAG_POSITION but is an int
bool HitEdge(bool);
void MoveUntilEdge(bool);
void MainFlow(int&, int, int, bool);
void MoveToRot(int&, int, bool);
logic_lib.cpp
#include "logic_lib.h"
#include "stepper.h"
#include "sensor.h"
const DIRECTION CLEANING_DIRECTION = FORWARD;
void InitializeLogic(int startButtonPin) {
pinMode(startButtonPin, INPUT); // TODO FIGURE THIS OUT INPUT TYPE FOR THE BUTTON
}
void WaitForStartup(int startButtonPin, int& currentRot, int bottomRot, int topRot, bool downRotation) {
while (1) {
if (StartCondition(startButtonPin)) {
MainFlow(currentRot, bottomRot, topRot, downRotation);
}
}
}
bool StartCondition(int startButtonPin) {
return (digitalRead(startButtonPin) == HIGH);
}
void SetRagPosition(RAG_POSITION ragPosition, int& currentRot, int bottomRot, int topRot, bool downRotation) {
int target;
if (ragPosition == DOWN) {
target = bottomRot;
}
else {
target = topRot;
}
MoveToRot(currentRot, target, downRotation);
}
void MoveToRot(int& currentRot, int targetRot, bool downRotation) {
int difference = targetRot - currentRot; // positive difference = move down
TurnXYOpposite(difference * (difference < 0 ? -1 : 1), (difference < 0) ? !downRotation : downRotation);
}
bool HitEdge(DIRECTION direction, int offTableThreshold) {
int mm, _;
SensorVal(mm, _, _, direction);
return (mm >= offTableThreshold);
}
DIRECTION MoveUntilEdge(DIRECTION currentDir) {
while (!HitEdge(currentDir)) {
TurnZ(180, currentDir);
delay(250);
}
return ((currentDir == FORWARD) ? BACK : FORWARD);
}
void MainFlow(int ¤tRot, int bottomRot, int topRot, bool downRotation) {
DIRECTION currentDirection = CLEANING_DIRECTION;
SetRagPosition(DOWN, currentRot, bottomRot, topRot, downRotation);
currentDirection = MoveUntilEdge(currentDirection);
SetRagPosition(UP, currentRot, bottomRot, topRot, downRotation);
MoveUntilEdge(currentDirection);
}
Pico & Arduino
Next I started trying to use the Raspberry Pi Pico to control the second sensor and have it talk to the Uno. This image revealed taht the SpnEn pin on the CNC Shield is connected to pin D12 on the Arduino. Then I plugged in the Pico, opened the Blink program in the Arduino IDE (I decided I'll use C++ for both microcontrollers because I was having trouble with the sensor and MicroPython earlier), changed the pin to 28, plugged an LED into GP28 (pin 34) and connected it through a resistor to ground. The upload failed, but I simply forgot to press the Bootloader button when plugging in my board. I then uploaded (I think I also originally had the wrong port since I had both the Uno and the Pico plugged into my computer), but it still didn't work, as I realized the resistor was very high, so I plugged it directly into ground, which worked!
I first tried using this library, but this forum revealed that it won't work because it relies on the Raw Pico SDK instead of Arduino.h.
Merritt Backerman also pointed out that we don't need a constant line of communication between the Pico and the Uno - we can just send a stop signal when the sensor is near the edge.
I found this tutorial and decided that I'll give MicroPython one more try, and if it doesn't work, I'll use C++ with CMake.
When I plugged the sensor into GPIO 17 and 16 and connected power and ground, I ran this code and it output 0x29!
import machine
# Create I2C object
i2c = machine.I2C(0, scl=machine.Pin(17), sda=machine.Pin(16))
# Print out any addresses found
devices = i2c.scan()
if devices:
for d in devices:
print(hex(d))
else:
print("No devices found")
I give up. I can't find the right registers to replace the registers in this example program for an accelerometer (I only knew to change ADXL343_ADDR from 0x53 to 0x29 because that's what the i2c_scan.py program returned).
import machine
import utime
import ustruct
import sys
###############################################################################
# Constants
# I2C address
ADXL343_ADDR = 0x29
# Registers
REG_DEVID = 0x00
REG_POWER_CTL = 0x2D
REG_DATAX0 = 0x32
# Other constants
DEVID = 0xE5
SENSITIVITY_2G = 1.0 / 256 # (g/LSB)
EARTH_GRAVITY = 9.80665 # Earth's gravity in [m/s^2]
###############################################################################
# Settings
# Initialize I2C with pins
i2c = machine.I2C(0,
scl=machine.Pin(17),
sda=machine.Pin(16),
freq=400000)
devices = i2c.scan()
addrADDR = hex(devices[0])
###############################################################################
# Functions
def reg_write(i2c, addr, reg, data):
"""
Write bytes to the specified register.
"""
# Construct message
msg = bytearray()
msg.append(data)
# Write out message to register
i2c.writeto_mem(addr, reg, msg)
def reg_read(i2c, addr, reg, nbytes=1):
"""
Read byte(s) from specified register. If nbytes > 1, read from consecutive
registers.
"""
# Check to make sure caller is asking for 1 or more bytes
if nbytes < 1:
return bytearray()
print("starting to rest after writing")
utime.sleep(1)
print("done resting")
# Request data from specified register(s) over I2C
data = i2c.readfrom_mem(addr, reg, nbytes)
return data
###############################################################################
# Main
# Read device ID to make sure that we can communicate with the ADXL343
data = reg_read(i2c, ADXL343_ADDR, REG_DEVID)
if (data != bytearray((DEVID,))):
print("ERROR: Could not communicate with ADXL343")
sys.exit()
# Read Power Control register
data = reg_read(i2c, ADXL343_ADDR, REG_POWER_CTL)
print(data)
# Tell ADXL343 to start taking measurements by setting Measure bit to high
data = int.from_bytes(data, "big") | (1 << 3)
reg_write(i2c, ADXL343_ADDR, REG_POWER_CTL, data)
# Test: read Power Control register back to make sure Measure bit was set
data = reg_read(i2c, ADXL343_ADDR, REG_POWER_CTL)
print(data)
# Wait before taking measurements
utime.sleep(2.0)
# Run forever
while True:
# Read X, Y, and Z values from registers (16 bits each)
data = reg_read(i2c, ADXL343_ADDR, REG_DATAX0, 6)
# Convert 2 bytes (little-endian) into 16-bit integer (signed)
acc_x = ustruct.unpack_from("<h", data, 0)[0]
acc_y = ustruct.unpack_from("<h", data, 2)[0]
acc_z = ustruct.unpack_from("<h", data, 4)[0]
# Convert measurements to [m/s^2]
acc_x = acc_x * SENSITIVITY_2G * EARTH_GRAVITY
acc_y = acc_y * SENSITIVITY_2G * EARTH_GRAVITY
acc_z = acc_z * SENSITIVITY_2G * EARTH_GRAVITY
# Print results
print("X:", "{:.2f}".format(acc_x), \
"| Y:", "{:.2f}".format(acc_y), \
"| Z:", "{:.2f}".format(acc_z))
utime.sleep(0.1)
I went through this tutorial again, but I got an error.
Traceback (most recent call last):
File "<stdin>", line 4, in <module>
File "PiicoDev_VL53L1X.py", line 111, in _init_
File "PiicoDev_VL53L1X.py", line 135, in reset
File "PiicoDev_VL53L1X.py", line 124, in writeReg
OSError: [Errno 5] EIO
Desperately, I looked to the comments where I found someone with the same error!
But I couldn't find they're forum post. Instead, I went to the link and navigated to the forum where I searched for the error, and I found this discussion which revealed that THERE IS A PROBLEM WITH THE NEWEST VERSION OF MICROPYTHON AND I NEED TO INSTALL 1.18! When I did this in Thonny, the sensor worked perfectly!
I tried replicating all of this on the Pico W (I had switched to the normal one), but noticed that in Thonny I can't install any version below 1.19, so I'll stick to the normal Pico for now.
Here's PiicoDev_Unified.py
'''
PiicoDev.py: Unifies I2C drivers for different builds of MicroPython
Changelog:
- 2022-10-13 P.Johnston Add helptext to run i2csetup script on Raspberry Pi
- 2022-10-14 M.Ruppe Explicitly set default I2C initialisation parameters for machine-class (Raspberry Pi Pico + W)
- 2023-01-31 L.Howell Add minimal support for ESP32
'''
import os
_SYSNAME = os.uname().sysname
compat_ind = 1
i2c_err_str = 'PiicoDev could not communicate with module at address 0x{:02X}, check wiring'
setupi2c_str = ', run "sudo curl -L https://piico.dev/i2csetup | bash". Suppress this warning by setting suppress_warnings=True'
if _SYSNAME == 'microbit':
from microbit import i2c
from utime import sleep_ms
elif _SYSNAME == 'Linux':
from smbus2 import SMBus, i2c_msg
from time import sleep
from math import ceil
def sleep_ms(t):
sleep(t/1000)
else:
from machine import I2C, Pin
from utime import sleep_ms
class I2CBase:
def writeto_mem(self, addr, memaddr, buf, *, addrsize=8):
raise NotImplementedError('writeto_mem')
def readfrom_mem(self, addr, memaddr, nbytes, *, addrsize=8):
raise NotImplementedError('readfrom_mem')
def write8(self, addr, buf, stop=True):
raise NotImplementedError('write')
def read16(self, addr, nbytes, stop=True):
raise NotImplementedError('read')
def __init__(self, bus=None, freq=None, sda=None, scl=None):
raise NotImplementedError('__init__')
class I2CUnifiedMachine(I2CBase):
def __init__(self, bus=None, freq=None, sda=None, scl=None):
if bus is not None and freq is not None and sda is not None and scl is not None:
print('Using supplied freq, sda and scl to create machine I2C')
self.i2c = I2C(bus, freq=freq, sda=sda, scl=scl)
elif _SYSNAME == 'esp32' and (bus is None and freq is None and sda is None and scl is None):
raise Exception('Please input bus, frequency, machine.pin SDA and SCL objects to use ESP32')
else:
self.i2c = I2C(0, scl=Pin(9), sda=Pin(8), freq=100000)
self.writeto_mem = self.i2c.writeto_mem
self.readfrom_mem = self.i2c.readfrom_mem
def write8(self, addr, reg, data):
if reg is None:
self.i2c.writeto(addr, data)
else:
self.i2c.writeto(addr, reg + data)
def read16(self, addr, reg):
self.i2c.writeto(addr, reg, False)
return self.i2c.readfrom(addr, 2)
class I2CUnifiedMicroBit(I2CBase):
def __init__(self, freq=None):
if freq is not None:
print('Initialising I2C freq to {}'.format(freq))
microbit.i2c.init(freq=freq)
def writeto_mem(self, addr, memaddr, buf, *, addrsize=8):
ad = memaddr.to_bytes(addrsize // 8, 'big') # pad address for eg. 16 bit
i2c.write(addr, ad + buf)
def readfrom_mem(self, addr, memaddr, nbytes, *, addrsize=8):
ad = memaddr.to_bytes(addrsize // 8, 'big') # pad address for eg. 16 bit
i2c.write(addr, ad, repeat=True)
return i2c.read(addr, nbytes)
def write8(self, addr, reg, data):
if reg is None:
i2c.write(addr, data)
else:
i2c.write(addr, reg + data)
def read16(self, addr, reg):
i2c.write(addr, reg, repeat=True)
return i2c.read(addr, 2)
class I2CUnifiedLinux(I2CBase):
def __init__(self, bus=None, suppress_warnings=True):
if suppress_warnings == False:
with open('/boot/config.txt') as config_file:
if 'dtparam=i2c_arm=on' in config_file.read():
pass
else:
print('I2C is not enabled. To enable' + setupi2c_str)
config_file.close()
with open('/boot/config.txt') as config_file:
if 'dtparam=i2c_arm_baudrate=400000' in config_file.read():
pass
else:
print('Slow baudrate detected. If glitching occurs' + setupi2c_str)
config_file.close()
if bus is None:
bus = 1
self.i2c = SMBus(bus)
def readfrom_mem(self, addr, memaddr, nbytes, *, addrsize=8):
data = [None] * nbytes # initialise empty list
self.smbus_i2c_read(addr, memaddr, data, nbytes, addrsize=addrsize)
return data
def writeto_mem(self, addr, memaddr, buf, *, addrsize=8):
self.smbus_i2c_write(addr, memaddr, buf, len(buf), addrsize=addrsize)
def smbus_i2c_write(self, address, reg, data_p, length, addrsize=8):
ret_val = 0
data = []
for index in range(length):
data.append(data_p[index])
if addrsize == 8:
msg_w = i2c_msg.write(address, [reg] + data)
elif addrsize == 16:
msg_w = i2c_msg.write(address, [reg >> 8, reg & 0xff] + data)
else:
raise Exception('address must be 8 or 16 bits long only')
self.i2c.i2c_rdwr(msg_w)
return ret_val
def smbus_i2c_read(self, address, reg, data_p, length, addrsize=8):
ret_val = 0
if addrsize == 8:
msg_w = i2c_msg.write(address, [reg]) # warning this is set up for 16-bit addresses
elif addrsize == 16:
msg_w = i2c_msg.write(address, [reg >> 8, reg & 0xff]) # warning this is set up for 16-bit addresses
else:
raise Exception('address must be 8 or 16 bits long only')
msg_r = i2c_msg.read(address, length)
self.i2c.i2c_rdwr(msg_w, msg_r)
if ret_val == 0:
for index in range(length):
data_p[index] = ord(msg_r.buf[index])
return ret_val
def write8(self, addr, reg, data):
if reg is None:
d = int.from_bytes(data, 'big')
self.i2c.write_byte(addr, d)
else:
r = int.from_bytes(reg, 'big')
d = int.from_bytes(data, 'big')
self.i2c.write_byte_data(addr, r, d)
def read16(self, addr, reg):
regInt = int.from_bytes(reg, 'big')
return self.i2c.read_word_data(addr, regInt).to_bytes(2, byteorder='little', signed=False)
def create_unified_i2c(bus=None, freq=None, sda=None, scl=None, suppress_warnings=True):
if _SYSNAME == 'microbit':
i2c = I2CUnifiedMicroBit(freq=freq)
elif _SYSNAME == 'Linux':
i2c = I2CUnifiedLinux(bus=bus, suppress_warnings=suppress_warnings)
else:
i2c = I2CUnifiedMachine(bus=bus, freq=freq, sda=sda, scl=scl)
return i2c
PiicoDev_VL53L1X.py
from PiicoDev_Unified import *
compat_str = '\nUnified PiicoDev library out of date. Get the latest module: https://piico.dev/unified \n'
VL51L1X_DEFAULT_CONFIGURATION = bytes([
0x00, # 0x2d : set bit 2 and 5 to 1 for fast plus mode (1MHz I2C), else don't touch */
0x00, # 0x2e : bit 0 if I2C pulled up at 1.8V, else set bit 0 to 1 (pull up at AVDD) */
0x00, # 0x2f : bit 0 if GPIO pulled up at 1.8V, else set bit 0 to 1 (pull up at AVDD) */
0x01, # 0x30 : set bit 4 to 0 for active high interrupt and 1 for active low (bits 3:0 must be 0x1), use SetInterruptPolarity() */
0x02, # 0x31 : bit 1 = interrupt depending on the polarity, use CheckForDataReady() */
0x00, # 0x32 : not user-modifiable (NUM)*/
0x02, # 0x33 : NUM */
0x08, # 0x34 : NUM */
0x00, # 0x35 : NUM */
0x08, # 0x36 : NUM */
0x10, # 0x37 : NUM */
0x01, # 0x38 : NUM */
0x01, # 0x39 : NUM */
0x00, # 0x3a : NUM */
0x00, # 0x3b : NUM */
0x00, # 0x3c : NUM */
0x00, # 0x3d : NUM */
0xff, # 0x3e : NUM */
0x00, # 0x3f : NUM */
0x0F, # 0x40 : NUM */
0x00, # 0x41 : NUM */
0x00, # 0x42 : NUM */
0x00, # 0x43 : NUM */
0x00, # 0x44 : NUM */
0x00, # 0x45 : NUM */
0x20, # 0x46 : interrupt configuration 0->level low detection, 1-> level high, 2-> Out of window, 3->In window, 0x20-> New sample ready , TBC */
0x0b, # 0x47 : NUM */
0x00, # 0x48 : NUM */
0x00, # 0x49 : NUM */
0x02, # 0x4a : NUM */
0x0a, # 0x4b : NUM */
0x21, # 0x4c : NUM */
0x00, # 0x4d : NUM */
0x00, # 0x4e : NUM */
0x05, # 0x4f : NUM */
0x00, # 0x50 : NUM */
0x00, # 0x51 : NUM */
0x00, # 0x52 : NUM */
0x00, # 0x53 : NUM */
0xc8, # 0x54 : NUM */
0x00, # 0x55 : NUM */
0x00, # 0x56 : NUM */
0x38, # 0x57 : NUM */
0xff, # 0x58 : NUM */
0x01, # 0x59 : NUM */
0x00, # 0x5a : NUM */
0x08, # 0x5b : NUM */
0x00, # 0x5c : NUM */
0x00, # 0x5d : NUM */
0x01, # 0x5e : NUM */
0xdb, # 0x5f : NUM */
0x0f, # 0x60 : NUM */
0x01, # 0x61 : NUM */
0xf1, # 0x62 : NUM */
0x0d, # 0x63 : NUM */
0x01, # 0x64 : Sigma threshold MSB (mm in 14.2 format for MSB+LSB), use SetSigmaThreshold(), default value 90 mm */
0x68, # 0x65 : Sigma threshold LSB */
0x00, # 0x66 : Min count Rate MSB (MCPS in 9.7 format for MSB+LSB), use SetSignalThreshold() */
0x80, # 0x67 : Min count Rate LSB */
0x08, # 0x68 : NUM */
0xb8, # 0x69 : NUM */
0x00, # 0x6a : NUM */
0x00, # 0x6b : NUM */
0x00, # 0x6c : Intermeasurement period MSB, 32 bits register, use SetIntermeasurementInMs() */
0x00, # 0x6d : Intermeasurement period */
0x0f, # 0x6e : Intermeasurement period */
0x89, # 0x6f : Intermeasurement period LSB */
0x00, # 0x70 : NUM */
0x00, # 0x71 : NUM */
0x00, # 0x72 : distance threshold high MSB (in mm, MSB+LSB), use SetD:tanceThreshold() */
0x00, # 0x73 : distance threshold high LSB */
0x00, # 0x74 : distance threshold low MSB ( in mm, MSB+LSB), use SetD:tanceThreshold() */
0x00, # 0x75 : distance threshold low LSB */
0x00, # 0x76 : NUM */
0x01, # 0x77 : NUM */
0x0f, # 0x78 : NUM */
0x0d, # 0x79 : NUM */
0x0e, # 0x7a : NUM */
0x0e, # 0x7b : NUM */
0x00, # 0x7c : NUM */
0x00, # 0x7d : NUM */
0x02, # 0x7e : NUM */
0xc7, # 0x7f : ROI center, use SetROI() */
0xff, # 0x80 : XY ROI (X=Width, Y=Height), use SetROI() */
0x9B, # 0x81 : NUM */
0x00, # 0x82 : NUM */
0x00, # 0x83 : NUM */
0x00, # 0x84 : NUM */
0x01, # 0x85 : NUM */
0x01, # 0x86 : clear interrupt, use ClearInterrupt() */
0x40 # 0x87 : start ranging, use StartRanging() or StopRanging(), If you want an automatic start after VL53L1X_init() call, put 0x40 in location 0x87 */
])
class PiicoDev_VL53L1X:
def __init__(self, bus=None, freq=None, sda=None, scl=None, address=0x29):
try:
if compat_ind >= 1:
pass
else:
print(compat_str)
except:
print(compat_str)
self.i2c = create_unified_i2c(bus=bus, freq=freq, sda=sda, scl=scl)
self.addr = address
self.reset()
sleep_ms(1)
if self.read_model_id() != 0xEACC:
raise RuntimeError('Failed to find expected ID register values. Check wiring!')
# write default configuration
self.i2c.writeto_mem(self.addr, 0x2D, VL51L1X_DEFAULT_CONFIGURATION, addrsize=16)
sleep_ms(100)
# the API triggers this change in VL53L1_init_and_start_range() once a
# measurement is started; assumes MM1 and MM2 are disabled
self.writeReg16Bit(0x001E, self.readReg16Bit(0x0022) * 4)
sleep_ms(200)
def writeReg(self, reg, value):
return self.i2c.writeto_mem(self.addr, reg, bytes([value]), addrsize=16)
def writeReg16Bit(self, reg, value):
return self.i2c.writeto_mem(self.addr, reg, bytes([(value >> 8) & 0xFF, value & 0xFF]), addrsize=16)
def readReg(self, reg):
return self.i2c.readfrom_mem(self.addr, reg, 1, addrsize=16)[0]
def readReg16Bit(self, reg):
data = self.i2c.readfrom_mem(self.addr, reg, 2, addrsize=16)
return (data[0]<<8) + data[1]
def read_model_id(self):
return self.readReg16Bit(0x010F)
def reset(self):
self.writeReg(0x0000, 0x00)
sleep_ms(100)
self.writeReg(0x0000, 0x01)
def read(self):
try:
data = self.i2c.readfrom_mem(self.addr, 0x0089, 17, addrsize=16) # RESULT__RANGE_STATUS
except:
print(i2c_err_str.format(self.addr))
return float('NaN')
range_status = data[0]
# report_status = data[1]
stream_count = data[2]
dss_actual_effective_spads_sd0 = (data[3]<<8) + data[4]
# peak_signal_count_rate_mcps_sd0 = (data[5]<<8) + data[6]
ambient_count_rate_mcps_sd0 = (data[7]<<8) + data[8]
# sigma_sd0 = (data[9]<<8) + data[10]
# phase_sd0 = (data[11]<<8) + data[12]
final_crosstalk_corrected_range_mm_sd0 = (data[13]<<8) + data[14]
peak_signal_count_rate_crosstalk_corrected_mcps_sd0 = (data[15]<<8) + data[16]
#status = None
#if range_status in (17, 2, 1, 3):
#status = "HardwareFail"
#elif range_status == 13:
#status = "MinRangeFail"
#elif range_status == 18:
#status = "SynchronizationInt"
#elif range_status == 5:
#status = "OutOfBoundsFail"
#elif range_status == 4:
#status = "SignalFail"
#elif range_status == 6:
#status = "SignalFail"
#elif range_status == 7:
#status = "WrapTargetFail"
#elif range_status == 12:
#status = "XtalkSignalFail"
#elif range_status == 8:
#status = "RangeValidMinRangeClipped"
#elif range_status == 9:
#if stream_count == 0:
#status = "RangeValidNoWrapCheckFail"
#else:
#status = "OK"
return final_crosstalk_corrected_range_mm_sd0
def change_addr(self, new_addr):
self.writeReg(0x0001, new_addr & 0x7F)
sleep_ms(50)
self.addr = new_addr
main.py
from PiicoDev_VL53L1X import PiicoDev_VL53L1X
from time import sleep
distSensor = PiicoDev_VL53L1X()
output_pin = machine.Pin(28, machine.Pin.OUT)
while True:
dist = distSensor.read() # read the distance in millimetres
print(str(dist) + " mm") # convert the number to a string and print
sleep(0.1)
Next I changed main.py to the following.
from PiicoDev_VL53L1X import PiicoDev_VL53L1X
from time import sleep
distSensor = PiicoDev_VL53L1X()
output_pin = machine.Pin(28, machine.Pin.OUT)
while True:
dist = distSensor.read() # read the distance in millimetres
print(str(dist) + " mm") # convert the number to a string and print
sleep(0.1)
if (dist > 100):
output_pin.value(1)
else:
output_pin.value(0)
I connected GPIO28 to SpnEn (D12) on the Uno and ran this program on the Uno.
void setup() {
// put your setup code here, to run once:
pinMode(12, INPUT);
Serial.begin(9600);
}
void loop() {
// put your main code here, to run repeatedly:
Serial.println(digitalRead(12));
}
And it worked! Whenever I lift up the sensor the Arduino reads 1 and when it's facing the table it reads 0.
Working Wiring:
- SCL on sensor - 12 on pico (GPIO 9, I2C SCL)
- SDA on sensor - 11 on pico (GPIO 8, I2C SDA)
- Ground on sensor - Ground on Pico - Ground on Arduino
- VBUS on Pico - Power on sensor
- SpnEn (D12) on Arduino - 34 on pico (GPIO 28)
I realized there may be a problem if this pin is used when the stepper motors are running.
FINAL_PROGRAM.ino (pin 7 - I was looking on the pinout sheet for extra I/O pins but I noticied when I used it I couldn't control one of the axes' rotation - I'll mess with this in a bit)
#include "sensor.h"
#include "serial.h"
#include "stepper.h"
#include "logic_lib.h"
const int CALIBRATION_BUTTON_PIN = 7; // TO DO DO THIS //// FIX FIX FIX NOT 7 BUT WIRING WRONG
const int START_BUTTON_PIN = 13; // TO DO DO THIS //
const bool SKIP_CALIBRATION = false; // TO DO DO THIS //
const bool DOWN_ROTATION_Z = false; // TO DO DO THIS //
int currentRotation;
int BOTTOM_ROTATION;
int TOP_ROTATION;
void setup() {
InitializeSerial();
InitializeSensorCommunicationI2C();
InitializeSteppers();
InitializeLogic(START_BUTTON_PIN);
TurnX(100000, true);
if (SKIP_CALIBRATION) return;
Serial.println("Press START to initialize calibration");
while (!digitalRead(START_BUTTON_PIN));
Serial.println("Calibrating -- press CALIBRATION when the Z-axis is in the right position");
Calibrate(CALIBRATION_BUTTON_PIN, DOWN_ROTATION_Z, BOTTOM_ROTATION, TOP_ROTATION, currentRotation);
Serial.print("Current rot is ");
Serial.println(currentRotation);
Serial.print("Bottom rot is ");
Serial.println(BOTTOM_ROTATION);
Serial.print("Top rot is ");
Serial.println(TOP_ROTATION);
Serial.println("Calibration complete -- press START to begin");
}
void loop() {
WaitForStartup(START_BUTTON_PIN, currentRotation, BOTTOM_ROTATION, TOP_ROTATION, DOWN_ROTATION_Z);
}
void Calibrate(int calibrationButtonPin, bool downRotation, int &_bottomRot, int &_topRot, int &_currentRot) {
pinMode(calibrationButtonPin, INPUT); // CHECK AND SEE IF PULLUP OR PULLDOWN
int bottomRot = 0;
int topRot;
int currentRot = 0;
while (digitalRead(calibrationButtonPin) == LOW) { // assumes x and y are the two z motors and z is the wheel motor
TurnXYOpposite(180, !downRotation);
//TurnY(180, !downRotation);
currentRot += 180; // going up = positive angle
Serial.println(currentRot);
//delay(1/*500*/);
}
bottomRot = currentRot;
_bottomRot = bottomRot;
_topRot = topRot;
_currentRot = currentRot;
}
sensor.h
#include <Wire.h>
#include "SparkFun_VL53L1X.h"
void InitializeSensorCommunicationI2C();
void SensorVal(float&, float&, float&, bool); // mm, in, ft
//void HitEdge(bool forward);
sensor.cpp
#include "sensor.h"
SFEVL53L1X sensor;
/*
Reading distance from the laser based VL53L1X
By: Nathan Seidle
SparkFun Electronics
Date: April 4th, 2018
License: This code is public domain but you buy me a beer if you use this and we meet someday (Beerware license).
SparkFun labored with love to create this code. Feel like supporting open source hardware?
Buy a board from SparkFun! https://www.sparkfun.com/products/14667
This example prints the distance to an object.
Are you getting weird readings? Be sure the vacuum tape has been removed from the sensor.
*/
//Uncomment the following line to use the optional shutdown and interrupt pins.
//SFEVL53L1X distanceSensor(Wire, SHUTDOWN_PIN, INTERRUPT_PIN);
void SensorVal(float &mm, float &in, float &ft, bool forwardSensor) { // AS OF NOW FORWARD SENSOR DOESN'T DO ANYTHING
sensor.startRanging(); //Write configuration bytes to initiate measurement
while (!sensor.checkForDataReady())
{
delay(1);
}
int distance = sensor.getDistance(); //Get the result of the measurement from the sensor
sensor.clearInterrupt();
sensor.stopRanging();
float distanceMm = distance;
float distanceInches = distance * 0.0393701;
float distanceFeet = distanceInches / 12.0;
mm = distanceMm;
in = distanceInches;
ft = distanceFeet;
}
/*bool HitEdge(bool forward) {
float mm, __;
SensorVal(mm, __, __, forward);
return mm > 100;
}*/
void InitializeSensorCommunicationI2C() {
Wire.begin();
Serial.begin(115200);
Serial.println("VL53L1X Qwiic Test");
if (sensor.begin() != 0) //Begin returns 0 on a good init
{
Serial.println("Sensor failed to begin. Please check wiring. Freezing...");
while (1)
;
}
Serial.println("Sensor online!");
}
serial.h
void InitializeSerial();
*serial.cpp
#include<Arduino.h>
#include "serial.h"
void InitializeSerial() {
Serial.begin(115200);
}
stepper.h
void InitializeSteppers();
void TurnAxis(float, int, int, bool);
void TurnX(float, bool); // angle in degrees
void TurnY(float, bool); // angle in degrees
void TurnXYOpposite(float, bool);
void TurnZ(float, bool); // angle in degrees
const int StepX = 2;
const int DirX = 5;
const int StepY = 3;
const int DirY = 6;
const int StepZ = 4;
const int DirZ = 7;
stepper.cpp
#include "stepper.h"
#include "sensor.h"
void InitializeSteppers() {
pinMode(StepX,OUTPUT);
pinMode(DirX,OUTPUT);
pinMode(StepY,OUTPUT);
pinMode(DirY,OUTPUT);
pinMode(StepZ,OUTPUT);
pinMode( DirZ,OUTPUT);
}
void TurnAxis(float angle, int stepVar, int dirVar, bool clockwise) {
auto s = clockwise ? HIGH : LOW;
digitalWrite(dirVar, s);
int steps = 200 * (angle / 360);
for(int i = 0; i < steps; i++) {
digitalWrite(stepVar,HIGH);
delayMicroseconds(500);
digitalWrite(stepVar,LOW);
delayMicroseconds(500);
}
}
void TurnX(float angle, bool clockwise) {TurnAxis(angle, StepX, DirX, clockwise);}
void TurnY(float angle, bool clockwise) {TurnAxis(angle, StepY, DirY, clockwise);}
void TurnXYOpposite(float angle, bool xClockwise) {
digitalWrite(DirX, xClockwise);
digitalWrite(DirY, !xClockwise);
int steps = 200 * (angle / 360);
for(int i = 0; i < steps; i++) {
digitalWrite(StepX,HIGH);
digitalWrite(StepY,HIGH);
delayMicroseconds(500);
digitalWrite(StepX,LOW);
digitalWrite(StepY,LOW);
delayMicroseconds(500);
}
}
void TurnZ(float angle, bool clockwise) {TurnAxis(angle, StepZ, DirZ, clockwise);}
logic_lib.h
#include<Arduino.h>
const int offTableThreshold = 100;
const int OUTSIDE_SENSOR_INDICATOR_PIN = 12;
enum RAG_POSITION {
DOWN = 1,
UP = 2
};
enum DIRECTION {
FORWARD = true,
BACK = false
};
void InitializeLogic(int);
void Calibrate();
void WaitForStartup(int, int&, int, int, bool);
bool StartCondition(int);
void SetRagPosition(int, int&, int, int, bool); // first is really RAG_POSITION but is an int
bool HitEdge(bool);
void MoveUntilEdge(bool);
void MainFlow(int&, int, int, bool);
void MoveToRot(int&, int, bool);
logic_lib.cpp
#include "logic_lib.h"
#include "stepper.h"
#include "sensor.h"
const DIRECTION CLEANING_DIRECTION = FORWARD;
void InitializeLogic(int startButtonPin) {
pinMode(startButtonPin, INPUT); // TODO FIGURE THIS OUT INPUT TYPE FOR THE BUTTON
pinMode(OUTSIDE_SENSOR_INDICATOR_PIN, INPUT);
}
void WaitForStartup(int startButtonPin, int& currentRot, int bottomRot, int topRot, bool downRotation) {
while (1) {
if (StartCondition(startButtonPin)) {
Serial.println("Starting");
MainFlow(currentRot, bottomRot, topRot, downRotation);
}
}
}
bool StartCondition(int startButtonPin) {
return (digitalRead(startButtonPin) == HIGH);
}
void SetRagPosition(RAG_POSITION ragPosition, int& currentRot, int bottomRot, int topRot, bool downRotation) {
int target;
if (ragPosition == DOWN) {
target = bottomRot;
}
else {
target = topRot;
}
MoveToRot(currentRot, target, downRotation);
}
void MoveToRot(int& currentRot, int targetRot, bool downRotation) {
Serial.print("Moving ");
int difference = targetRot - currentRot; // positive difference = move down
Serial.println(difference);
for (int i = 0; i < targetRot * (targetRot < 0 ? -1 : 1); i += 180) {
TurnXYOpposite(180, (difference > 0) ? !downRotation : downRotation);
}
}
bool HitEdge(DIRECTION direction) {
if (direction == BACK) return (digitalRead(OUTSIDE_SENSOR_INDICATOR_PIN) == HIGH);
float mm, _;
SensorVal(mm, _, _, direction);
return (mm >= offTableThreshold);
}
DIRECTION MoveUntilEdge(DIRECTION currentDir) {
while (!HitEdge(currentDir)) {
TurnZ(180, currentDir);
delay(250);
}
return ((currentDir == FORWARD) ? BACK : FORWARD);
}
void MainFlow(int ¤tRot, int bottomRot, int topRot, bool downRotation) {
DIRECTION currentDirection = CLEANING_DIRECTION;
SetRagPosition(DOWN, currentRot, bottomRot, topRot, downRotation);
currentDirection = MoveUntilEdge(currentDirection);
SetRagPosition(UP, currentRot, bottomRot, topRot, downRotation);
MoveUntilEdge(currentDirection);
}
I also noticed sometimes I got the Erno/OS error when the I2C pins weren't wired properly or the current wasn't running to the sensor, or when there was an input to the VSYS while the program was being run via USBC.
Making electronics
I found this EagleCAD component, even though it's not the same I have, I think it'll work.
This source taught me I can type ripup into the command line and select All which will remove all routes.
![imgs of milled board]TODO
Testing The Leadscrew
Running the motor with the leadscrew didn't work at first, but it turns out it was simply a cable, and when I switched it out, it worked.
I also realized that I had to change the code to where instead of TurnXYOpposite(1890, true) where 1890 is just an example for the calibrated rotation to a loop of turning 180 degrees many times, or else the motor would turn less than it was supposed to. You can see these changes in the final version of the code.
I finalized the code, here it is:
FINAL_PROGRAM.ino
#include "sensor.h"
#include "serial.h"
#include "stepper.h"
#include "logic_lib.h"
const int CALIBRATION_BUTTON_PIN = 13;
const int START_BUTTON_PIN = 12;
const bool SKIP_CALIBRATION = false;
const bool DOWN_ROTATION_Z = false;
int currentRotation;
int BOTTOM_ROTATION;
int TOP_ROTATION;
void setup() {
InitializeSerial();
InitializeSensorCommunicationI2C();
InitializeSteppers();
InitializeLogic(START_BUTTON_PIN);
if (SKIP_CALIBRATION) return;
Serial.println("Press START to initialize calibration");
while (!digitalRead(START_BUTTON_PIN));
Serial.println("Calibrating -- press CALIBRATION when the Z-axis is in the right position");
Calibrate(CALIBRATION_BUTTON_PIN, DOWN_ROTATION_Z, BOTTOM_ROTATION, TOP_ROTATION, currentRotation);
Serial.print("Current rot is ");
Serial.println(currentRotation);
Serial.print("Bottom rot is ");
Serial.println(BOTTOM_ROTATION);
Serial.print("Top rot is ");
Serial.println(TOP_ROTATION);
Serial.println("Calibration complete -- press START to begin");
}
void loop() {
WaitForStartup(START_BUTTON_PIN, currentRotation, BOTTOM_ROTATION, TOP_ROTATION, DOWN_ROTATION_Z);
}
void Calibrate(int calibrationButtonPin, bool downRotation, int &_bottomRot, int &_topRot, int &_currentRot) {
pinMode(calibrationButtonPin, INPUT); // CHECK AND SEE IF PULLUP OR PULLDOWN
int bottomRot = 0;
int topRot;
int currentRot = 0;
while (digitalRead(calibrationButtonPin) == LOW) { // assumes x and y are the two z motors and z is the wheel motor
TurnXYOpposite(180, !downRotation);
//TurnY(180, !downRotation);
currentRot += 180; // going up = positive angle
Serial.println(currentRot);
//delay(1/*500*/);
}
bottomRot = currentRot;
_bottomRot = bottomRot;
_topRot = topRot;
_currentRot = currentRot;
}
sensor.h
#include <Wire.h>
#include "SparkFun_VL53L1X.h"
void InitializeSensorCommunicationI2C();
void SensorVal(float&, float&, float&, bool); // mm, in, ft
//void HitEdge(bool forward);
sensor.cpp
#include "sensor.h"
SFEVL53L1X sensor;
/*
Reading distance from the laser based VL53L1X
By: Nathan Seidle
SparkFun Electronics
Date: April 4th, 2018
License: This code is public domain but you buy me a beer if you use this and we meet someday (Beerware license).
SparkFun labored with love to create this code. Feel like supporting open source hardware?
Buy a board from SparkFun! https://www.sparkfun.com/products/14667
This example prints the distance to an object.
Are you getting weird readings? Be sure the vacuum tape has been removed from the sensor.
*/
//Uncomment the following line to use the optional shutdown and interrupt pins.
//SFEVL53L1X distanceSensor(Wire, SHUTDOWN_PIN, INTERRUPT_PIN);
void SensorVal(float &mm, float &in, float &ft, bool forwardSensor) { // AS OF NOW FORWARD SENSOR DOESN'T DO ANYTHING
sensor.startRanging(); //Write configuration bytes to initiate measurement
while (!sensor.checkForDataReady())
{
delay(1);
}
int distance = sensor.getDistance(); //Get the result of the measurement from the sensor
sensor.clearInterrupt();
sensor.stopRanging();
float distanceMm = distance;
float distanceInches = distance * 0.0393701;
float distanceFeet = distanceInches / 12.0;
mm = distanceMm;
in = distanceInches;
ft = distanceFeet;
}
/*bool HitEdge(bool forward) {
float mm, __;
SensorVal(mm, __, __, forward);
return mm > 100;
}*/
void InitializeSensorCommunicationI2C() {
Wire.begin();
Serial.begin(115200);
Serial.println("VL53L1X Qwiic Test");
if (sensor.begin() != 0) //Begin returns 0 on a good init
{
Serial.println("Sensor failed to begin. Please check wiring. Freezing...");
while (1)
;
}
Serial.println("Sensor online!");
}
serial.h
void InitializeSerial();
serial.cpp
#include<Arduino.h>
#include "serial.h"
void InitializeSerial() {
Serial.begin(115200);
}
stepper.h
void InitializeSteppers();
void TurnAxis(float, int, int, bool);
void TurnX(float, bool); // angle in degrees
void TurnY(float, bool); // angle in degrees
void TurnXYOpposite(float, bool);
void TurnZ(float, bool); // angle in degrees
const int StepX = 2;
const int DirX = 5;
const int StepY = 3;
const int DirY = 6;
const int StepZ = 4;
const int DirZ = 7;
stepper.cpp
#include "stepper.h"
#include "sensor.h"
void InitializeSteppers() {
pinMode(StepX,OUTPUT);
pinMode(DirX,OUTPUT);
pinMode(StepY,OUTPUT);
pinMode(DirY,OUTPUT);
pinMode(StepZ,OUTPUT);
pinMode( DirZ,OUTPUT);
}
void TurnAxis(float angle, int stepVar, int dirVar, bool clockwise) {
auto s = clockwise ? HIGH : LOW;
digitalWrite(dirVar, s);
int steps = 200 * (angle / 360);
for(int i = 0; i < steps; i++) {
digitalWrite(stepVar,HIGH);
delayMicroseconds(500);
digitalWrite(stepVar,LOW);
delayMicroseconds(500);
}
}
void TurnX(float angle, bool clockwise) {TurnAxis(angle, StepX, DirX, clockwise);}
void TurnY(float angle, bool clockwise) {TurnAxis(angle, StepY, DirY, clockwise);}
void TurnXYOpposite(float angle, bool xClockwise) {
digitalWrite(DirX, xClockwise);
digitalWrite(DirY, xClockwise);
int steps = 200 * (angle / 360);
for(int i = 0; i < steps; i++) {
digitalWrite(StepX,HIGH);
digitalWrite(StepY,HIGH);
delayMicroseconds(500);
digitalWrite(StepX,LOW);
digitalWrite(StepY,LOW);
delayMicroseconds(500);
}
}
void TurnZ(float angle, bool clockwise) {TurnAxis(angle, StepZ, DirZ, clockwise);}
logic_lib.h
#include<Arduino.h>
const int offTableThreshold = 500;
const int OUTSIDE_SENSOR_INDICATOR_PIN = A3;
enum RAG_POSITION {
DOWN = 1,
UP = 2
};
enum DIRECTION {
FORWARD = true,
BACK = false
};
void InitializeLogic(int);
void Calibrate();
void WaitForStartup(int, int&, int, int, bool);
bool StartCondition(int);
void SetRagPosition(int, int&, int, int, bool); // first is really RAG_POSITION but is an int
bool HitEdge(bool);
void MoveUntilEdge(bool);
void MainFlow(int&, int, int, bool);
void MoveToRot(int&, int, bool);
logic_lib.cpp
#include "logic_lib.h"
#include "stepper.h"
#include "sensor.h"
const DIRECTION CLEANING_DIRECTION = FORWARD;
void InitializeLogic(int startButtonPin) {
pinMode(startButtonPin, INPUT); // TODO FIGURE THIS OUT INPUT TYPE FOR THE BUTTON
pinMode(OUTSIDE_SENSOR_INDICATOR_PIN, INPUT);
}
void WaitForStartup(int startButtonPin, int& currentRot, int bottomRot, int topRot, bool downRotation) {
while (1) {
if (StartCondition(startButtonPin)) {
Serial.println("Starting");
MainFlow(currentRot, bottomRot, topRot, downRotation);
}
}
}
bool StartCondition(int startButtonPin) {
return (digitalRead(startButtonPin) == HIGH);
}
void SetRagPosition(RAG_POSITION ragPosition, int& currentRot, int bottomRot, int topRot, bool downRotation) {
int target;
if (ragPosition == DOWN) {
target = topRot; // check if flipped
}
else {
target = bottomRot; // check if flipped
}
MoveToRot(currentRot, target, downRotation);
}
void MoveToRot(int& currentRot, int targetRot, bool downRotation) {
Serial.print("Moving ");
int difference = targetRot - currentRot; // positive difference = move down
Serial.println(difference);
for (int i = 0; i < targetRot * (targetRot < 0 ? -1 : 1); i += 180) {
TurnXYOpposite(180, (difference > 0) ? !downRotation : downRotation);
}
}
bool HitEdge(DIRECTION direction) {
if (direction == FORWARD) return (digitalRead(OUTSIDE_SENSOR_INDICATOR_PIN) == HIGH);
float mm, _;
SensorVal(mm, _, _, direction);
return (mm >= offTableThreshold);
}
DIRECTION MoveUntilEdge(DIRECTION currentDir) {
while (!HitEdge(currentDir)) {
TurnZ(180, currentDir);
delay(0.5);
}
return ((currentDir == FORWARD) ? BACK : FORWARD);
}
void MainFlow(int ¤tRot, int bottomRot, int topRot, bool downRotation) {
DIRECTION currentDirection = CLEANING_DIRECTION;
SetRagPosition(DOWN, currentRot, bottomRot, topRot, downRotation);
Serial.println("moving until edge...");
currentDirection = MoveUntilEdge(currentDirection);
SetRagPosition(UP, currentRot, bottomRot, topRot, downRotation);
MoveUntilEdge(currentDirection);
}
ON PICO:
main.py
from machine import Pin
from PiicoDev_VL53L1X import PiicoDev_VL53L1X
from time import sleep
from utime import sleep
led = Pin(25, Pin.OUT)
for i in range(6):
led.toggle()
sleep(1)
for i in range(6):
led.toggle()
sleep(0.5)
distSensor = PiicoDev_VL53L1X()
output_pin = machine.Pin(28, machine.Pin.OUT)
b = False
while True:
dist = distSensor.read() # read the distance in millimetres
print(str(dist) + " mm") # convert the number to a string and print
sleep(0.1)
if (dist > 500):
if not (b == True):
led.toggle()
b = not b
output_pin.value(1)
else:
if b:
led.toggle()
b = not b
output_pin.value(0)
PiicoDev_VL53L1X.py
from PiicoDev_Unified import *
compat_str = '\nUnified PiicoDev library out of date. Get the latest module: https://piico.dev/unified \n'
VL51L1X_DEFAULT_CONFIGURATION = bytes([
0x00, # 0x2d : set bit 2 and 5 to 1 for fast plus mode (1MHz I2C), else don't touch */
0x00, # 0x2e : bit 0 if I2C pulled up at 1.8V, else set bit 0 to 1 (pull up at AVDD) */
0x00, # 0x2f : bit 0 if GPIO pulled up at 1.8V, else set bit 0 to 1 (pull up at AVDD) */
0x01, # 0x30 : set bit 4 to 0 for active high interrupt and 1 for active low (bits 3:0 must be 0x1), use SetInterruptPolarity() */
0x02, # 0x31 : bit 1 = interrupt depending on the polarity, use CheckForDataReady() */
0x00, # 0x32 : not user-modifiable (NUM)*/
0x02, # 0x33 : NUM */
0x08, # 0x34 : NUM */
0x00, # 0x35 : NUM */
0x08, # 0x36 : NUM */
0x10, # 0x37 : NUM */
0x01, # 0x38 : NUM */
0x01, # 0x39 : NUM */
0x00, # 0x3a : NUM */
0x00, # 0x3b : NUM */
0x00, # 0x3c : NUM */
0x00, # 0x3d : NUM */
0xff, # 0x3e : NUM */
0x00, # 0x3f : NUM */
0x0F, # 0x40 : NUM */
0x00, # 0x41 : NUM */
0x00, # 0x42 : NUM */
0x00, # 0x43 : NUM */
0x00, # 0x44 : NUM */
0x00, # 0x45 : NUM */
0x20, # 0x46 : interrupt configuration 0->level low detection, 1-> level high, 2-> Out of window, 3->In window, 0x20-> New sample ready , TBC */
0x0b, # 0x47 : NUM */
0x00, # 0x48 : NUM */
0x00, # 0x49 : NUM */
0x02, # 0x4a : NUM */
0x0a, # 0x4b : NUM */
0x21, # 0x4c : NUM */
0x00, # 0x4d : NUM */
0x00, # 0x4e : NUM */
0x05, # 0x4f : NUM */
0x00, # 0x50 : NUM */
0x00, # 0x51 : NUM */
0x00, # 0x52 : NUM */
0x00, # 0x53 : NUM */
0xc8, # 0x54 : NUM */
0x00, # 0x55 : NUM */
0x00, # 0x56 : NUM */
0x38, # 0x57 : NUM */
0xff, # 0x58 : NUM */
0x01, # 0x59 : NUM */
0x00, # 0x5a : NUM */
0x08, # 0x5b : NUM */
0x00, # 0x5c : NUM */
0x00, # 0x5d : NUM */
0x01, # 0x5e : NUM */
0xdb, # 0x5f : NUM */
0x0f, # 0x60 : NUM */
0x01, # 0x61 : NUM */
0xf1, # 0x62 : NUM */
0x0d, # 0x63 : NUM */
0x01, # 0x64 : Sigma threshold MSB (mm in 14.2 format for MSB+LSB), use SetSigmaThreshold(), default value 90 mm */
0x68, # 0x65 : Sigma threshold LSB */
0x00, # 0x66 : Min count Rate MSB (MCPS in 9.7 format for MSB+LSB), use SetSignalThreshold() */
0x80, # 0x67 : Min count Rate LSB */
0x08, # 0x68 : NUM */
0xb8, # 0x69 : NUM */
0x00, # 0x6a : NUM */
0x00, # 0x6b : NUM */
0x00, # 0x6c : Intermeasurement period MSB, 32 bits register, use SetIntermeasurementInMs() */
0x00, # 0x6d : Intermeasurement period */
0x0f, # 0x6e : Intermeasurement period */
0x89, # 0x6f : Intermeasurement period LSB */
0x00, # 0x70 : NUM */
0x00, # 0x71 : NUM */
0x00, # 0x72 : distance threshold high MSB (in mm, MSB+LSB), use SetD:tanceThreshold() */
0x00, # 0x73 : distance threshold high LSB */
0x00, # 0x74 : distance threshold low MSB ( in mm, MSB+LSB), use SetD:tanceThreshold() */
0x00, # 0x75 : distance threshold low LSB */
0x00, # 0x76 : NUM */
0x01, # 0x77 : NUM */
0x0f, # 0x78 : NUM */
0x0d, # 0x79 : NUM */
0x0e, # 0x7a : NUM */
0x0e, # 0x7b : NUM */
0x00, # 0x7c : NUM */
0x00, # 0x7d : NUM */
0x02, # 0x7e : NUM */
0xc7, # 0x7f : ROI center, use SetROI() */
0xff, # 0x80 : XY ROI (X=Width, Y=Height), use SetROI() */
0x9B, # 0x81 : NUM */
0x00, # 0x82 : NUM */
0x00, # 0x83 : NUM */
0x00, # 0x84 : NUM */
0x01, # 0x85 : NUM */
0x01, # 0x86 : clear interrupt, use ClearInterrupt() */
0x40 # 0x87 : start ranging, use StartRanging() or StopRanging(), If you want an automatic start after VL53L1X_init() call, put 0x40 in location 0x87 */
])
class PiicoDev_VL53L1X:
def __init__(self, bus=None, freq=None, sda=None, scl=None, address=0x29):
try:
if compat_ind >= 1:
pass
else:
print(compat_str)
except:
print(compat_str)
self.i2c = create_unified_i2c(bus=bus, freq=freq, sda=sda, scl=scl)
self.addr = address
self.reset()
sleep_ms(1000)
if self.read_model_id() != 0xEACC:
raise RuntimeError('Failed to find expected ID register values. Check wiring!')
# write default configuration
self.i2c.writeto_mem(self.addr, 0x2D, VL51L1X_DEFAULT_CONFIGURATION, addrsize=16)
sleep_ms(100)
# the API triggers this change in VL53L1_init_and_start_range() once a
# measurement is started; assumes MM1 and MM2 are disabled
self.writeReg16Bit(0x001E, self.readReg16Bit(0x0022) * 4)
sleep_ms(200)
def writeReg(self, reg, value):
return self.i2c.writeto_mem(self.addr, reg, bytes([value]), addrsize=16)
def writeReg16Bit(self, reg, value):
return self.i2c.writeto_mem(self.addr, reg, bytes([(value >> 8) & 0xFF, value & 0xFF]), addrsize=16)
def readReg(self, reg):
return self.i2c.readfrom_mem(self.addr, reg, 1, addrsize=16)[0]
def readReg16Bit(self, reg):
data = self.i2c.readfrom_mem(self.addr, reg, 2, addrsize=16)
return (data[0]<<8) + data[1]
def read_model_id(self):
return self.readReg16Bit(0x010F)
def reset(self):
self.writeReg(0x0000, 0x00)
sleep_ms(100)
self.writeReg(0x0000, 0x01)
def read(self):
try:
data = self.i2c.readfrom_mem(self.addr, 0x0089, 17, addrsize=16) # RESULT__RANGE_STATUS
except:
print(i2c_err_str.format(self.addr))
return float('NaN')
range_status = data[0]
# report_status = data[1]
stream_count = data[2]
dss_actual_effective_spads_sd0 = (data[3]<<8) + data[4]
# peak_signal_count_rate_mcps_sd0 = (data[5]<<8) + data[6]
ambient_count_rate_mcps_sd0 = (data[7]<<8) + data[8]
# sigma_sd0 = (data[9]<<8) + data[10]
# phase_sd0 = (data[11]<<8) + data[12]
final_crosstalk_corrected_range_mm_sd0 = (data[13]<<8) + data[14]
peak_signal_count_rate_crosstalk_corrected_mcps_sd0 = (data[15]<<8) + data[16]
#status = None
#if range_status in (17, 2, 1, 3):
#status = "HardwareFail"
#elif range_status == 13:
#status = "MinRangeFail"
#elif range_status == 18:
#status = "SynchronizationInt"
#elif range_status == 5:
#status = "OutOfBoundsFail"
#elif range_status == 4:
#status = "SignalFail"
#elif range_status == 6:
#status = "SignalFail"
#elif range_status == 7:
#status = "WrapTargetFail"
#elif range_status == 12:
#status = "XtalkSignalFail"
#elif range_status == 8:
#status = "RangeValidMinRangeClipped"
#elif range_status == 9:
#if stream_count == 0:
#status = "RangeValidNoWrapCheckFail"
#else:
#status = "OK"
return final_crosstalk_corrected_range_mm_sd0
def change_addr(self, new_addr):
self.writeReg(0x0001, new_addr & 0x7F)
sleep_ms(50)
self.addr = new_addr
PiicoDev_Unified.py
'''
PiicoDev.py: Unifies I2C drivers for different builds of MicroPython
Changelog:
- 2022-10-13 P.Johnston Add helptext to run i2csetup script on Raspberry Pi
- 2022-10-14 M.Ruppe Explicitly set default I2C initialisation parameters for machine-class (Raspberry Pi Pico + W)
- 2023-01-31 L.Howell Add minimal support for ESP32
'''
import os
_SYSNAME = os.uname().sysname
compat_ind = 1
i2c_err_str = 'PiicoDev could not communicate with module at address 0x{:02X}, check wiring'
setupi2c_str = ', run "sudo curl -L https://piico.dev/i2csetup | bash". Suppress this warning by setting suppress_warnings=True'
if _SYSNAME == 'microbit':
from microbit import i2c
from utime import sleep_ms
elif _SYSNAME == 'Linux':
from smbus2 import SMBus, i2c_msg
from time import sleep
from math import ceil
def sleep_ms(t):
sleep(t/1000)
else:
from machine import I2C, Pin
from utime import sleep_ms
class I2CBase:
def writeto_mem(self, addr, memaddr, buf, *, addrsize=8):
raise NotImplementedError('writeto_mem')
def readfrom_mem(self, addr, memaddr, nbytes, *, addrsize=8):
raise NotImplementedError('readfrom_mem')
def write8(self, addr, buf, stop=True):
raise NotImplementedError('write')
def read16(self, addr, nbytes, stop=True):
raise NotImplementedError('read')
def __init__(self, bus=None, freq=None, sda=None, scl=None):
raise NotImplementedError('__init__')
class I2CUnifiedMachine(I2CBase):
def __init__(self, bus=None, freq=None, sda=None, scl=None):
if bus is not None and freq is not None and sda is not None and scl is not None:
print('Using supplied freq, sda and scl to create machine I2C')
self.i2c = I2C(bus, freq=freq, sda=sda, scl=scl)
elif _SYSNAME == 'esp32' and (bus is None and freq is None and sda is None and scl is None):
raise Exception('Please input bus, frequency, machine.pin SDA and SCL objects to use ESP32')
else:
self.i2c = I2C(0, scl=Pin(9), sda=Pin(8), freq=100000)
self.writeto_mem = self.i2c.writeto_mem
self.readfrom_mem = self.i2c.readfrom_mem
def write8(self, addr, reg, data):
if reg is None:
self.i2c.writeto(addr, data)
else:
self.i2c.writeto(addr, reg + data)
def read16(self, addr, reg):
self.i2c.writeto(addr, reg, False)
return self.i2c.readfrom(addr, 2)
class I2CUnifiedMicroBit(I2CBase):
def __init__(self, freq=None):
if freq is not None:
print('Initialising I2C freq to {}'.format(freq))
microbit.i2c.init(freq=freq)
def writeto_mem(self, addr, memaddr, buf, *, addrsize=8):
ad = memaddr.to_bytes(addrsize // 8, 'big') # pad address for eg. 16 bit
i2c.write(addr, ad + buf)
def readfrom_mem(self, addr, memaddr, nbytes, *, addrsize=8):
ad = memaddr.to_bytes(addrsize // 8, 'big') # pad address for eg. 16 bit
i2c.write(addr, ad, repeat=True)
return i2c.read(addr, nbytes)
def write8(self, addr, reg, data):
if reg is None:
i2c.write(addr, data)
else:
i2c.write(addr, reg + data)
def read16(self, addr, reg):
i2c.write(addr, reg, repeat=True)
return i2c.read(addr, 2)
class I2CUnifiedLinux(I2CBase):
def __init__(self, bus=None, suppress_warnings=True):
if suppress_warnings == False:
with open('/boot/config.txt') as config_file:
if 'dtparam=i2c_arm=on' in config_file.read():
pass
else:
print('I2C is not enabled. To enable' + setupi2c_str)
config_file.close()
with open('/boot/config.txt') as config_file:
if 'dtparam=i2c_arm_baudrate=400000' in config_file.read():
pass
else:
print('Slow baudrate detected. If glitching occurs' + setupi2c_str)
config_file.close()
if bus is None:
bus = 1
self.i2c = SMBus(bus)
def readfrom_mem(self, addr, memaddr, nbytes, *, addrsize=8):
data = [None] * nbytes # initialise empty list
self.smbus_i2c_read(addr, memaddr, data, nbytes, addrsize=addrsize)
return data
def writeto_mem(self, addr, memaddr, buf, *, addrsize=8):
self.smbus_i2c_write(addr, memaddr, buf, len(buf), addrsize=addrsize)
def smbus_i2c_write(self, address, reg, data_p, length, addrsize=8):
ret_val = 0
data = []
for index in range(length):
data.append(data_p[index])
if addrsize == 8:
msg_w = i2c_msg.write(address, [reg] + data)
elif addrsize == 16:
msg_w = i2c_msg.write(address, [reg >> 8, reg & 0xff] + data)
else:
raise Exception('address must be 8 or 16 bits long only')
self.i2c.i2c_rdwr(msg_w)
return ret_val
def smbus_i2c_read(self, address, reg, data_p, length, addrsize=8):
ret_val = 0
if addrsize == 8:
msg_w = i2c_msg.write(address, [reg]) # warning this is set up for 16-bit addresses
elif addrsize == 16:
msg_w = i2c_msg.write(address, [reg >> 8, reg & 0xff]) # warning this is set up for 16-bit addresses
else:
raise Exception('address must be 8 or 16 bits long only')
msg_r = i2c_msg.read(address, length)
self.i2c.i2c_rdwr(msg_w, msg_r)
if ret_val == 0:
for index in range(length):
data_p[index] = ord(msg_r.buf[index])
return ret_val
def write8(self, addr, reg, data):
if reg is None:
d = int.from_bytes(data, 'big')
self.i2c.write_byte(addr, d)
else:
r = int.from_bytes(reg, 'big')
d = int.from_bytes(data, 'big')
self.i2c.write_byte_data(addr, r, d)
def read16(self, addr, reg):
regInt = int.from_bytes(reg, 'big')
return self.i2c.read_word_data(addr, regInt).to_bytes(2, byteorder='little', signed=False)
def create_unified_i2c(bus=None, freq=None, sda=None, scl=None, suppress_warnings=True):
if _SYSNAME == 'microbit':
i2c = I2CUnifiedMicroBit(freq=freq)
elif _SYSNAME == 'Linux':
i2c = I2CUnifiedLinux(bus=bus, suppress_warnings=suppress_warnings)
else:
i2c = I2CUnifiedMachine(bus=bus, freq=freq, sda=sda, scl=scl)
return i2c
Then I had a lot of trouble with the OSError shown before when trying to get the milled board with a pico to work. I spent a long time resoldering, desoldering, bug fixing, until we eventually decided to use a breadboard for the moment. We got everything working on the breadboard with a 9V battery plugged into the Arduino (which this source confirmed is optimal), and I tried using a 15.3VDC transformer. When I was screwing this in to the CNC Shield, I saw a spark and immediately sparked. I realized I didn't unplug the transformer and was very scared that I had fried the electronics. I went back to the old power supply, and it didn't work! Oh gosh! I used a different arduino and chord and it would get hot still, so I unplugged everything, got another Arduino, and just plugged in the SDA and SCL pins with one sensor, and I kept getting the OSError and couldn't get it to connect! I realized that I simply wasn't running power to the sensor, and when I fixed this, it worked! I hooked everything back up again and it all worked! (I eventually decided to use D12, D13, and A3 for the start button pin, calibration button pin, and communication from the Raspberry Pi Pico, respectively). We also decided to use the power supply with long jumper wires that we stripped and soldered to male connectors.
Also sometimes I got the previous error where it wouldn't write success or failure in the serial monitor and it wold just write that it failed when I unplugged it. This turned out to be an error with the wiring.
Making the Video
I made the video in Adobe Premiere Pro and didn't run into any issues.