/*
*******************************************************************************
Christian SIMON,
FabLabSU
Sorbonne Universite
* Date: 2025/01/24
M5Stack Basic + Step Motor Module (tested with v1 leaving VERSION_1_0 commented)
XRF bench automation
Documentation :
https://wiki.fablab.sorbonne-universite.fr/BookStack/books/motorisation-banc-de-mesure
*******************************************************************************
Some code parts taken from various examples:
- StepmotorPulse_M039
- vl53l0x
- SerialEvent by Tom Igoe
*/
#include "M5Unified.h"
#include "Module_Stepmotor.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "Adafruit_VL53L0X.h"
#if defined ( ARDUINO )
#include <Arduino.h>
#endif
//#define VERSION_1_0
Adafruit_VL53L0X lox = Adafruit_VL53L0X();
String inputString = "";
bool stringComplete = false;
static Module_Stepmotor driver;
int period_min = 25;
int period_max = 400;
int period = period_min;
/*
SerialEvent occurs whenever a new data comes in the hardware serial RX. This
routine is run between each time loop() runs, so using delay inside loop can
delay response. Multiple bytes of data may be available.
*/
void serialEvent() {
while (Serial.available()) {
// get the new byte:
char inChar = (char)Serial.read();
// add it to the inputString:
inputString += inChar;
// if the incoming character is a newline, set a flag so the main loop can
// do something about it:
if (inChar == '\n') {
stringComplete = true;
}
}
}
void setup() {
M5.begin();
Serial.begin(115200);
// reserve 200 bytes for the inputString:
inputString.reserve(200);
M5.Lcd.setTextSize(2);
Wire.begin(21, 22, 400000UL);
driver.init(Wire);
/*
Serial.println("Adafruit VL53L0X test");
if (!lox.begin()) {
Serial.println(F("Failed to boot VL53L0X"));
while(1);
}
// power
Serial.println(F("VL53L0X API Simple Ranging example\n\n"));
*/
#ifndef VERSION_1_0
driver.resetMotor(0, 0);
driver.resetMotor(1, 0);
driver.resetMotor(2, 0);
#else
driver.setMicrostepResolution(DIRECT_STEPMOTOR::kMicrosteps32);
#endif
driver.enableMotor(1);
Serial1.begin(115200, SERIAL_8N1, 35, 5);
Serial2.begin(115200, SERIAL_8N1, 34, 26);
Serial2.setTimeout(100);
ledcAttach(16, 10000, 8); // Args: Channel, Freq, Resolution
ledcAttach(12, 10000, 8);
ledcAttach(15, 6000, 8);
ledcWrite(16, 0); // X axis
ledcWrite(12, 0); // "Y" = -X
ledcWrite(15, 0); // Z
pinMode(17, OUTPUT); // X axis
pinMode(13, OUTPUT); // "Y" = -X
pinMode(0, OUTPUT); // Z
digitalWrite(17, 0);
digitalWrite(13, 1);
digitalWrite(0, 1); // initialize in upward direction (Vz > 0)
M5.Lcd.drawString("setup completed", 0, 0, 2);
Serial.println(F("setup completed"));
M5.update();
}
/*
// syntax:
// change direction:
digitalWrite(motor, step_dir); motor = 17, 13, 0 (X,1-X, Z respectively)
ledcWrite(motor, 0); // disable
ledcWrite(motor, 1); // enable
uint32_t ledcChangeFrequency(uint8_t pin, uint32_t freq, uint8_t resolution); // for speed change
//
*/
void loop() {
static String commandString[10]; // command can have up to ten args ? Why not ?
static String messageString;
int StringCount = 0;
// static uint8_t step_dir = 0;
// static uint8_t reset_mtr = 0;
static Module_Stepmotor::MicrostepResolution_t micro_res = Module_Stepmotor::kMicrosteps32;
Serial1.print("Y"); // Qu'est-ce que c'est ?
M5.update();
/*
if (Serial2.available()) {
char inChar = (char)Serial2.read();
if (inChar == 'Y') {
M5.Lcd.fillRect(105, 180, 120, 20, TFT_GREEN);
}
} else {
M5.Lcd.fillRect(105, 180, 120, 20, TFT_RED);
}
*/
if (stringComplete) {
Serial.println("received ");
Serial.println(inputString);
Serial.println("\n");
M5.Lcd.drawString("received", 0, 10, 2);
M5.Lcd.drawString(inputString, 0, 30, 2);
// copy the received string for later analysis
messageString = "" + inputString;
messageString.trim();
// clear the string:
inputString = "";
stringComplete = false;
// split the received string for analysis:
//String[] commandString = split(messageString, " ");
// split the received string for analysis:
while (messageString.length() > 0) {
int index = messageString.indexOf(' ');
if (index == -1) // No space found
{
commandString[StringCount++] = messageString;
break;
}
else
{
commandString[StringCount++] = messageString.substring(0, index);
messageString = messageString.substring(index+1);
}
}
// messageString is now split in commandString[StringCount] sub-strings
}
if(commandString[0] == "IZ") {
commandString[0] = "";
ledcWrite(16,0);
ledcWrite(12,0);
ledcWrite(15,0);
digitalWrite(0, 1); // +Z direction
ledcWrite(15,1);
driver.getExtIOStatus();
while(driver.ext_io_status[1]) {
driver.getExtIOStatus();
}
ledcWrite(15,0);
digitalWrite(0, 0); // -Z direction
M5.Lcd.drawString("reached Z=0", 0, 40, 2);
//commandString[0] = "D";
}
if(commandString[0] == "IX") {
commandString[0] = "";
ledcWrite(16,0);
ledcWrite(12,0);
ledcWrite(15,0);
digitalWrite(17, 0);
digitalWrite(13, 1);
ledcWrite(16,1);
ledcWrite(12,1);
driver.getExtIOStatus();
while(driver.ext_io_status[3]) {
driver.getExtIOStatus();
}
ledcWrite(15,0);
digitalWrite(0, 0); // -Z direction
M5.Lcd.drawString("reached X=0", 0, 40, 2);
}
driver.getExtIOStatus();
if((commandString[0]=="+Z")&&(driver.ext_io_status[1])) {
ledcWrite(16,0);
ledcWrite(12,0);
ledcWrite(15,0);
commandString[0] = "";
digitalWrite(0, 1);
ledcWrite(15,1);
}
if(commandString[0]=="-Z") {
ledcWrite(16,0);
ledcWrite(12,0);
ledcWrite(15,0);
commandString[0] = "";
digitalWrite(0, 0);
ledcWrite(15,1);
}
if(commandString[0]=="VZ") {
ledcChangeFrequency(15, commandString[1].toInt()*1000, 8);
commandString[0] = "";
}
if(commandString[0]=="VX") {
ledcChangeFrequency(16, commandString[1].toInt()*1000, 8);
ledcChangeFrequency(12, commandString[1].toInt()*1000, 8);
commandString[0] = "";
}
if(commandString[0]=="+X") {
ledcWrite(16,0);
ledcWrite(12,0);
ledcWrite(15,0);
commandString[0] = "";
digitalWrite(17, 1);
digitalWrite(13, 0);
ledcWrite(16,1);
ledcWrite(12,1);
}
if(commandString[0]=="-X") {
ledcWrite(16,0);
ledcWrite(12,0);
ledcWrite(15,0);
commandString[0] = "";
digitalWrite(17, 0);
digitalWrite(13, 1);
ledcWrite(16,1);
ledcWrite(12,1);
}
if(commandString[0]=="S") {
commandString[0] = "";
ledcWrite(16,0);
ledcWrite(12,0);
ledcWrite(15,0);
}
if(commandString[0]=="D") {
commandString[0] = "";
VL53L0X_RangingMeasurementData_t measure;
Serial.print("Reading a measurement... ");
M5.Lcd.drawString("Reading...", 0, 0, 2);
lox.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
if (measure.RangeStatus != 4) { // phase failures have incorrect data
Serial.print("Distance (mm): ");
Serial.println(measure.RangeMilliMeter);
M5.Lcd.drawString("D mm", 20, 20, 2);
M5.Lcd.drawString(String(measure.RangeMilliMeter), 40, 40, 2);
} else {
Serial.println(" out of range ");
M5.Lcd.drawString("out of range", 260, 220, 2);
}
}
if(commandString[0]!="") {
Serial.println("Unknown command");
M5.Lcd.drawString("Unkown command", 10, 300, 2);
commandString[0] = "";
}
// test +X move
if (M5.BtnA.wasPressed()) {
digitalWrite(17, 1);
digitalWrite(13, 0);
ledcWrite(16,1);
ledcWrite(12,1);
}
// test -X move
if (M5.BtnB.wasPressed()) {
digitalWrite(17, 0);
digitalWrite(13, 1);
ledcWrite(16,1);
ledcWrite(12,1);
}
// stop
if (M5.BtnC.wasPressed()) {
ledcWrite(16,0);
ledcWrite(12,0);
}
driver.getExtIOStatus();
if (driver.ext_io_status[1]) {
M5.Lcd.fillRect(300, 0, 20, 20, TFT_GREEN); // end-course Z up switch is free
} else {
M5.Lcd.fillRect(300, 0, 20, 20, TFT_RED); // end-course switch is pressed
}
// To be installed ?
/*
if (driver.ext_io_status[2]) {
M5.Lcd.fillRect(300, 21, 20, 20, TFT_GREEN); // end-course Z down switch is free
} else {
M5.Lcd.fillRect(300, 21, 20, 20, TFT_RED); // end-course switch is pressed
}
if (driver.ext_io_status[3]) {
M5.Lcd.fillRect(300, 42, 20, 20, TFT_GREEN); // end-course X left switch is free
} else {
M5.Lcd.fillRect(300, 42, 20, 20, TFT_RED); // end-course switch is pressed
}
*/
}