From ae8af439c2bee7bb631d352e1bfe119d7ad2f9ff Mon Sep 17 00:00:00 2001 From: "avery.babka" Date: Fri, 25 Jul 2025 14:06:04 -0500 Subject: [PATCH] Upload files to "/" --- MCP7940_ADI.ino | 320 ++++++++++++++++++++++ Oled_ADI.h | 1 + Oled_ADI.ino | 266 ++++++++++++++++++ motor.h | 121 ++++++++ motor.ino | 711 ++++++++++++++++++++++++++++++++++++++++++++++++ 5 files changed, 1419 insertions(+) create mode 100644 MCP7940_ADI.ino create mode 100644 Oled_ADI.h create mode 100644 Oled_ADI.ino create mode 100644 motor.h create mode 100644 motor.ino diff --git a/MCP7940_ADI.ino b/MCP7940_ADI.ino new file mode 100644 index 0000000..acfa6ed --- /dev/null +++ b/MCP7940_ADI.ino @@ -0,0 +1,320 @@ +#include "MCP7940_ADI.h" + +#include + +// Serial Monitor Cheat Cheat for setting time and alarms +// SETTING TIME +// format set a to 1 if 12hr mode, then b to 1 if PM, otherwise set to 00 for 24hr mode +// Thh:mm:22 ab mm/dd/yr w + +// Example for setting time to 3:41:00PM 2/4/17 SAT +////T03:41:00 11 02/04/17 7 + +// SETTING ALARM +// abc = alarm mask, like 001=Minutes alarm +// d = AM/PM, 1=PM w=weekday +// A0hh:mm:ss abc d mm/dd w +// Example for setting alarm for 12:13PM on Sunday 2/4 +// A012:13:00 111 1 02/04 1 + +// defines +#define RTCADDR B1101111 // page11 datasheet +#define RTCSEC 0x00 +#define RTCMIN 0x01 +#define RTCHOUR 0x02 +#define RTCWKDAY 0x03 +#define RTCDATE 0x04 +#define RTCMTH 0x05 +#define RTCYEAR 0x06 +#define CONTROL 0x07 +#define OSCTRIM 0x08 +#define ALM0SEC 0x0A +#define ALM0MIN 0x0B +#define ALM0HOUR 0x0C +#define ALM0WKDAY 0x0D +#define ALM0DATE 0x0E +#define ALM0MTH 0x0F +#define ALM1SEC 0x11 +#define ALM1MIN 0x12 +#define ALM1HOUR 0x13 +#define ALM1WKDAY 0x14 +#define ALM1DATE 0x15 +#define ALM1MTH 0x16 +#define PWRDNMIN 0x18 +#define PWRDNHOUR 0x19 +#define PWRDNDATE 0x1A +#define PWRDNMTH 0x1B +#define PWRUPMIN 0x1C +#define PWRUPHOUR 0x1D +#define PWRUPDATE 0x1E +#define PWRUPMTH 0x1F + +// variables used here +byte rtcSeconds, rtcMinutes, rtcHours; +byte rtcWeekDay, rtcDay, rtcMonth, rtcYear; +boolean rtc12hrMode, rtcPM, rtcOscRunning, rtcPowerFail, rtcVbatEn; +String weekDay[] = {"SUN", "MON", "TUE", "WED", "THU", "FRI", "SAT"}; +boolean mfpPinTriggered = false; + +void rtcInit() +{ // RTC Initialize + int i; + // sets up I2C at 100kHz + Wire.setClock(100000); + Wire.begin(); + + Wire.beginTransmission(RTCADDR); + Wire.write(CONTROL); + Wire.write(0x00); // clear out the entire control register // was 0x41 + Wire.endTransmission(); + + Wire.beginTransmission(RTCADDR); + Wire.write(RTCWKDAY); + Wire.endTransmission(); + Wire.requestFrom(RTCADDR, 1); + delay(1); + byte rtcWeekdayRegister = Wire.read(); + rtcWeekdayRegister |= 0x08; // enable Battery backup + Wire.beginTransmission(RTCADDR); + Wire.write(RTCWKDAY); + Wire.write(rtcWeekdayRegister); + Wire.endTransmission(); + + Wire.beginTransmission(RTCADDR); + Wire.write(RTCSEC); + Wire.endTransmission(); + Wire.requestFrom(RTCADDR, 1); + delay(1); + byte rtcSecondRegister = Wire.read(); // read out seconds + rtcSecondRegister |= 0x80; // flip the start bit to ON + Wire.beginTransmission(RTCADDR); + Wire.write(RTCSEC); + Wire.write(rtcSecondRegister); // write it back in... now the RTC is running + Wire.endTransmission(); + + // read time out of rtc + Wire.beginTransmission(RTCADDR); + Wire.write(RTCSEC); + Wire.endTransmission(); + Wire.requestFrom(RTCADDR, 7); // pull out all timekeeping registers + delay(1); // little delay +} + +uint32_t getEpochRtc(void) +{ + + uint32_t ep; + + Wire.beginTransmission(RTCADDR); + Wire.write(RTCSEC); + Wire.endTransmission(); + Wire.requestFrom(RTCADDR, 7); // pull out all timekeeping registers + delay(1); // little delay + + // now read each byte in and clear off bits we don't need, hence the AND operations + rtcSeconds = Wire.read() & 0x7F; + rtcMinutes = Wire.read() & 0x7F; + rtcHours = Wire.read() & 0x7F; + rtcWeekDay = Wire.read() & 0x3F; + rtcDay = Wire.read() & 0x3F; + rtcMonth = Wire.read() & 0x3F; + rtcYear = Wire.read(); + + // now format the data, combine lower and upper parts of byte to give decimal number + rtcSeconds = (rtcSeconds >> 4) * 10 + (rtcSeconds & 0x0F); + rtcMinutes = (rtcMinutes >> 4) * 10 + (rtcMinutes & 0x0F); + + if ((rtcHours >> 6) == 1) // check for 12hr mode + rtc12hrMode = true; + else + rtc12hrMode = false; + + // 12hr check and formatting of Hours + if (rtc12hrMode) + { // 12 hr mode so get PM/AM + if ((rtcHours >> 5) & 0x01 == 1) + rtcPM = true; + else + rtcPM = false; + rtcHours = ((rtcHours >> 4) & 0x01) * 10 + (rtcHours & 0x0F); // only up to 12 + } + else + { // 24hr mode + rtcPM = false; + rtcHours = ((rtcHours >> 4) & 0x03) * 10 + (rtcHours & 0x0F); // uses both Tens digits, '23' + } + + // weekday register has some other bits in it, that are pulled out here + if ((rtcWeekDay >> 5) & 0x01 == 1) + rtcOscRunning = true; // good thing to check to make sure the RTC is running + else + rtcOscRunning = false; + if ((rtcWeekDay >> 4) & 0x01 == 1) + rtcPowerFail = true; // if the power fail bit is set, we can then go pull the timestamp for when it happened + else + rtcPowerFail = false; + if ((rtcWeekDay >> 3) & 0x01 == 1) // check to make sure the battery backup is enabled + rtcVbatEn = true; + else + rtcVbatEn = false; + + rtcWeekDay = rtcWeekDay & 0x07; // only the bottom 3 bits for the actual weekday value + + // more formatting bytes into decimal numbers + rtcDay = (rtcDay >> 4) * 10 + (rtcDay & 0x0F); + rtcMonth = ((rtcMonth >> 4) & 0x01) * 10 + (rtcMonth & 0x0F); + rtcYear = (rtcYear >> 4) * 10 + (rtcYear & 0x0F); + + dt.Second = rtcSeconds; + dt.Minute = rtcMinutes; + dt.Hour = rtcHours; + dt.Wday = rtcWeekDay; + dt.Day = rtcDay; + dt.Month = rtcMonth; + dt.Year = rtcYear; + + // dt.Year += 30; //from 1970. not 2000 + dt.Year += 30; + ep = makeTime(dt); + // dt.Year -= 30; + + if (VB) + { + Serial.print("Epoch Created: "); + Serial.println(ep); + // print everything out + Serial.print(rtcHours); + Serial.print(":"); + Serial.print(rtcMinutes); + Serial.print(":"); + Serial.print(rtcSeconds); + + if (rtc12hrMode == true && rtcPM == true) + Serial.print(" PM "); + else if (rtc12hrMode == true && rtcPM == false) + Serial.print(" AM "); + + if (rtc12hrMode == false) + Serial.print(" 24hr "); + + if (rtcWeekDay > 0) + if (rtcWeekDay < 8) + { + // Serial.print("WeekDay="); + // Serial.print(rtcWeekDay); + Serial.print(weekDay[rtcWeekDay - 1]); + Serial.print(" "); + Serial.print(rtcMonth); + Serial.print("/"); + Serial.print(rtcDay); + Serial.print("/"); + Serial.print(rtcYear); + Serial.println(""); + } + } + + return (ep); +} + +/* + void checkAlarm() { + if (mfpPinTriggered == true) { + Serial.println("MFP Triggered"); + mfpPinTriggered = false; + Wire.beginTransmission(RTCADDR);//Enable Alarm0 + Wire.write(ALM0WKDAY); + Wire.endTransmission(); + Wire.requestFrom(RTCADDR, 1); + delay(1); + byte alarm0Check = Wire.read(); + Serial.println(alarm0Check, BIN); + if (((alarm0Check >> 3) & 0x01) == 1) + Serial.println("Alarm0 Triggered"); + else Serial.println("Alarm0 False Alarm"); + } + + } +*/ + +void setEpochRtc(uint32_t ep) +{ + tmElements_t et; + + parms.write(69, ep); // just to monitor last rtc set + + Serial.print("Setting RTC with Epoch: "); + Serial.println(ep); + + // breakTime off by 30 years + breakTime(ep, et); + et.Year -= 30; + + // let's go change the time: + // first stop the clock: + Wire.beginTransmission(RTCADDR); + Wire.write(RTCSEC); + Wire.write(0x00); + Wire.endTransmission(); + + getEpochRtc(); + // rtcGetTime();//go grab the time again just to make sure the osc stopped + + if (rtcOscRunning == false) + { // oscillator stopped, we're good + Serial.println("RTC has stopped - Changing Time"); + Wire.beginTransmission(RTCADDR); // set the time/date + Wire.write(RTCSEC); + Wire.write(((et.Second / 10) << 4) + (et.Second % 10)); + Wire.write(((et.Minute / 10) << 4) + (et.Minute % 10)); + Wire.write(((et.Hour / 10) << 4) + (et.Hour % 10)); + Wire.write(et.Wday + 8); + Wire.write(((et.Day / 10) << 4) + (et.Day % 10)); + Wire.write(((et.Month / 10) << 4) + (et.Month % 10)); + Wire.write(((et.Year / 10) << 4) + (et.Year % 10)); // two digit year + Wire.endTransmission(); + + Wire.beginTransmission(RTCADDR); // start back up + Wire.write(RTCSEC); + Wire.endTransmission(); + Wire.requestFrom(RTCADDR, 1); + delay(1); + byte rtcSecondRegister = Wire.read(); + rtcSecondRegister |= 0x80; // start bit! + Wire.beginTransmission(RTCADDR); + Wire.write(RTCSEC); + Wire.write(rtcSecondRegister); + Wire.endTransmission(); + + Wire.beginTransmission(RTCADDR); // Enable Alarm0 + Wire.write(CONTROL); + Wire.endTransmission(); + Wire.requestFrom(RTCADDR, 1); + delay(1); + byte rtcControlRegister = Wire.read(); + rtcControlRegister |= 0x10; // enable alm0 + Wire.beginTransmission(RTCADDR); + Wire.write(CONTROL); + Wire.write(rtcControlRegister); + Wire.endTransmission(); + } + Serial.println(" --RTC set finished"); +} + +String makeTimeStr(void) +{ + getEpochRtc(); + return (String(dt.Year - 30) + "-" + String(dt.Month) + "-" + String(dt.Day) + "-" + String(dt.Hour) + "-" + String(dt.Minute) + "-" + String(dt.Second)); +} + +void toggleMFP(void) +{ + Wire.beginTransmission(RTCADDR); + Wire.write(CONTROL); + Wire.write(0x80); + Wire.endTransmission(); + delay(250); + Wire.beginTransmission(RTCADDR); + Wire.write(CONTROL); + Wire.write(0x00); + Wire.endTransmission(); +} diff --git a/Oled_ADI.h b/Oled_ADI.h new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/Oled_ADI.h @@ -0,0 +1 @@ + diff --git a/Oled_ADI.ino b/Oled_ADI.ino new file mode 100644 index 0000000..bca8882 --- /dev/null +++ b/Oled_ADI.ino @@ -0,0 +1,266 @@ + +#include +#include +#include + +#define SCREEN_WIDTH 128 // OLED display width, in pixels +#define SCREEN_HEIGHT 32 // OLED display height, in pixels + +// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins) +#define OLED_RESET 4 // Reset pin # (or -1 if sharing Arduino reset pin) +Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET); + +#define LOGO_HEIGHT 32 +#define LOGO_WIDTH 128 + +#define line 12 + +/* + static const unsigned char PROGMEM logo_bmp[] = + { + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0xFF, 0xE0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x03, 0xC0, 0x78, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0x80, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x07, 0x00, 0x0E, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0F, 0x80, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x0C, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x18, 0x00, 0x03, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x30, 0x00, 0x01, 0xC0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x70, 0x00, 0x00, 0xC0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x0F, 0xC0, 0x60, 0x00, 0xF0, 0xE1, 0xE0, 0xF8, 0x1F, 0xC0, 0xF1, 0x87, 0x87, 0x83, 0xC0, 0xFC, + 0x3F, 0xF0, 0xE0, 0x01, 0xF8, 0x61, 0xE0, 0xF8, 0x7F, 0xE0, 0xF7, 0x8F, 0x87, 0x83, 0xE1, 0xCE, + 0x60, 0xF0, 0xE0, 0x01, 0xF8, 0x61, 0xE0, 0xF8, 0x41, 0xF0, 0xF7, 0x87, 0x87, 0x83, 0xE3, 0x86, + 0x00, 0xF8, 0xE0, 0x01, 0xF8, 0x71, 0xE0, 0xF8, 0x00, 0xF0, 0xFF, 0x87, 0x87, 0x83, 0xC3, 0x80, + 0x00, 0x78, 0xC0, 0x01, 0xF8, 0x71, 0xE0, 0xF8, 0x00, 0xF0, 0xF8, 0x07, 0x87, 0x83, 0xC3, 0xC0, + 0x00, 0xF8, 0xE0, 0x79, 0xF8, 0x71, 0xE0, 0xF8, 0x01, 0xF0, 0xF0, 0x07, 0x87, 0x83, 0xC3, 0xF8, + 0x0F, 0xF8, 0xE1, 0xFD, 0xF8, 0x71, 0xE0, 0xF8, 0x1F, 0xF0, 0xF0, 0x07, 0x87, 0x83, 0xC3, 0xFE, + 0x7C, 0x78, 0xE1, 0xCF, 0xF8, 0xF1, 0xE0, 0xF8, 0x78, 0xF0, 0xF0, 0x07, 0x8F, 0x83, 0xC1, 0xFF, + 0xF8, 0x78, 0xE3, 0xFF, 0xF8, 0xE1, 0xE0, 0xF8, 0xF0, 0xF0, 0xF0, 0x07, 0x8F, 0x83, 0xC0, 0x3F, + 0xF0, 0x78, 0xF3, 0xFE, 0xF9, 0xE1, 0xE0, 0xF8, 0xF0, 0xF0, 0xF0, 0x07, 0x87, 0x83, 0xC0, 0x0F, + 0xF0, 0xF8, 0x79, 0x8F, 0xF9, 0xE1, 0xF0, 0xF9, 0xF0, 0xF0, 0xF0, 0x07, 0x87, 0x87, 0xC2, 0x07, + 0xF8, 0xF8, 0x7D, 0x87, 0xFB, 0xC1, 0xFF, 0xF8, 0xF1, 0xF0, 0xF0, 0x0F, 0x87, 0xFF, 0xE3, 0x0E, + 0x7F, 0x7C, 0x3F, 0x00, 0x1F, 0x80, 0xFE, 0xF8, 0xFE, 0xFC, 0xF0, 0x0F, 0x83, 0xFB, 0xE3, 0xFC, + 0x1C, 0x38, 0x13, 0xFF, 0xF9, 0x80, 0x38, 0x00, 0x3C, 0x38, 0x00, 0x00, 0x00, 0xE0, 0x00, 0xF0, + 0x00, 0x00, 0x1F, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xE0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x03, 0xFF, 0xF8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x7F, 0x1F, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x03, 0xFF, 0xE0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x00, 0x01, 0xC0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x1F, 0xFF, 0xFF, 0xFE, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xFF, 0xFF, 0xE0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xFC, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 + }; +*/ + +String OledLine1 = ""; +String OledLine2 = ""; +String OledLine3 = ""; +String OledLine4 = ""; +String OledLine5 = ""; +String OledLine6 = ""; + +void oled_init() +{ + + Serial.println("Init Oled"); + + // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally + if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) + { // Address 0x3C for 128x32 + Serial.println(F("SSD1306 allocation failed")); + // for(;;); // Don't proceed, loop forever + } + // drawbitmap(); + + display.display(); + // delay(2000); // Pause for 2 seconds + // displayText("Init Finished"); + OledLine5 = String("Init Finished"); + OledLine4 = String("Version: ") + myVersion; + display.setRotation(2); + ScrollInText(); +} + +void ShowSerialScreen(void) +{ + sprintf(tmpstr, "Ver: %d-%d %d", myVersion, parm[1], parm[64]); + OledLine4 = String(tmpstr); + OledLine5 = makeTimeStr(); + OledLine6 = String("Batt: ") + String(voltageNow / 10) + String("V"); + + ScrollInText(); + + Serial.println(OledLine4); + Serial.println(OledLine5); + Serial.println(OledLine6); +} + +void showAddrScreen(void) +{ + char tmpstr[30]; + + sprintf(tmpstr, "Addr %d Next = ADDR", parm[1]); + OledLine4 = String(tmpstr); + OledLine5 = String(" Save = BOOT"); + OledLine6 = String(" Exit = RESET"); + ScrollInText(); +} + +void displayText(String str) +{ + // if(Timers[OLEDHOLDTIMER] < 50) return; + OledLine4 = String(""); + OledLine5 = str; + OledLine6 = String(""); + ScrollInText(); + Serial.println(str); +} + +/* + void ShowSensorScreen(long serial, int dat, int ssi) + { + OledLine4 = String(serial); + OledLine4 += " "; + OledLine4 += make_time_str(); + //OledLine4 += String(tme / 100); + //OledLine4 += String(":"); + //OledLine4 += String(tme % 100); + OledLine5 = "DATA: "; + OledLine5 += String(dat); + OledLine6 = "RSSI: "; + OledLine6 += String(ssi); + OledLine6 += " Q: "; + OledLine6 += atQ_size(); + ScrollInText(); + // Serial.println(OledLine4); + // Serial.println(OledLine5); + // Serial.println(OledLine6); + } +*/ + +#define SCROLL 0 // leave 0 because scroll times out power relay +void ScrollInText(void) +{ + + Timers[OLEDBLANKTIMER] = 0; + + display.setTextSize(1); + display.setTextColor(WHITE); + + if (SCROLL == 1) + for (int i = 1; i < 33; i++) + { + display.clearDisplay(); + display.setCursor(1, 1 - i); + display.println(OledLine1); + display.setCursor(1, (line * 1) - i); + display.println(OledLine2); + display.setCursor(1, (line * 2) - i); + display.println(OledLine3); + display.setCursor(1, (line * 3) - i); + display.println(OledLine4); + display.setCursor(1, (line * 4) - i); + display.println(OledLine5); + display.setCursor(1, (line * 5) - i); + display.println(OledLine6); + display.display(); + delay(20); + } + + OledLine1 = OledLine4; + OledLine2 = OledLine5; + OledLine3 = OledLine6; + + display.clearDisplay(); + display.setCursor(1, 1); + display.println(OledLine1); + display.setCursor(1, (line * 1)); + display.println(OledLine2); + display.setCursor(1, (line * 2)); + display.println(OledLine3); + display.display(); +} + +void displayVoltage(void) +{ + int v; + v = io_array[BATT_V]; // / 10; + sprintf(tmpstr, "V: %2d.%d", v / 10, v % 10); + displayTextXY(tmpstr, 40, 0, 1, 1); + v = motorGetCurrent(0); + sprintf(tmpstr, "M0: %2d.%d", v / 10, v % 10); + displayTextXY(tmpstr, 40, 10, 0, 1); + v = motorGetCurrent(1); + sprintf(tmpstr, "M1: %2d.%d", v / 10, v % 10); + displayTextXY(tmpstr, 40, 20, 0, 1); + v = motorGetPosition(0); + sprintf(tmpstr, "%4d", v); + displayTextXY(tmpstr, 100, 10, 0, 1); + v = motorGetPosition(1); + sprintf(tmpstr, "%4d", v); + displayTextXY(tmpstr, 100, 20, 0, 1); +} + +void displayTextXY(char *buff, uint8_t x, uint8_t y, bool clear, bool dispNow) +{ + if (Timers[OLEDHOLDTIMER] < 50) + return; + + if (clear) + display.clearDisplay(); + display.setCursor(x, y); + display.println(buff); + if (dispNow) + display.display(); +} + +/* + void drawbitmap(void) { + display.clearDisplay(); + + display.drawBitmap( + (display.width() - LOGO_WIDTH ) / 2, + (display.height() - LOGO_HEIGHT) / 2, + logo_bmp, LOGO_WIDTH, LOGO_HEIGHT, 1); + display.display(); + delay(1000); + } +*/ + +//Draws A Loading Bar While Updating From OTA +void drawLoadingBar(int current, int total) +{ + static int lastProgress = -1; // Track the last progress value + + int progress = static_cast(static_cast(current) / total * 100); + progress = constrain(progress, 0, 100); + + // Only update the display if progress has changed + if (progress != lastProgress) + { + const int TEXT_X = (SCREEN_WIDTH - 17 * 6) / 2; + int rectWidth = map(progress, 0, 100, 0, SCREEN_WIDTH); + Serial.println(progress); + //digitalWrite(HEARTBEATLED,!digitalRead(HEARTBEATLED)); + display.clearDisplay(); + + // Draw the progress bar + display.fillRect(0, 12, rectWidth, 20, WHITE); + + // Draw "Downloading" text + display.setTextColor(WHITE); + display.setCursor(TEXT_X, 0); + display.print("Downloading "); + display.print(progress); + display.print(" %"); + + // Update the display + display.display(); + + + + + lastProgress = progress; + } +} diff --git a/motor.h b/motor.h new file mode 100644 index 0000000..86c78f1 --- /dev/null +++ b/motor.h @@ -0,0 +1,121 @@ +#ifndef MOTOR_H +#define MOTOR_H + +bool motorInit(); +void motorTask(); +void motorMove(uint16_t position); + +void motorStart(uint8_t motor); // 1 - Master, 2 - Slave, 3 - Both +void motorStop(); +uint8_t motorCycle(uint16_t cycles, uint16_t cycle_delay); + +void motorSetSpeed(uint8_t motor, uint8_t duty); +uint8_t motorGetSetSpeed(uint8_t motor); +uint8_t motorGetActualSpeed(uint8_t motor); + +void motorSetPosition(uint8_t motor, uint16_t position); +uint16_t motorGetPosition(uint8_t motor); + +void motorSetCurrentLimit(uint8_t motor, uint16_t limit); +uint16_t motorGetCurrent(uint8_t motor); + +uint8_t motorGetFlags(uint8_t motor, uint8_t flag); + +void motorJog(uint8_t motor, uint8_t state, uint16_t position); + +#define MASTER 0 +#define SLAVE 1 + +enum +{ + MOTOR_ERROR_NONE = 0, + MOTOR_ERROR_TIMEOUT, // 1 + MOTOR_ERROR_MAX_DELTA, // 2 + MOTOR_ERROR_M1_FATAL, // 3 + MOTOR_ERROR_M2_FATAL, // 4 + MOTOR_ERROR_M1_CURRENT, // 5 + MOTOR_ERROR_M2_CURRENT, // 6 + MOTOR_ERROR_M1_SAT, // 7 + MOTOR_ERROR_M2_SAT, // 8 + MOTOR_ERROR_M1_VOLTS, // 9 + MOTOR_ERROR_M2_VOLTS, // 10 + MOTOR_ERROR_M1_TEMP, // 11 + MOTOR_ERROR_M2_TEMP, // 12 + MOTOR_ERROR_M1_BKDRV, // 13 + MOTOR_ERROR_M2_BKDRV, // 14 + MOTOR_ERROR_M1_PARM, // 15 + MOTOR_ERROR_M2_PARM, // 16 + MOTOR_ERROR_START_TIMEOUT // 17 + +} motorError_t; + +uint16_t motorError; + +enum +{ + MOTOR_IDLE = 0, + MOTOR_START = 1, + MOTOR_RAMP = 10, + MOTOR_RUN = 50, + MOTOR_END = 100, + MOTOR_SEAL = 110, + MOTOR_STOP = 200, + MOTOR_CYCLE_DELAY = 1000 +} motorState_t; + +extern int motorState; + +enum +{ + MOTOR_FLAG_VOLTAGE_ERROR = 0, + MOTOR_FLAG_TEMP_ERROR, + MOTOR_FLAG_MOTION, + MOTOR_FLAG_OVERLOAD, + MOTOR_FLAG_BACKDRIVE, + MOTOR_FLAG_PARAMETER, + MOTOR_FLAG_SATURATION, + MOTOR_FLAG_FATAL_ERROR +}; + +bool motorDir; // 0 = extend, 1 = retract +#define EXTEND 1 +#define RETRACT 0 + +int8_t currentDir; // actual running direction + // +#define RUN_IDLE 0 +#define RUN_RETRACT -1 +#define RUN_EXTEND 1 + +uint16_t masterSpeed; +// extern uint16_t masterSpeed; + +extern bool bSyncEnable; +bool jogMotor; + +// the following values to be suppled by +// end application +// extern int32_t parm[100]; +#define P_MOTOR_ADJ_INTERVAL 41 +#define P_MOTOR_ADJ_I 42 +#define P_MOTOR_ADJ_P 43 +#define P_MAX_DELTA 44 +#define P_MAX_CURRENT 45 +#define P_MOTOR_SPEED 46 +#define P_EXTEND_LIMIT 47 +#define P_SLAVE_OFFSET 48 +#define P_MAX 49 + +const int32_t defParms[P_MAX] = + { + 0, + 5, + 100, + 50, + 200, + 250, + 50, + 3000, + 0}; + +#endif // MOTOR_H diff --git a/motor.ino b/motor.ino new file mode 100644 index 0000000..f1ba1c7 --- /dev/null +++ b/motor.ino @@ -0,0 +1,711 @@ +// ------------------------------------------------------------------------ +// J1939 - Receiving Messages +// ------------------------------------------------------------------------ +// +// IMPOPRTANT: Depending on the CAN shield used for this programming sample, +// please make sure you set the proper CS pin in module can.cpp. +// +// This Arduino program is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +#include +// #include + +#include "motor.h" +#include "mcp_can.h" +#include "can_ext.h" +// #include "display.h" +// #include "j1939dtc.h" +// #include "j1939.h" +#include "printControl.h" + +byte nMyAddr = 0; +#define SA_PREFERRED 128 +#define ADDRESSRANGEBOTTOM 129 +#define ADDRESSRANGETOP 247 + +#define GLOBALADDRESS 255 +#define NULLADDRESS 254 // NAME Fields Default + +// from Thompson Electrak document 5.2.1 +#define NAME_IDENTITY_NUMBER 0 // 0xFFFFFF +#define NAME_MANUFACTURER_CODE 547 // 0xFFF +#define NAME_FUNCTION_INSTANCE 0 +#define NAME_ECU_INSTANCE 0 // 0x01 +#define NAME_FUNCTION 255 +#define NAME_RESERVED 0 +#define NAME_VEHICLE_SYSTEM 0 +#define NAME_VEHICLE_SYSTEM_INSTANCE 0 +#define NAME_INDUSTRY_GROUP 0x00 // Global +#define NAME_ARBITRARY_ADDRESS_CAPABLE 0x01 + +unsigned char ecuNAME[] = + { + (byte)(NAME_IDENTITY_NUMBER & 0xFF), + (byte)((NAME_IDENTITY_NUMBER >> 8) & 0xFF), + (byte)(((NAME_MANUFACTURER_CODE << 5) & 0xFF) | (NAME_IDENTITY_NUMBER >> 16)), + (byte)(NAME_MANUFACTURER_CODE >> 3), + (byte)((NAME_FUNCTION_INSTANCE << 3) | NAME_ECU_INSTANCE), + (byte)(NAME_FUNCTION), + (byte)(NAME_VEHICLE_SYSTEM << 1), + (byte)((NAME_ARBITRARY_ADDRESS_CAPABLE << 7) | (NAME_INDUSTRY_GROUP << 4) | (NAME_VEHICLE_SYSTEM_INSTANCE))}; + +union u_data +{ + struct + { + byte LB; + byte HB; + } v; + unsigned int w; +}; + +int motorState; + +byte blank_response[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x7F, 0xFF, 0xFF}; +void SetNAME(long v81, int nManufacturerCode, byte nFunctionInstance, byte nECUInstance, + byte nFunction, byte nVehicleSystem, byte nVehicleSystemInstance, byte nIndustryGroup, byte nArbitraryAddressCapable); + +void serviceCAN(); +void motorSync(); +uint16_t motorErrorFlags(byte motor); + +// J1939 j1939; + +// Thompson Electrak HD CAN status data +#define motorPGN 0xEF00 +#define canAddrClaim 0xEE00 + +typedef union +{ + byte v[8]; + struct + { + unsigned position : 14; + unsigned current : 9; + unsigned speed : 5; + unsigned ve : 2; + unsigned te : 2; + unsigned motion : 1; + unsigned overload : 1; + unsigned backdrive : 1; + unsigned parameter : 1; + unsigned saturation : 1; + unsigned fatalerror : 1; + unsigned factory : 18; + unsigned unused : 8; + } bits; +} motorStatus_t; + +// Thompson Electrak HD CAN control data +typedef union +{ + byte v[8]; + struct + { + unsigned position : 14; + unsigned current_limit : 9; + unsigned speed : 5; + unsigned move : 1; + unsigned factory : 35; + } bits; +} motorControl_t; + +motorStatus_t mStatus[2]; +motorControl_t mCtl[2]; +bool newData[2]; + +#define addrM 19 // master can address +#define addrS 20 // slave can address + +uint8_t M1Speed; +uint8_t M2Speed; + +float motorIntegral = 0; +int16_t maxDelta, minDelta; +uint8_t maxSpeed; +uint32_t startTime, runTime; +uint16_t maxCurrentM, maxCurrentS; + +bool bDir = 1; // 1 = extend, 0 - retract +bool bSyncEnable = 1; + +uint16_t cycleMax; +uint16_t cycleCount; +uint16_t cycleDelay; + +struct +{ + unsigned master : 1; + unsigned slave : 1; +} motor_start; + +uint16_t startMaster, startSlave; +uint16_t lastPosition; + +int32_t slaveCurrentBalance = 0; // v984 DES + +/******************* end of variables ***********************/ + +// des +uint16_t getMaxCurrent(uint16_t i) +{ + if (i == 0) + return (maxCurrentM); + return (maxCurrentS); +} + +uint16_t getCurrent(uint16_t i) +{ + if (i == 0) + return (motorGetCurrent(MASTER)); + return (motorGetCurrent(SLAVE)); +} + +void setCurrentBalance(int32_t i) +{ + slaveCurrentBalance = i; +} + +// ------------------------------------------------------------------------ +// SYSTEM: Setup routine runs on power-up or reset +// ------------------------------------------------------------------------ +bool motorInit() +{ + byte i; + bool canInit = 0; + + // j1939.init(); + + /* + // Set the NAME + SetNAME(NAME_IDENTITY_NUMBER, + NAME_MANUFACTURER_CODE, + NAME_FUNCTION_INSTANCE, + NAME_ECU_INSTANCE, + NAME_FUNCTION, + NAME_VEHICLE_SYSTEM, + NAME_VEHICLE_SYSTEM_INSTANCE, + NAME_INDUSTRY_GROUP, + NAME_ARBITRARY_ADDRESS_CAPABLE); + + */ + + // Initialize the CAN controller + if (canInitialize(CAN_250KBPS) == CAN_OK) // Baud rates defined in mcp_can_dfs.h + { + Serial.print("CAN Init OK.\n\r\n\r"); + canInit = 1; + } + else + { + Serial.print("CAN Init Failed.\n\r"); + return (0); + delay(1000); + } + + j1939Transmit(motorPGN, 6, 255, addrM, (byte *)&mCtl[MASTER], 8); + j1939Transmit(motorPGN, 6, 255, addrS, (byte *)&mCtl[SLAVE], 8); + + mCtl[MASTER].bits.position = 0; + mCtl[MASTER].bits.current_limit = 250; + + mCtl[SLAVE].bits.position = 0; + mCtl[SLAVE].bits.current_limit = 250; + + motorState = MOTOR_IDLE; + + return (1); + +} // end setup + +// ------------------------------------------------------------------------ +// Main Loop - Arduino Entry Point +// ------------------------------------------------------------------------ +void serviceCan() +{ + // Declarations + byte nPriority; + byte nSrcAddr; + byte nDestAddr; + byte nData[8]; + int nDataLen; + long lPGN; + long pgnReq; + byte i; + + char sString[80]; + + // j1939.tmrControl(); + // Check for received J1939 messages + if (j1939Receive(&lPGN, &nPriority, &nSrcAddr, &nDestAddr, nData, &nDataLen) == 0) + { + if (lPGN != 0) + { + if (nDataLen == 0) + { + strcpy(printBuff, "\tNo Data"); + printPrint(); + } + else + { + for (int nIndex = 0; nIndex < nDataLen; nIndex++) + { + mStatus[nSrcAddr - addrM].v[nIndex] = nData[nIndex]; + // Serial.print( nData[nIndex]); Serial.print(" "); + } + // Serial.println(); + } + + if (nSrcAddr != addrM && nSrcAddr != addrS) + return; + switch ((int)lPGN) + { + case motorPGN: + + newData[nSrcAddr - addrM] = 1; + j1939Transmit(motorPGN, 6, 255, nSrcAddr, (byte *)&mCtl[nSrcAddr - addrM], 8); + break; + + case canAddrClaim: + break; + + default: + if (vbCAN) + { + sprintf(printBuff, "\r\nUnknown PGN 0x%X %ld", lPGN, lPGN); + printPrint(); + } + break; + } // end 1PGN switch + } + } // end if +} // end loop + +void motorTask() +{ + serviceCan(); + + switch (motorState) + { + case MOTOR_IDLE: + if (motor_start.master || motor_start.slave) + { + motorState = MOTOR_START; + } + currentDir = RUN_IDLE; + break; + + case MOTOR_START: // + + // mStatus[0].bits.position = 0; + // mStatus[1].bits.position = 0; + + masterSpeed = parm[P_MOTOR_SPEED]; + motorSetSpeed(MASTER, masterSpeed); // convert duty cycle to 0 - 31 + motorSetSpeed(SLAVE, masterSpeed); + motorError = MOTOR_ERROR_NONE; + + motorIntegral = 0.0; + lastPosition = mStatus[MASTER].bits.position; + maxDelta = 0; + minDelta = 0; + maxSpeed = 0; + maxCurrentM = 0; + maxCurrentS = 0; + startTime = millis(); + Timers[MOTOR_TIMER] = 0; + printStartOfRun(); + + // start the motors + mCtl[MASTER].bits.move = motor_start.master; + mCtl[SLAVE].bits.move = motor_start.slave; + newData[MASTER] = 0; + newData[SLAVE] = 0; + motorState = MOTOR_RAMP; + break; + + case MOTOR_RAMP: // wait here until movement is detected + if (newData[MASTER] == 1 && newData[SLAVE] == 1) + { + if ((mStatus[MASTER].bits.motion == 1) && (mStatus[SLAVE].bits.motion == 1)) + { + motorState = MOTOR_RUN; + } + newData[MASTER] = 0; + newData[SLAVE] = 0; + } + if (Timers[MOTOR_TIMER] > parm[30]) + { + motorError = MOTOR_ERROR_START_TIMEOUT; + Serial.println("Start Timeout"); + motorState = MOTOR_STOP; + } + break; + + case MOTOR_RUN: + if (newData[MASTER] == 1 && newData[SLAVE] == 1) + { + motorSync(); + if ((mStatus[MASTER].bits.motion == 0) && (mStatus[SLAVE].bits.motion == 0)) + { + motorState = MOTOR_STOP; + } + newData[MASTER] = 0; + newData[SLAVE] = 0; + Timers[MOTOR_TIMER] = 0; + if (mStatus[MASTER].bits.position > lastPosition) + currentDir = RUN_EXTEND; + else + currentDir = RUN_RETRACT; + } + if (motorError != MOTOR_ERROR_NONE) + { + motorState = MOTOR_STOP; + } + + if (Timers[MOTOR_TIMER] > parm[31]) + { + motorError = MOTOR_ERROR_TIMEOUT; + mStatus[0].bits.position = 9999; + mStatus[1].bits.position = 9999; + Serial.println("Run Timeout"); + } + break; + + case MOTOR_STOP: + runTime = (millis() - startTime); + printEndOfRun(); + mCtl[MASTER].bits.move = 0; + mCtl[SLAVE].bits.move = 0; + motor_start.master = 0; + motor_start.slave = 0; + motorState = MOTOR_IDLE; + break; + + default: + Serial.print("Bad State "); + Serial.println(motorState); + sprintf(printBuff, "Unknown State %d", motorState); + printPrint(); + cycleCount = 0; + motorState = MOTOR_STOP; + break; + } +} + +// *************** Start/Stop *********************** +void motorMove(uint16_t position) +{ + mCtl[MASTER].bits.position = position; + mCtl[SLAVE].bits.position = position; + motor_start.master = 1; + motor_start.slave = 1; + if (position > motorGetPosition(MASTER)) + motorDir = EXTEND; + else + motorDir = RETRACT; +} + +void motorStart(uint8_t motor) +{ + if (motorState == MOTOR_IDLE) + { + motor_start.master = (motor & 1); + motor_start.slave = (motor & 2) >> 1; + motorState = MOTOR_START; + } +} + +void motorStop() +{ + if (motorState != MOTOR_IDLE) + { + motor_start.master = 0; + motor_start.slave = 0; + motorState = MOTOR_STOP; + } +} + +// ************* Speed ************************** +void motorSetSpeed(uint8_t motor, uint8_t duty) +{ + uint16_t temp; + char strTemp[30]; + temp = duty; + temp *= 32; + temp /= 100; + if (temp > 31) + temp = 31; + mCtl[motor].bits.speed = temp; +} + +uint8_t motorGetSetSpeed(uint8_t motor) +{ + uint16_t duty; + duty = mCtl[motor].bits.speed; + duty = (duty * 100) / 32; + return ((uint8_t)duty); +} +uint8_t motorGetActualSpeed(uint8_t motor) +{ + uint16_t duty; + duty = mStatus[motor].bits.speed; + duty = (duty * 100) / 32; + return ((uint8_t)duty); +} + +//************ Position ****************************** +void motorSetPosition(uint8_t motor, uint16_t position) +{ + mCtl[motor].bits.position = (position & 0x3FFF); +} +uint16_t motorGetPosition(uint8_t motor) +{ + return (mStatus[motor].bits.position); +} +//*********** Current ******************************** +void motorSetCurrentLimit(uint8_t motor, uint16_t limit) +{ + mCtl[motor].bits.current_limit = (limit & 0x1FF); +} +uint16_t motorGetCurrent(uint8_t motor) +{ + return (mStatus[motor].bits.current); +} +//************* Flags ********************************* +uint8_t motorGetFlags(uint8_t motor, uint8_t flag) +{ + uint8_t data = 0; + switch (flag) + { + case MOTOR_FLAG_VOLTAGE_ERROR: + data = mStatus[motor].bits.ve; + break; + case MOTOR_FLAG_TEMP_ERROR: + data = mStatus[motor].bits.te; + break; + case MOTOR_FLAG_MOTION: + data = mStatus[motor].bits.motion; + break; + case MOTOR_FLAG_OVERLOAD: + data = mStatus[motor].bits.overload; + break; + case MOTOR_FLAG_BACKDRIVE: + data = mStatus[motor].bits.backdrive; + break; + case MOTOR_FLAG_PARAMETER: + data = mStatus[motor].bits.parameter; + break; + case MOTOR_FLAG_SATURATION: + data = mStatus[motor].bits.saturation; + break; + case MOTOR_FLAG_FATAL_ERROR: + data = mStatus[motor].bits.fatalerror; + break; + } + return (data); +} + +/************************************************************************* + Synchronize motors + M1 will always run at set speed, M2 will be adjusted to match + M1 encoder counter + + M2Speed = (delta * P/100) + (I/100 + IG) + M1Speed(masterSpeed) + +**************************************************************************/ +void motorSync(void) +{ + + float delta; + float newSpeed; + static byte syncCounter = 0; + uint16_t slavePosition; // V504 + + char sString[30]; + + if (++syncCounter < parm[P_MOTOR_ADJ_INTERVAL]) + return; + + syncCounter = 0; + // slavePosition = mStatus[SLAVE].bits.position + parm[P_SLAVE_OFFSET]; //***** V504 + + slavePosition = mStatus[SLAVE].bits.position + parm[P_SLAVE_OFFSET] + slaveCurrentBalance; //***** V983 DES + + if (motorDir == EXTEND) + delta = mStatus[MASTER].bits.position - slavePosition; // V504 + else + delta = ((3000 - mStatus[MASTER].bits.position) - (3000 - slavePosition)); // V504 + + if (delta == 0) + return; + + // if the motors get too far apart, shutdown + if (abs(delta) > parm[P_MAX_DELTA]) + { + // shut down + motorError = MOTOR_ERROR_MAX_DELTA; + /* + Serial.printf("DELTA Error %d %d\n",mStatus[MASTER].bits.position,slavePosition); + Serial.printf("Parm 44 = %d, Delta = %d\n",parm[44],delta); + Serial.printf("DELTA Error %d %d\n",3000 - mStatus[MASTER].bits.position,3000 - slavePosition); + while(1); + */ + return; + } + M1Speed = masterSpeed; + + if (delta > 0) + if (delta > maxDelta) + maxDelta = delta; + + if (delta < 0) + if (delta < minDelta) + minDelta = (int16_t)delta; + + if (vbSync) + { + dtostrf(delta, 4, 0, sString); + sprintf(sString, "%s,%u,%u,", sString, motorGetPosition(MASTER), motorGetPosition(SLAVE)); + strcpy(printBuff, sString); + } + + if (delta < 0) // slave is ahead of master + motorIntegral -= (float)parm[P_MOTOR_ADJ_I] / 100.0; + else + motorIntegral += (float)parm[P_MOTOR_ADJ_I] / 100.0; + if (vbSync) + { + dtostrf(motorIntegral, 6, 2, sString); + sprintf(sString, "%s,", sString); + strcat(printBuff, sString); + } + newSpeed = (delta * (float)parm[P_MOTOR_ADJ_P]) / 100.0; + + if (vbSync) + { + dtostrf(newSpeed, 6, 2, sString); + sprintf(sString, "%s,", sString); + strcat(printBuff, sString); + } + + newSpeed += motorIntegral; + + if (vbSync) + { + dtostrf(newSpeed, 6, 2, sString); + sprintf(sString, "%s,", sString); + strcat(printBuff, sString); + // Serial.print(newSpeed,3); + // Serial.print(","); + } + + newSpeed += M1Speed; + + if (vbSync) + { + dtostrf(newSpeed, 6, 2, sString); + sprintf(sString, "%s,", sString); + strcat(printBuff, sString); + } + + if (newSpeed >= 20 && newSpeed <= 100) + { + M2Speed = (byte)newSpeed; + } + else if (newSpeed > 100) + { + M2Speed = 100; + } + else + { + M2Speed = 20; + } + if (M2Speed > maxSpeed) + maxSpeed = M2Speed; + + if (bSyncEnable) + motorSetSpeed(SLAVE, M2Speed); + + if (vbSync) + { + sprintf(sString, "%u,%04u,%04u,%04u,%04u", + M2Speed, motorGetSetSpeed(MASTER), motorGetSetSpeed(SLAVE), + motorGetCurrent(MASTER), motorGetCurrent(SLAVE)); + strcat(printBuff, sString); + printPrint(); + } + + if (motorGetCurrent(MASTER) > maxCurrentM) + maxCurrentM = motorGetCurrent(MASTER); + if (motorGetCurrent(SLAVE) > maxCurrentS) + maxCurrentS = motorGetCurrent(SLAVE); +} + +void motorJog(uint8_t motor, uint8_t state, uint16_t position) +{ + mCtl[motor].bits.position = position; + mCtl[motor].bits.move = ~state; + + if (state == 1) // button released + { + if (vb) + printPosition(); + } +} + +uint16_t motorErrorFlags(byte motor) +{ + uint16_t flags; + + flags = mStatus[motor].v[4] >> 4; + flags += (mStatus[motor].v[5] << 4); + return flags; +} + +uint16_t getMotorError(void) +{ + return (motorError); +} + +void clearMotorError(void) +{ + motorError = 0; + mStatus[0].bits.position = 0; + mStatus[1].bits.position = 0; +} + +// SetNAME is not needed + +// void SetNAME(long v81, int nManufacturerCode, byte nFunctionInstance, byte nECUInstance, +// byte nFunction, byte nVehicleSystem, byte nVehicleSystemInstance, byte nIndustryGroup, byte nArbitraryAddressCapable) +// { +// byte i; +// Serial.print("0X"); +// for(i = 0; i < 8; i++) +// Serial.print(ecuNAME[7-i],HEX); + +// Serial.println("\r\n"); + +// ecuNAME[0] = (byte)(v81 & 0xFF); +// ecuNAME[1] = (byte)((v81 >> 8) & 0xFF); +// ecuNAME[2] = (byte)(((nManufacturerCode << 5) & 0xFF) | (v81 >> 16)); +// ecuNAME[3] = (byte)(nManufacturerCode >> 3); +// ecuNAME[4] = (byte)((nFunctionInstance << 3) | nECUInstance); +// ecuNAME[5] = (byte)(nFunction); +// ecuNAME[6] = (byte)(nVehicleSystem << 1); +// ecuNAME[7] = (byte)((nArbitraryAddressCapable << 7) | (nIndustryGroup << 4) | (nVehicleSystemInstance)); +// Serial.print("0X"); +// for(i = 0; i < 8; i++) +// Serial.print(ecuNAME[7-i],HEX); + +// Serial.println("\r\n"); +// }