io card support

This commit is contained in:
Siwat Sirichai 2023-12-27 23:15:11 +07:00
parent 75967bf1b3
commit 1f8ad10609
16 changed files with 999 additions and 0 deletions

18
.vscode/c_cpp_properties.json vendored Normal file
View File

@ -0,0 +1,18 @@
{
"configurations": [
{
"name": "windows-gcc-x64",
"includePath": [
"${workspaceFolder}/**"
],
"compilerPath": "gcc",
"cStandard": "${default}",
"cppStandard": "${default}",
"intelliSenseMode": "windows-gcc-x64",
"compilerArgs": [
""
]
}
],
"version": 4
}

24
.vscode/launch.json vendored Normal file
View File

@ -0,0 +1,24 @@
{
"version": "0.2.0",
"configurations": [
{
"name": "C/C++ Runner: Debug Session",
"type": "cppdbg",
"request": "launch",
"args": [],
"stopAtEntry": false,
"externalConsole": true,
"cwd": ".",
"program": "build/Debug/outDebug",
"MIMode": "gdb",
"miDebuggerPath": "gdb",
"setupCommands": [
{
"description": "Enable pretty-printing for gdb",
"text": "-enable-pretty-printing",
"ignoreFailures": true
}
]
}
]
}

59
.vscode/settings.json vendored Normal file
View File

@ -0,0 +1,59 @@
{
"C_Cpp_Runner.cCompilerPath": "gcc",
"C_Cpp_Runner.cppCompilerPath": "g++",
"C_Cpp_Runner.debuggerPath": "gdb",
"C_Cpp_Runner.cStandard": "",
"C_Cpp_Runner.cppStandard": "",
"C_Cpp_Runner.msvcBatchPath": "",
"C_Cpp_Runner.useMsvc": false,
"C_Cpp_Runner.warnings": [
"-Wall",
"-Wextra",
"-Wpedantic",
"-Wshadow",
"-Wformat=2",
"-Wcast-align",
"-Wconversion",
"-Wsign-conversion",
"-Wnull-dereference"
],
"C_Cpp_Runner.msvcWarnings": [
"/W4",
"/permissive-",
"/w14242",
"/w14287",
"/w14296",
"/w14311",
"/w14826",
"/w44062",
"/w44242",
"/w14905",
"/w14906",
"/w14263",
"/w44265",
"/w14928"
],
"C_Cpp_Runner.enableWarnings": true,
"C_Cpp_Runner.warningsAsError": false,
"C_Cpp_Runner.compilerArgs": [],
"C_Cpp_Runner.linkerArgs": [],
"C_Cpp_Runner.includePaths": [],
"C_Cpp_Runner.includeSearch": [
"*",
"**/*"
],
"C_Cpp_Runner.excludeSearch": [
"**/build",
"**/build/**",
"**/.*",
"**/.*/**",
"**/.vscode",
"**/.vscode/**"
],
"C_Cpp_Runner.useAddressSanitizer": false,
"C_Cpp_Runner.useUndefinedSanitizer": false,
"C_Cpp_Runner.useLeakSanitizer": false,
"C_Cpp_Runner.showCompilationTime": false,
"C_Cpp_Runner.useLinkTimeOptimization": false,
"C_Cpp_Runner.msvcSecureNoWarnings": false
}

View File

@ -0,0 +1,38 @@
#include <ESPMegaPRO.h>
#include <AnalogCard.hpp>
void AnalogCard::dacWrite(uint8_t pin, uint16_t value) {
switch(pin) {
case 0:
this->dac0.writeDAC(value);
break;
case 1:
this->dac1.writeDAC(value);
break;
case 2:
this->dac2.writeDAC(value);
break;
case 3:
this->dac3.writeDAC(value);
break;
}
}
uint16_t AnalogCard::analogRead(uint8_t pin) {
if (pin >= 0 && pin <= 3) {
return this->analogInputBankA.readADC_SingleEnded(pin);
} else if (pin >= 4 && pin <= 7) {
return this->analogInputBankB.readADC_SingleEnded(pin - 4);
}
}
void AnalogCard::begin() {
this->dac0 = MCP4725(DAC0_ADDRESS);
this->dac1 = MCP4725(DAC1_ADDRESS);
this->dac2 = MCP4725(DAC2_ADDRESS);
this->dac3 = MCP4725(DAC3_ADDRESS);
this->dac0.begin();
this->dac1.begin();
this->dac2.begin();
this->dac3.begin();
this->analogInputBankA.begin(ANALOG_INPUT_BANK_A_ADDRESS);
this->analogInputBankB.begin(ANALOG_INPUT_BANK_B_ADDRESS);
}

View File

@ -0,0 +1,26 @@
#pragma once
#include <ESPMegaPRO.h>
#include <Adafruit_ADS1X15.h>
#include <MCP4725.h>
#define ANALOG_INPUT_BANK_A_ADDRESS 0x48
#define ANALOG_INPUT_BANK_B_ADDRESS 0x49
#define DAC0_ADDRESS 0x60
#define DAC1_ADDRESS 0x61
#define DAC2_ADDRESS 0x62
#define DAC3_ADDRESS 0x63
class AnalogCard {
public:
AnalogCard();
void dacWrite(uint8_t pin, uint16_t value);
uint16_t analogRead(uint8_t pin);
void begin();
private:
MCP4725 dac0;
MCP4725 dac1;
MCP4725 dac2;
MCP4725 dac3;
Adafruit_ADS1115 analogInputBankA;
Adafruit_ADS1115 analogInputBankB;
};

View File

@ -0,0 +1,96 @@
#pragma once
#include <ESPMegaPRO.h>
#include <DigitalInputCard.hpp>
// Instantiate the card with the specified address
DigitalInputCard::DigitalInputCard(uint8_t address_a, uint8_t address_b) {
this->address_a = address_a;
this->address_b = address_b;
}
// Instantiate the card with the specified position on the dip switch
// Bit 0,1,2 are for bank A
// Bit 3,4,5 are for bank B
DigitalInputCard::DigitalInputCard(bool bit0, bool bit1, bool bit2, bool bit3, bool bit4, bool bit5) {
this->address_a = 0x20;
this->address_b = 0x20;
if (bit0) this->address_a += 1;
if (bit1) this->address_a += 2;
if (bit2) this->address_a += 4;
if (bit3) this->address_b += 1;
if (bit4) this->address_b += 2;
if (bit5) this->address_b += 4;
}
// Initialize the card
void DigitalInputCard::begin() {
this->inputBankA = PCF8574(this->address_a);
this->inputBankB = PCF8574(this->address_b);
this->inputBankA.begin();
this->inputBankB.begin();
}
// Refresh and Read the input from the specified pin, always refresh the input buffers
uint8_t DigitalInputCard::digitalRead(uint8_t pin) {
digitalRead(pin, true);
}
// Read the input from the specified pin, also refresh the input buffers if refresh is true
uint8_t DigitalInputCard::digitalRead(uint8_t pin, bool refresh) {
// First check if the pin is in bank A or B
if (pin >= 0 && pin <= 7) {
// Refresh the input buffers if refresh is true
if (refresh) refreshInputBankA();
// Extract the bit from the buffer
return ((inputBufferA >> (7 - pin)) & 1);
} else if (pin >= 8 && pin <= 15) {
// Refresh the input buffers if refresh is true
if (refresh) refreshInputBankB();
// Extract the bit from the buffer
return ((inputBufferB >> (15 - pin)) & 1);
}
}
// Preform a loop to refresh the input buffers
void DigitalInputCard::loop() {
// Store the current input buffers
uint8_t inputBufferA_old = inputBufferA;
uint8_t inputBufferB_old = inputBufferB;
// Refresh the input buffers
refreshInputBankA();
refreshInputBankB();
// Iterate over all pins and check if they changed
for (int i = 0; i < 16; i++) {
// Check which bank the pin is in
if (i<8) {
// Check if the pin changed
if (((inputBufferA_old >> (7 - i)) & 1) != ((inputBufferA >> (7 - i)) & 1)) {
// Call the callback function if it is not null and pass the pin and the new value
if (callback != NULL) callback(i, ((inputBufferA >> (7 - i)) & 1));
}
} else {
// Check if the pin changed
if (((inputBufferB_old >> (15 - i)) & 1) != ((inputBufferB >> (15 - i)) & 1)) {
// Call the callback function if it is not null and pass the pin and the new value
if (callback != NULL) callback(i, ((inputBufferB >> (15 - i)) & 1));
}
}
}
}
// Get the input buffer for bank A
uint8_t DigitalInputCard::getInputBufferA() {
return inputBufferA;
}
// Get the input buffer for bank B
uint8_t DigitalInputCard::getInputBufferB() {
return inputBufferB;
}
// Register a callback function to be called when a pin changes
void DigitalInputCard::registerCallback(void (*callback)(int, bool)) {
this->callback = callback;
}
// Refresh the input buffer for bank A
void DigitalInputCard::refreshInputBankA() {
inputBufferA = inputBankA.read8();
}
// Refresh the input buffer for bank B
void DigitalInputCard::refreshInputBankB() {
inputBufferB = inputBankB.read8();
}

View File

@ -0,0 +1,35 @@
#pragma once
#include <ESPMegaPRO.h>
#include <PCF8574.h>
class DigitalInputCard {
public:
// Instantiate the card with the specified address
DigitalInputCard(uint8_t address_a, uint8_t address_b);
// Instantiate the card with the specified position on the dip switch
DigitalInputCard(bool bit0, bool bit1, bool bit2, bool bit3, bool bit4, bool bit5);
// Initialize the card
void begin();
// Refresh and Read the input from the specified pin, always refresh the input buffers
uint8_t digitalRead(uint8_t pin);
// Read the input from the specified pin, also refresh the input buffers if refresh is true
uint8_t digitalRead(uint8_t pin, bool refresh);
// Preform a loop to refresh the input buffers
void loop();
// Get the input buffer for bank A
uint8_t getInputBufferA();
// Get the input buffer for bank B
uint8_t getInputBufferB();
// Register a callback function to be called when a pin changes
void registerCallback(void (*callback)(int, bool));
private:
PCF8574 inputBankA;
PCF8574 inputBankB;
uint8_t address_a;
uint8_t address_b;
uint8_t inputBufferA;
uint8_t inputBufferB;
void (*callback)(int, bool);
void refreshInputBankA();
void refreshInputBankB();
};

View File

@ -0,0 +1,32 @@
#include <DigitalOutputCard.hpp>
DigitalOutputCard::DigitalOutputCard(uint8_t address) {
this->address = address;
}
// Instantiate the card with the specified position on the dip switch
DigitalOutputCard::DigitalOutputCard(bool bit0, bool bit1, bool bit2, bool bit3, bool bit4) {
this->address = 0x20;
if (bit0) this->address += 1;
if (bit1) this->address += 2;
if (bit2) this->address += 4;
if (bit3) this->address += 8;
if (bit4) this->address += 16;
}
// Initialize the card
void DigitalOutputCard::begin() {
this->pwm = Adafruit_PWMServoDriver(this->address);
this->pwm.begin();
}
// Set the output to the specified state
void DigitalOutputCard::digitalWrite(uint8_t pin, bool state) {
this->pwm.setPin(pin, state ? 4095 : 0);
}
// Set the output to the specified pwm value
void DigitalOutputCard::analogWrite(uint8_t pin, uint16_t value) {
// If value is greater than 4095, set it to 4095
if (value > 4095) value = 4095;
// Set the pwm value
this->pwm.setPin(pin, value);
}

View File

@ -0,0 +1,20 @@
#pragma once
#include <ESPMegaPRO.h>
#include <Adafruit_PWMServoDriver.h>
class DigitalOutputCard
{
public:
// Instantiate the card with the specified address
DigitalOutputCard(uint8_t address);
// Instantiate the card with the specified position on the dip switch
DigitalOutputCard(bool bit0, bool bit1, bool bit2, bool bit3, bool bit4);
// Initialize the card
void begin();
// Set the output to the specified state
void digitalWrite(uint8_t pin, bool state);
// Set the output to the specified pwm value
void analogWrite(uint8_t pin, uint16_t value);
private:
Adafruit_PWMServoDriver pwm;
uint8_t address;
};

View File

@ -0,0 +1,18 @@
{
"configurations": [
{
"name": "windows-gcc-x64",
"includePath": [
"${workspaceFolder}/**"
],
"compilerPath": "gcc",
"cStandard": "${default}",
"cppStandard": "${default}",
"intelliSenseMode": "windows-gcc-x64",
"compilerArgs": [
""
]
}
],
"version": 4
}

View File

@ -0,0 +1,24 @@
{
"version": "0.2.0",
"configurations": [
{
"name": "C/C++ Runner: Debug Session",
"type": "cppdbg",
"request": "launch",
"args": [],
"stopAtEntry": false,
"externalConsole": true,
"cwd": "d:/Git/ESPMegaPRO-v3-SDK/Template Project/src",
"program": "d:/Git/ESPMegaPRO-v3-SDK/Template Project/src/build/Debug/outDebug",
"MIMode": "gdb",
"miDebuggerPath": "gdb",
"setupCommands": [
{
"description": "Enable pretty-printing for gdb",
"text": "-enable-pretty-printing",
"ignoreFailures": true
}
]
}
]
}

View File

@ -0,0 +1,58 @@
{
"C_Cpp_Runner.cCompilerPath": "gcc",
"C_Cpp_Runner.cppCompilerPath": "g++",
"C_Cpp_Runner.debuggerPath": "gdb",
"C_Cpp_Runner.cStandard": "",
"C_Cpp_Runner.cppStandard": "",
"C_Cpp_Runner.msvcBatchPath": "",
"C_Cpp_Runner.useMsvc": false,
"C_Cpp_Runner.warnings": [
"-Wall",
"-Wextra",
"-Wpedantic",
"-Wshadow",
"-Wformat=2",
"-Wcast-align",
"-Wconversion",
"-Wsign-conversion",
"-Wnull-dereference"
],
"C_Cpp_Runner.msvcWarnings": [
"/W4",
"/permissive-",
"/w14242",
"/w14287",
"/w14296",
"/w14311",
"/w14826",
"/w44062",
"/w44242",
"/w14905",
"/w14906",
"/w14263",
"/w44265",
"/w14928"
],
"C_Cpp_Runner.enableWarnings": true,
"C_Cpp_Runner.warningsAsError": false,
"C_Cpp_Runner.compilerArgs": [],
"C_Cpp_Runner.linkerArgs": [],
"C_Cpp_Runner.includePaths": [],
"C_Cpp_Runner.includeSearch": [
"*",
"**/*"
],
"C_Cpp_Runner.excludeSearch": [
"**/build",
"**/build/**",
"**/.*",
"**/.*/**",
"**/.vscode",
"**/.vscode/**"
],
"C_Cpp_Runner.useAddressSanitizer": false,
"C_Cpp_Runner.useUndefinedSanitizer": false,
"C_Cpp_Runner.useLeakSanitizer": false,
"C_Cpp_Runner.showCompilationTime": false,
"C_Cpp_Runner.useLinkTimeOptimization": false
}

View File

@ -0,0 +1,19 @@
#include <ESPMegaPRO.h>
#include <DigitalInputCard.hpp>
#include <DigitalOutputCard.hpp>
DigitalInputCard icard = DigitalInputCard(0x20, 0x21);
void setup() {
Serial.begin(115200);
// Instantiate Input Card
icard.begin();
}
void loop() {
// put your main code here, to run repeatedly:
for (int i = 0; i < 16; i++) {
Serial.print(icard.digitalRead(i));
}
}

View File

@ -0,0 +1,505 @@
//
// FILE: MultiSpeedI2CScanner.ino
// AUTHOR: Rob Tillaart
// VERSION: 0.1.17
// PURPOSE: I2C scanner at different speeds
// DATE: 2013-11-05
// URL: https://github.com/RobTillaart/MultiSpeedI2CScanner
// URL: http://forum.arduino.cc/index.php?topic=197360
//
#include <Arduino.h>
#include <Wire.h>
// FOR INTERNAL I2C BUS NANO 33 BLE
// #define WIRE_IMPLEMENT_WIRE1 1
// extern TwoWire Wire1;
void I2Cscan();
void displayHelp();
void reset();
void setAddress();
void setSpeed(char sp);
char getCommand();
TwoWire *wire;
const char version[] = "0.1.16";
// INTERFACE COUNT (TESTED TEENSY 3.5 AND ARDUINO DUE ONLY)
int wirePortCount = 1;
int selectedWirePort = 0;
// scans devices from 50 to 800 KHz I2C speeds.
// speed lower than 50 and above 400 can cause problems
long speed[10] = { 100, 200, 300, 400 };
int speeds;
int addressStart = 8;
int addressEnd = 119;
// DELAY BETWEEN TESTS
// for delay between tests of found devices.
#ifndef RESTORE_LATENCY
#define RESTORE_LATENCY 5
#endif
bool delayFlag = false;
// MINIMIZE OUTPUT
bool printAll = true;
bool header = true;
bool disableIRQ = false;
// STATE MACHINE
enum states {
STOP, ONCE, CONT, HELP
};
states state = STOP;
// TIMING
uint32_t startScan;
uint32_t stopScan;
///////////////////////////////////////////////////////////////////////////
//
// MAIN CODE
//
void setup()
{
Serial.begin(115200);
while (!Serial);
Wire.begin(14,33);
#if defined WIRE_IMPLEMENT_WIRE1 || WIRE_INTERFACES_COUNT > 1
Wire1.begin();
wirePortCount++;
#endif
#if defined WIRE_IMPLEMENT_WIRE2 || WIRE_INTERFACES_COUNT > 2
Wire2.begin();
wirePortCount++;
#endif
#if defined WIRE_IMPLEMENT_WIRE3 || WIRE_INTERFACES_COUNT > 3
Wire3.begin();
wirePortCount++;
#endif
#if defined WIRE_IMPLEMENT_WIRE4 || WIRE_INTERFACES_COUNT > 4
Wire4.begin();
wirePortCount++;
#endif
#if defined WIRE_IMPLEMENT_WIRE5 || WIRE_INTERFACES_COUNT > 5
Wire5.begin();
wirePortCount++;
#endif
wire = &Wire;
Serial.println();
reset();
}
void loop()
{
yield();
char command = getCommand();
switch (command)
{
case '@':
selectedWirePort = (selectedWirePort + 1) % wirePortCount;
Serial.print(F("<I2C PORT=Wire"));
Serial.print(selectedWirePort);
Serial.println(F(">"));
switch (selectedWirePort)
{
case 0:
wire = &Wire;
break;
#if defined WIRE_IMPLEMENT_WIRE1 || WIRE_INTERFACES_COUNT > 1
case 1:
wire = &Wire1;
break;
#endif
#if defined WIRE_IMPLEMENT_WIRE2 || WIRE_INTERFACES_COUNT > 2
case 2:
wire = &Wire2;
break;
#endif
#if defined WIRE_IMPLEMENT_WIRE3 || WIRE_INTERFACES_COUNT > 3
case 3:
wire = &Wire3;
break;
#endif
#if defined WIRE_IMPLEMENT_WIRE4 || WIRE_INTERFACES_COUNT > 4
case 4:
wire = &Wire4;
break;
#endif
#if defined WIRE_IMPLEMENT_WIRE5 || WIRE_INTERFACES_COUNT > 5
case 5:
wire = &Wire5;
break;
#endif
}
break;
case 's':
state = ONCE;
break;
case 'c':
state = CONT;
break;
case 'd':
delayFlag = !delayFlag;
Serial.print(F("<delay="));
Serial.println(delayFlag ? F("5>") : F("0>"));
break;
case 'e':
// eeprom test TODO
break;
case 'h':
header = !header;
Serial.print(F("<header="));
Serial.println(header ? F("yes>") : F("no>"));
break;
case 'p':
printAll = !printAll;
Serial.print(F("<print="));
Serial.println(printAll ? F("all>") : F("found>"));
break;
case 'i':
disableIRQ = !disableIRQ;
Serial.print(F("<irq="));
Serial.println(disableIRQ ? F("diabled>") : F("enabled>"));
break;
case '0':
case '1':
case '2':
case '4':
case '8':
case '9':
case 'M':
case 'N':
case 'O':
case 'P':
setSpeed(command);
break;
case 'r':
reset();
break;
case 'a':
setAddress();
break;
case 'q':
case '?':
state = HELP;
break;
default:
break;
}
switch (state)
{
case ONCE:
I2Cscan();
state = HELP;
break;
case CONT:
I2Cscan();
delay(1000);
break;
case HELP:
displayHelp();
state = STOP;
break;
case STOP:
break;
default: // ignore all non commands
break;
}
}
//////////////////////////////////////////////////////////////////////
void reset()
{
setSpeed('9');
selectedWirePort = 0;
addressStart = 8;
addressEnd = 119;
delayFlag = false;
printAll = true;
header = true;
disableIRQ = false;
state = STOP;
displayHelp();
}
void setAddress()
{
if (addressStart == 0)
{
addressStart = 8;
addressEnd = 119;
}
else
{
addressStart = 0;
addressEnd = 127;
}
Serial.print(F("<address Range = "));
Serial.print(addressStart);
Serial.print(F(".."));
Serial.print(addressEnd);
Serial.println(F(">"));
}
void setSpeed(char sp)
{
switch (sp)
{
case '1':
speed[0] = 100;
speeds = 1;
break;
case '2':
speed[0] = 200;
speeds = 1;
break;
case '4':
speed[0] = 400;
speeds = 1;
break;
case '8':
speed[0] = 800;
speeds = 1;
break;
case '9': // limited to 400 KHz
speeds = 8;
for (int i = 1; i <= speeds; i++) speed[i - 1] = i * 50;
break;
case '0': // limited to 800 KHz
speeds = 8;
for (int i = 1; i <= speeds; i++) speed[i - 1] = i * 100;
break;
// new in 0.1.10 - experimental
case 'M':
speed[0] = 1000;
speeds = 1;
break;
case 'N':
speed[0] = 3400;
speeds = 1;
break;
case 'O':
speed[0] = 5000;
speeds = 1;
break;
case 'P':
speed[0] = 100;
speed[1] = 400;
speed[2] = 1000;
speed[3] = 3400;
speed[4] = 5000;
speeds = 5;
break;
}
Serial.print("<speeds =");
for (int i = 0; i < speeds; i++)
{
Serial.print(' ');
Serial.print(speed[i]);
}
Serial.println(" >");
}
char getCommand()
{
char c = '\0';
if (Serial.available())
{
c = Serial.read();
}
return c;
}
void displayHelp()
{
Serial.print(F("\nArduino MultiSpeed I2C Scanner - "));
Serial.println(version);
Serial.println();
Serial.print(F("I2C ports: "));
Serial.print(wirePortCount);
Serial.print(F(" Current: Wire"));
Serial.println(selectedWirePort);
Serial.println(F("\t@ = toggle Wire - Wire1 .. Wire5 [e.g. TEENSY or Arduino Due]"));
Serial.println(F("Scan mode:"));
Serial.println(F("\ts = single scan"));
Serial.println(F("\tc = continuous scan - 1 second delay"));
Serial.println(F("\tq = quit continuous scan"));
Serial.println(F("\td = toggle latency delay between successful tests. 0 - 5 ms"));
Serial.println(F("\ti = toggle enable/disable interrupts"));
Serial.println(F("Output:"));
Serial.println(F("\tp = toggle printAll - printFound."));
Serial.println(F("\th = toggle header - noHeader."));
Serial.println(F("\ta = toggle address range, 0..127 - 8..119 (default)"));
Serial.println(F("Speeds:"));
Serial.println(F("\t0 = 100..800 KHz - step 100 (warning - can block!!)"));
Serial.println(F("\t1 = 100 KHz"));
Serial.println(F("\t2 = 200 KHz"));
Serial.println(F("\t4 = 400 KHz"));
Serial.println(F("\t9 = 50..400 KHz - step 50 < DEFAULT >"));
Serial.println();
Serial.println(F("\t!! HIGH SPEEDS - WARNING - can block - not applicable for UNO"));
Serial.println(F("\t8 = 800 KHz"));
Serial.println(F("\tM = 1000 KHz"));
Serial.println(F("\tN = 3400 KHz"));
Serial.println(F("\tO = 5000 KHz"));
Serial.println(F("\tP = 100 400 1000 3400 5000 KHz (standards)"));
Serial.println(F("\n\tr = reset to startup defaults."));
Serial.println(F("\t? = help - this page"));
Serial.println();
}
void I2Cscan()
{
startScan = millis();
uint8_t count = 0;
if (disableIRQ)
{
noInterrupts();
}
if (header)
{
Serial.print(F("TIME\tDEC\tHEX\t"));
for (uint8_t s = 0; s < speeds; s++)
{
Serial.print(F("\t"));
Serial.print(speed[s]);
}
Serial.println(F("\t[KHz]"));
for (uint8_t s = 0; s < speeds + 5; s++)
{
Serial.print(F("--------"));
}
Serial.println();
delay(100);
}
for (uint8_t address = addressStart; address <= addressEnd; address++)
{
bool printLine = printAll;
bool found[speeds];
bool fnd = false;
for (uint8_t s = 0; s < speeds ; s++)
{
yield(); // keep ESP happy
#if ARDUINO < 158 && defined (TWBR)
uint16_t PREV_TWBR = TWBR;
TWBR = (F_CPU / (speed[s] * 1000) - 16) / 2;
if (TWBR < 2)
{
Serial.println("ERROR: not supported speed");
TWBR = PREV_TWBR;
return;
}
#else
wire->setClock(speed[s] * 1000UL);
#endif
wire->beginTransmission (address);
found[s] = (wire->endTransmission () == 0);
fnd |= found[s];
// give device 5 millis
if (fnd && delayFlag) delay(RESTORE_LATENCY);
}
if (fnd) count++;
printLine |= fnd;
if (printLine)
{
Serial.print(millis());
Serial.print(F("\t"));
Serial.print(address, DEC);
Serial.print(F("\t0x"));
if (address < 0x10) Serial.print(0, HEX);
Serial.print(address, HEX);
Serial.print(F("\t"));
for (uint8_t s = 0; s < speeds ; s++)
{
Serial.print(F("\t"));
Serial.print(found[s] ? F("V") : F("."));
}
Serial.println();
}
}
/*
// FOOTER
if (header)
{
for (uint8_t s = 0; s < speeds + 5; s++)
{
Serial.print(F("--------"));
}
Serial.println();
Serial.print(F("TIME\tDEC\tHEX\t"));
for (uint8_t s = 0; s < speeds; s++)
{
Serial.print(F("\t"));
Serial.print(speed[s]);
}
Serial.println(F("\t[KHz]"));
}
*/
stopScan = millis();
if (header)
{
Serial.println();
Serial.print(count);
Serial.print(F(" devices found in "));
Serial.print(stopScan - startScan);
Serial.println(F(" milliseconds."));
}
interrupts();
}
// -- END OF FILE --

View File

@ -0,0 +1,27 @@
#include <ESPMegaPRO.h>
void lcd_send_stop_bit();
void setup() {
Serial.begin(115200);
lcd_send_stop_bit();
Serial.print("rest");
lcd_send_stop_bit();
Serial.print("connect");
lcd_send_stop_bit();
delay(1000);
Serial.print("whmi-wri 1024,115200,res0");
lcd_send_stop_bit();
}
void loop() {
while(Serial.available()) {
Serial.write(Serial.read());
}
}
void lcd_send_stop_bit() {
Serial.write(0xFF);
Serial.write(0xFF);
Serial.write(0xFF);
}