commit f28f5ac12cf5c6006097ad05b607ab02f7d45a92 Author: Cole Deck Date: Fri Sep 1 17:26:52 2023 -0500 initial commit diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/.vscode/extensions.json b/.vscode/extensions.json new file mode 100644 index 0000000..080e70d --- /dev/null +++ b/.vscode/extensions.json @@ -0,0 +1,10 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ], + "unwantedRecommendations": [ + "ms-vscode.cpptools-extension-pack" + ] +} diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..78c0006 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,9 @@ +{ + "C_Cpp.errorSquiggles": "disabled", + "files.associations": { + "cmath": "cpp", + "*.tcc": "cpp", + "string": "cpp", + "ranges": "cpp" + } +} \ No newline at end of file diff --git a/include/README b/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/lib/README b/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/platformio.ini b/platformio.ini new file mode 100644 index 0000000..e29f1f5 --- /dev/null +++ b/platformio.ini @@ -0,0 +1,23 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:pico] +platform = https://github.com/maxgerhardt/platform-raspberrypi.git +board = rpipicow +framework = arduino +platform_packages = + framework-arduinopico@https://github.com/earlephilhower/arduino-pico.git#master +upload_port = /run/media/amelia/RPI-RP2/ +board_build.core = earlephilhower +lib_deps = + xreef/PCF8574 library@^2.3.6 + gbr1/rp2040-encoder-library@^0.1.1 + trevorwslee/DumbDisplay Arduino Library@^0.9.841 +debug_tool = cmsis-dap diff --git a/src/MCP3XXX.h b/src/MCP3XXX.h new file mode 100644 index 0000000..eb14477 --- /dev/null +++ b/src/MCP3XXX.h @@ -0,0 +1,321 @@ +// +// Copyright (c) 2018 Christopher Baker +// +// SPDX-License-Identifier: MIT +// +// Modified by Amelia Deck to use SPI1 + at a low frequency for optocouplers + + +#pragma once + + +#include +#include + + +/// \brief A template class supporting MCP3XXX ADC SPI chips. +/// +/// \tparam NumBits Number of ADC bits. +/// \tparam NumChannels Number of input channels. +/// \tparam MaxSPIClockSpeed Maximum SPI communication speed rate in Hz. +/// \tparam SPITransferLength The number of bytes transferred over SPI. +template +class MCP3XXX_ +{ +public: + + enum + { + /// \brief ADC error value. + ADC_ERROR_INVALID_CHANNEL = -1, + + /// \brief ADC error value. + ADC_UNSUPPORTED_CONFIGURATION = -2, + + /// \brief Number of ADC bits. + NUM_BITS = NumBits, + + /// \brief A bit mask based on the number of bits. + BIT_MASK = (1 << NUM_BITS) - 1, + + /// \brief Number of input channels. + NUM_CHANNELS = NumChannels, + + /// \brief Maximum SPI communication speed rate in Hz. + MAX_SPI_CLOCK_SPEED = MaxSPIClockSpeed, + + /// \brief The number of bytes transferred over SPI. + SPI_TRANSFER_LEGNTH = SPITransferLength + }; + + + /// \brief Construct a default MCP3XXX_ device. + MCP3XXX_() + { + } + + /// \brief Destroy the MCP3XXX_ device. + ~MCP3XXX_() + { + } + + /// \brief Set up the ADC using default hardware SPI pins. + /// + /// Hardware SPI pins vary based on the board being used. These default pins + /// are represented by the constants SS, MOSI, MISO and SCK. + /// + /// \sa https://www.arduino.cc/en/Reference/SPI + /// \param csPin Chip Select Pin. Default value is SS. + void begin(uint8_t csPin = SS) + { + _useHardwareSPI = true; + + _csPin = csPin; + _mosiPin = MOSI; + _misoPin = MISO; + _sckPin = SCK; + + // Set up pin modes. + pinMode(_csPin, OUTPUT); + + // Begin software SPI. + // Initializes the SPI bus by setting SCK, MOSI, and SS to outputs, + // pulling SCK and MOSI low, and SS high. + digitalWrite(_csPin, HIGH); // Redundant. + SPI1.begin(); + } + + /// \brief Set up the ADC using custom software SPI pins. + /// + /// This method forces the SPI to be accesed via software methods rather + /// than hardware SPI. This is true, even if the default hardware SPI pins + /// are used. + /// + /// \param csPin Chip Select Pin. + /// \param mosiPin MOSI pin. + /// \param misoPin MISO pin. + /// \param sckPin Clock pin. + void begin(uint8_t csPin, uint8_t mosiPin, uint8_t misoPin, uint8_t sckPin) + { + _useHardwareSPI = false; + + _csPin = csPin; + _mosiPin = mosiPin; + _misoPin = misoPin; + _sckPin = sckPin; + + // Set up pin modes manually. + pinMode(_csPin, OUTPUT); + pinMode(_mosiPin, OUTPUT); + pinMode(_misoPin, INPUT); + pinMode(_sckPin, OUTPUT); + + // Begin software SPI. We initiate CS Pin HIGH to prepare it to go LOW + // on our first read. + digitalWrite(_csPin, HIGH); + } + + /// \brief Read the analog value. + /// + /// Reads a single-ended analog value using the given channel. + /// + /// \param channel The channel (channel < NUM_CHANNELS) to read. + /// \returns values [0, MAX_VALUE) on success or an error code on failure. + uint32_t analogRead(uint8_t channel) const + { + if (channel < NUM_CHANNELS) + return _read(channel, false); + return ADC_ERROR_INVALID_CHANNEL; + } + + /// \brief Read a differential analog value by specifying the IN+ channel. + /// + /// Consecutive channel pairs can be differentially read. For instance, if + /// inPositiveChannel == 0, inNegativeChannel will be 1. + /// If inPositiveChannel == 1, then inNegativeChannel will be 0. Thus if + /// inPositiveChannel is odd, inNegativeChannel == (inPositiveChannel - 1). + /// if inPositiveChannel is even, inNegativeChannel == (inPositiveChannel + 1). + /// + /// \param inPositiveChannel The channel that should be input positive. + /// \returns Differential values. See the data sheet for information on how + /// to interpret these return values. + uint32_t analogReadDifferential(uint8_t inPositiveChannel) const + { + if (inPositiveChannel < NUM_CHANNELS) + return _read(inPositiveChannel, true); + return ADC_ERROR_INVALID_CHANNEL; + } + + /// \returns the number of ADC channels. + size_t numChannels() const + { + return NUM_CHANNELS; + } + + /// \returns the number of ADC bits. + size_t numBits() const + { + return NUM_BITS; + } + +private: + MCP3XXX_(const MCP3XXX_&); + MCP3XXX_& operator = (const MCP3XXX_&); + + /// \brief Read the value from the given channel using the given mode. + /// \param channel The channel to read. + /// \param differential If true, use differential read mode. + uint32_t _read(uint8_t channel, bool differential) const + { + // Data transfers are done using "8-bit segments" approach in data sheet. + // The sent data alignment resuls in correctly aligned return bytes after + // the SPI transfer. + uint8_t data[SPI_TRANSFER_LEGNTH]; + + // Check for MCP3004 + if (NUM_CHANNELS == 2) + { + if (NUM_BITS == 10) + { + // Start bit. + data[0] = 0b01000000; + // Differential bit. + data[0] |= (differential ? 0b00000000 : 0b00100000); + // Channel bit. + data[0] |= (channel == 0 ? 0b00000000 : 0b00010000); + // MSBF bit is set to 1. See section 5.1 of the data sheet. + data[0] |= 0b00001000; + // It doesn't matter what data[1] is set to. + } + else + { + return ADC_UNSUPPORTED_CONFIGURATION; + } + } + else + { + if (NUM_BITS == 10) + { + // The start bit. We position it here to align our output data. + data[0] = 0b00000001; + // Set the differential / single bit and the channel bits. + data[1] = (differential ? 0b00000000 : 0b10000000) | (channel << 4); + // It doesn't matter what data[2] is set to. + } + else + { + return ADC_UNSUPPORTED_CONFIGURATION; + } + } + + if (_useHardwareSPI) + { + // Here we replace the sent data with the received data. + SPI1.beginTransaction(SPISettings(MAX_SPI_CLOCK_SPEED, MSBFIRST, SPI_MODE0)); + digitalWrite(_csPin, LOW); + //unsigned long t1 = micros(); + for (size_t i = 0; i < SPI_TRANSFER_LEGNTH; ++i) + { + data[i] = SPI1.transfer(data[i]); + } + //unsigned long t2 = micros(); + digitalWrite(_csPin, HIGH); + SPI1.endTransaction(); + //Serial.print("Data collection time: "); + //Serial.print(t2-t1); + //Serial.println(" us"); + } + else + { + // Slower, but can use any pin. + // We could save a few operations by skipping some digitalWrites(), + // using bitwise operators and doing direct port-manipulation. + // But this is used because it is "easier" to read. + digitalWrite(_csPin, LOW); + for (size_t i = 0; i < SPI_TRANSFER_LEGNTH; ++i) + { + for (size_t j = 8; j-- > 0;) + { + // Set MOSI data. + digitalWrite(_mosiPin, bitRead(data[i], j)); + // Set Clock HIGH. + digitalWrite(_sckPin, HIGH); + // Read MISO data. + bitWrite(data[i], j, digitalRead(_misoPin)); + // Set Clock LOW. + digitalWrite(_sckPin, LOW); + } + } + digitalWrite(_csPin, HIGH); + } + + // Here we take the second two bytes returned as our value. + // This value is already correctly aligned since we are using the 8-bit + // segments approach. The BIT_MASK is calculated based on the number out + // bits specified in the template parameters. + return ((data[SPI_TRANSFER_LEGNTH - 2] << 8) | data[SPI_TRANSFER_LEGNTH - 1]) & BIT_MASK; + } + + + /// \brief Use hardware SPI to communicate. + bool _useHardwareSPI = true; + + /// \brief Chip Select pin. + uint8_t _csPin = SS; + + /// \brief MOSI pin. + uint8_t _mosiPin = MOSI; + + /// \brief MISO pin. + uint8_t _misoPin = MISO; + + /// \brief SCLK pin. + uint8_t _sckPin = SCK; + +}; + +/// \brief A typedef for the MCP3002. +/// Max clock frequency for 2.7V: 1200000 Hz +/// Max clock frequency for 5.0V: 3200000 Hz +/// \sa http://ww1.microchip.com/downloads/en/DeviceDoc/21294E.pdf +typedef MCP3XXX_<10, 2, 10000, 2> MCP3002; + +/// \brief A typedef for the MCP3004. +/// Max clock frequency for 2.7V: 1350000 Hz +/// Max clock frequency for 5.0V: 3600000 Hz +/// \sa http://ww1.microchip.com/downloads/en/DeviceDoc/21295C.pdf +typedef MCP3XXX_<10, 4, 10000> MCP3004; + +/// \brief A typedef for the MCP3008. +/// Max clock frequency for 2.7V: 1350000 Hz +/// Max clock frequency for 5.0V: 3600000 Hz +/// \sa http://ww1.microchip.com/downloads/en/DeviceDoc/21295C.pdf +//typedef MCP3XXX_<10, 8, 1350000> MCP3008; +typedef MCP3XXX_<10, 8, 5000> MCP3008; + +// /// \brief A typedef for the MCP3202. +// /// Max clock frequency for 2.7V: 900000 Hz +// /// Max clock frequency for 5.0V: 1800000 Hz +// /// \sa http://ww1.microchip.com/downloads/en/DeviceDoc/21034D.pdf +// typedef MCP3XXX_<12, 2, 900000> MCP3202; +// +// /// \brief A typedef for the MCP3204. +// /// Max clock frequency for 2.7V: 1000000 Hz +// /// Max clock frequency for 5.0V: 2000000 Hz +// /// \sa http://ww1.microchip.com/downloads/en/DeviceDoc/21298c.pdf +// typedef MCP3XXX_<12, 4, 1000000> MCP3204; +// +// /// \brief A typedef for the MCP3208. +// /// Max clock frequency for 2.7V: 1000000 Hz +// /// Max clock frequency for 5.0V: 2000000 Hz +// /// \sa http://ww1.microchip.com/downloads/en/DeviceDoc/21298c.pdf +// typedef MCP3XXX_<12, 8, 1000000> MCP3208; +// +// /// \brief A typedef for the MCP3208. +// /// Max clock frequency for 2.7V: 1050000 Hz +// /// Max clock frequency for 5.0V: 2100000 Hz +// /// \sa http://ww1.microchip.com/downloads/en/DeviceDoc/21697e.pdf +// typedef MCP3XXX_<13, 8, 1050000> MCP3304; diff --git a/src/base64.c b/src/base64.c new file mode 100644 index 0000000..7a3dc9a --- /dev/null +++ b/src/base64.c @@ -0,0 +1,126 @@ +#include "base64.h" + +const char b64_alphabet[] = "ABCDEFGHIJKLMNOPQRSTUVWXYZ" +"abcdefghijklmnopqrstuvwxyz" +"0123456789+/"; + +/* 'Private' declarations */ +void a3_to_a4(unsigned char * a4, unsigned char * a3); +void a4_to_a3(unsigned char * a3, unsigned char * a4); +unsigned char b64_lookup(char c); + +int base64_encode(char *output, char *input, int inputLen) { + int i = 0, j = 0; + int encLen = 0; + unsigned char a3[3]; + unsigned char a4[4]; + + while(inputLen--) { + a3[i++] = *(input++); + if(i == 3) { + a3_to_a4(a4, a3); + + for(i = 0; i < 4; i++) { + output[encLen++] = b64_alphabet[a4[i]]; + } + + i = 0; + } + } + + if(i) { + for(j = i; j < 3; j++) { + a3[j] = '\0'; + } + + a3_to_a4(a4, a3); + + for(j = 0; j < i + 1; j++) { + output[encLen++] = b64_alphabet[a4[j]]; + } + + while((i++ < 3)) { + output[encLen++] = '='; + } + } + output[encLen] = '\0'; + return encLen; +} + +int base64_decode(char * output, char * input, int inputLen) { + int i = 0, j = 0; + int decLen = 0; + unsigned char a3[3]; + unsigned char a4[4]; + + + while (inputLen--) { + if(*input == '=') { + break; + } + + a4[i++] = *(input++); + if (i == 4) { + for (i = 0; i <4; i++) { + a4[i] = b64_lookup(a4[i]); + } + + a4_to_a3(a3,a4); + + for (i = 0; i < 3; i++) { + output[decLen++] = a3[i]; + } + i = 0; + } + } + + if (i) { + for (j = i; j < 4; j++) { + a4[j] = '\0'; + } + + for (j = 0; j <4; j++) { + a4[j] = b64_lookup(a4[j]); + } + + a4_to_a3(a3,a4); + + for (j = 0; j < i - 1; j++) { + output[decLen++] = a3[j]; + } + } + //We don't need a null byte after the struct + return decLen; +} + +int base64_dec_len(char * input, int inputLen) { + int i = 0; + int numEq = 0; + for(i = inputLen - 1; input[i] == '='; i--) { + numEq++; + } + + return ((6 * inputLen) / 8) - numEq; +} + +inline void a3_to_a4(unsigned char * a4, unsigned char * a3) { + a4[0] = (a3[0] & 0xfc) >> 2; + a4[1] = ((a3[0] & 0x03) << 4) + ((a3[1] & 0xf0) >> 4); + a4[2] = ((a3[1] & 0x0f) << 2) + ((a3[2] & 0xc0) >> 6); + a4[3] = (a3[2] & 0x3f); +} + +inline void a4_to_a3(unsigned char * a3, unsigned char * a4) { + a3[0] = (a4[0] << 2) + ((a4[1] & 0x30) >> 4); + a3[1] = ((a4[1] & 0xf) << 4) + ((a4[2] & 0x3c) >> 2); + a3[2] = ((a4[2] & 0x3) << 6) + a4[3]; +} + +inline unsigned char b64_lookup(char c) { + if(c >='A' && c <='Z') return c - 'A'; + if(c >='a' && c <='z') return c - 71; + if(c >='0' && c <='9') return c + 4; + if(c == '+') return 62; + if(c == '/') return 63; + return -1; +} diff --git a/src/base64.h b/src/base64.h new file mode 100644 index 0000000..e2ffbd5 --- /dev/null +++ b/src/base64.h @@ -0,0 +1,68 @@ +/* + * Copyright (c) 2013 Adam Rudd. + */ +#ifdef __cplusplus +extern "C"{ +#endif + +#ifndef _BASE64_H +#define B64_ENC_LEN(n) (((n) + 2 - (((n) + 2) % 3)) / 3 * 4) +#define _BASE64_H + +/* b64_alphabet: + * Description: Base64 alphabet table, a mapping between integers + * and base64 digits + * Notes: This is an extern here but is defined in Base64.c + */ +extern const char b64_alphabet[]; + +/* base64_encode: + * Description: + * Encode a string of characters as base64 + * Parameters: + * output: the output buffer for the encoding, stores the encoded string + * input: the input buffer for the encoding, stores the binary to be encoded + * inputLen: the length of the input buffer, in bytes + * Return value: + * Returns the length of the encoded string + * Requirements: + * 1. output must not be null or empty + * 2. input must not be null + * 3. inputLen must be greater than or equal to 0 + */ +int base64_encode(char *output, char *input, int inputLen); + +/* base64_decode: + * Description: + * Decode a base64 encoded string into bytes + * Parameters: + * output: the output buffer for the decoding, + * stores the decoded binary + * input: the input buffer for the decoding, + * stores the base64 string to be decoded + * inputLen: the length of the input buffer, in bytes + * Return value: + * Returns the length of the decoded string + * Requirements: + * 1. output must not be null or empty + * 2. input must not be null + * 3. inputLen must be greater than or equal to 0 + */ +int base64_decode(char *output, char *input, int inputLen); +int base64_dec_len(char * input, int inputLen); +/* base64_enc_len: + * Description: + * Returns the length of a base64 encoded string whose decoded + * form is inputLen bytes long + * Parameters: + * inputLen: the length of the decoded string + * Return value: + * The length of a base64 encoded string whose decoded form + * is inputLen bytes long + * Requirements: + * None + */ +#endif // _BASE64_H +#ifdef __cplusplus +} // extern "C" +#endif diff --git a/src/crc16.c b/src/crc16.c new file mode 100644 index 0000000..8a4b015 --- /dev/null +++ b/src/crc16.c @@ -0,0 +1,32 @@ +//#include + +/* http://www.nongnu.org/avr-libc/user-manual/group__util__crc.html + * uint8_t serno[] = { 0x02, 0x1c, 0xb8, 0x01, 0, 0, 0, 0xa2 }; + * int + * checkcrc(void) + * { + * uint8_t crc = 0, i; + * for (i = 0; i < sizeof serno / sizeof serno[0]; i++) + * crc = _crc_ibutton_update(crc, serno[i]); + * return crc; // must be 0 + * + */ +// C implemtation for computer +#define uint16_t unsigned short + +unsigned short crc16(const unsigned char* data_p, int length){ + unsigned char x; + uint16_t crc = 0xFFFF; + + while (length--){ + x = crc >> 8 ^ *data_p++; + x ^= x>>4; + crc = (crc << 8) ^ ((unsigned short)(x << 12)) ^ ((unsigned short)(x <<5)) ^ ((unsigned short)x); + } + return crc; +} + +uint16_t compute_crc(char *data, int len){ + return crc16(data, len); +} + diff --git a/src/crc16.h b/src/crc16.h new file mode 100644 index 0000000..5216a9e --- /dev/null +++ b/src/crc16.h @@ -0,0 +1,13 @@ +#pragma once + +#ifdef __cplusplus +extern "C"{ +#endif + +//Include ASM optimised version for arduino + +uint16_t compute_crc(char *data, int len); + +#ifdef __cplusplus +} // extern "C" +#endif diff --git a/src/globals.h b/src/globals.h new file mode 100644 index 0000000..2069a01 --- /dev/null +++ b/src/globals.h @@ -0,0 +1,54 @@ +#pragma once +#include "packet.h" + +extern packet_t pA, pB, safe; +extern packet_t *astate, *incoming; +extern comm_state cs; +extern char comm_ok; +extern long last_p; + + + +#define SerComm Serial1 //Serial port connected to Xbee +#define DIAMOND_LEFT 0 +#define DIAMOND_DOWN 1 +#define DIAMOND_RIGHT 2 +#define DIAMOND_UP 3 +#define SHOULDER_TOP_LEFT 4 +#define SHOULDER_TOP_RIGHT 5 +#define SHOULDER_BOTTOM_LEFT 6 +#define SHOULDER_BOTTOM_RIGHT 7 +#define SMALL_LEFT 8 +#define SMALL_RIGHT 9 +//10 and 11 are probably the stick buttons +//but we haven't checked recently +#define DPAD_UP 12 +#define DPAD_RIGHT 13 +#define DPAD_DOWN 14 +#define DPAD_LEFT 15 + +// pins for motor controller 1 (right) +#define ALI1 0 +#define AHI1 1 +#define BHI1 2 +#define BLI1 3 +#define DENABLE1 8 +//#define DREADY1 30 + +// and 2 (left) +#define ALI2 4 +#define AHI2 5 +#define BHI2 6 +#define BLI2 7 +#define DENABLE2 9 +//#define DREADY2 31 + +#define try_enable_right(e,VBATT) try_enable_osmc(e,DENABLE1,VBATT,ALI1,BLI1,AHI1,BHI1) +#define try_enable_left(e,VBATT) try_enable_osmc(e,DENABLE2,VBATT,ALI2,BLI2,AHI2,BHI2) +#define drive_right(e,x) drive_osmc(e,DENABLE1,x,0,ALI1,BLI1,AHI1,BHI1) +#define drive_left(e,x) drive_osmc(e,DENABLE2,x,0,ALI2,BLI2,AHI2,BHI2) + +#define DEADBAND_HALF_WIDTH 10 // Control input deadband radius +#define FAILTIME 200 //Failsafe timeout in milliseconds +#define DEBUGPRINT(x) SerCommDbg.println(x) +#define SerCommDbg Serial //Serial port for debugging info diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..8b5728f --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,734 @@ +#include +#include +#include +#include "globals.h" +#include +#include "MCP3XXX.h" +#include "PCF8574.h" +#include "pio_encoder.h" +#include "dumbdisplay.h" +#include "wifidumbdisplay.h" +#include "zserio.h" +#include "SerialUART.h" + +const char* ssid = "TEST"; +const char* password = "pink4bubble"; +DumbDisplay dumbdisplay(new DDWiFiServerIO(ssid, password), 8192); +LcdDDLayer *optionsdisplay = NULL; +SevenSegmentRowDDLayer *sevenSeg; +JoystickDDLayer *appjoystick; + +packet_t pA, pB, safe; +packet_t *astate, *incoming; +comm_state cs; + +// character 0 1 2 3 4 5 6 7 8 9 A b C d E F +byte d[] = { 0x7e, 0x30, 0x6d, 0x79, 0x33, 0x5b, 0x5f, 0x70, 0x7f, 0x7b, 0x77, 0x1f, 0x4e, 0x3d, 0x4f, 0x47 }; + +PCF8574 ioex1(0x20, 20, 21); +PCF8574 ioex2(0x21, 20, 21); +PioEncoder enc1(18); // right +PioEncoder enc2(14); // left + +MCP3008 adc; +int count = 0; +int mode = 1; +char left_enabled = 0, right_enabled = 0; +int current_offset[2]; +int setup_complete = false; + +// driving vars +int target_left_power = 0; +int target_right_power = 0; +int left_power = 0; +int right_power = 0; +int acceleration = 1; +bool turbo = false; +int left_cooldown = 0; +int right_cooldown = 0; +int olddisplay = 99999; + +unsigned int getButton(unsigned int num) { + if (num <= 7) { + return (astate->btnlo >> num) & 0x01; + } else if (num > 7 && num <= 15) { + return (astate->btnhi >> (num - 8)) & 0x01; + } else { + return 0; + } +} +unsigned int getDPad() { + // four bits: left down right up + return (astate->btnhi >> 4); +} + + +void FeedbackHandler(DDLayer* pLayer, DDFeedbackType type, const DDFeedback&) { + if (pLayer == optionsdisplay) { + // clicked the "clear" button + if(turbo) { + turbo = false; + pLayer->backgroundColor("lightgray"); + } + else { + turbo = true; + pLayer->backgroundColor("red"); + } + //delay(100); + } +} + +void osmc_init() { + digitalWrite(ALI1, LOW); + digitalWrite(BLI1, LOW); + digitalWrite(AHI1, LOW); + digitalWrite(BHI1, LOW); + digitalWrite(ALI2, LOW); + digitalWrite(BLI2, LOW); + digitalWrite(AHI2, LOW); + digitalWrite(BHI2, LOW); + digitalWrite(DENABLE1, HIGH); + digitalWrite(DENABLE2, HIGH); + + pinMode(ALI1, OUTPUT); + pinMode(AHI1, OUTPUT); + pinMode(BLI1, OUTPUT); + pinMode(BHI1, OUTPUT); + pinMode(ALI2, OUTPUT); + pinMode(AHI2, OUTPUT); + pinMode(BLI2, OUTPUT); + pinMode(BHI2, OUTPUT); + pinMode(DENABLE1, OUTPUT); + pinMode(DENABLE2, OUTPUT); + //pinMode(DREADY1, INPUT); + //pinMode(DREADY2, INPUT); + + digitalWrite(22, LOW); + pinMode(22, OUTPUT); + analogWriteFreq(4000); // set PWM frequency to 16kHz +} + +char try_enable_osmc(char enabled, char enablepin, float vbatt, + char ali, char bli, char ahi, char bhi) { + // check that power is present at the OSMC + if (vbatt > 13) { + if (!enabled){ + delay(10); //"Short" delay required in order to prevent blowout! 10ms is conservative. + //delay(1000); + digitalWrite(enablepin, LOW); + } + return 1; + } + else { // controller has no power; zero inputs in case we power it again + digitalWrite(enablepin, HIGH); + digitalWrite(ali, LOW); + digitalWrite(bli, LOW); + digitalWrite(ahi, LOW); + digitalWrite(bhi, LOW); + return 0; + } +} + +float get_voltage(byte battery) { + int read = adc.analogRead(battery); + //Serial.println(read); + if (read == 1023) + return 0.0; + read = 0; + for(int i = 0; i < 5; i++) { + read += adc.analogRead(battery); + delay(1); + } + return (read/5.0) * 5 * 10 / 1024.0; +} + +void set_offset() { + int n = 100; + current_offset[0] = current_offset[1] = 0; + for (int i = 0; i < n; i++) { + current_offset[0] += adc.analogRead(2); + current_offset[1] += adc.analogRead(3); + delay(10); + } + current_offset[1] /= n; + current_offset[1] -= 512; + //Serial.println(current_offset[0]); + current_offset[0] /= n; + //Serial.println(current_offset[0]); + current_offset[0] -= 512; + //Serial.println(current_offset[0]); +} +float get_current(byte sensor) { + int read = 0; + for(int i = 0; i < 20; i++) { + read += adc.analogRead(sensor + 2); + delay(1); + //Serial.println(read/i); + } + //Serial.println(read/5.0); + return (read/20.0 - 512.0 - current_offset[sensor]) / 1.28; +} +// OSMC motor controller stuff +// Low side outputs must be PWM capable and NOT 5 or 6 (on Uno) +// Do not change timer0, +// Pins 7 and 8 use timer4 in phase correct mode +// Pins 11 and 12 use timer1 in phase correct mode +// OSMC ALI and BLI are the low side driver inputs and must ALWAYS be low/zero when the ready signal is not provided +// OSMC AHI and BHI are the high side driver inputs. +/* + * ----------- Vdd + * | | + * AHI BHI + * | | + * ---M--- + * | | + * ALI BLI + * | | + * --------- GND + */ +void drive_osmc(char enabled, char enablepin, int rawpower, char brake, + char ali, char bli, char ahi, char bhi) { + int power = constrain(rawpower, -176, 176); // NOTE - with optocouplers, ~176 becomes 100% + + if (!enabled) { + digitalWrite(ali, LOW); + digitalWrite(bli, LOW); + digitalWrite(ahi, LOW); + digitalWrite(bhi, LOW); + digitalWrite(enablepin, HIGH); + return; + } + //Stop! + if (abs(power) < 5) { + digitalWrite(ali, LOW); + digitalWrite(bli, LOW); + delayMicroseconds(63); + if (brake != 0) { + digitalWrite(ahi, HIGH); + digitalWrite(bhi, HIGH); + } else { + digitalWrite(ahi, LOW); + digitalWrite(bhi, LOW); + } + return; + } + //Serial.print("Driving OSMC with power "); + //Serial.println(power); + //Forward! + if (power > 0) { + digitalWrite(bhi, LOW); + digitalWrite(ali, LOW); + delayMicroseconds(63); + digitalWrite(ahi, HIGH); + analogWrite(bli, power); + } + //Reverse! + if (power < 0) { + digitalWrite(ahi, LOW); + digitalWrite(bli, LOW); + delayMicroseconds(63); + digitalWrite(bhi, HIGH); + analogWrite(ali, abs(power)); + } +} + +void set_mosfet(bool pin, bool value) { + ioex1.digitalWrite(pin, value); // first 2 pins of ioex1 (top) are the mosfets +} + +void set_digit(byte digit, byte value) +{ + Wire.beginTransmission(0x38); + Wire.write(0x20 + digit); + Wire.write(d[value]); + Wire.endTransmission(); + //Serial.print("Set digit "); + //Serial.print(digit); + //Serial.print(" to "); + //Serial.println(value); + //delay(5000); +} + +void set_raw(byte digit, byte value) { + Wire.beginTransmission(0x38); + Wire.write(0x20 + digit); + Wire.write(value); + Wire.endTransmission(); +} +void set_blank() { + Wire.beginTransmission(0x38); + Wire.write(0x20); + Wire.write((byte)0); + Wire.write((byte)0); + Wire.endTransmission(); +} +void set_hex(byte num) { + byte digit1 = num; + digit1 = digit1 >> 4; // shift right by 4 + //while (digit1 > 15) { + // digit1 -= 16; + //} + byte digit2 = num; + while (digit2 > 15) { + digit2 -= 16; + } + set_digit(0, digit1); + set_digit(1, digit2); + if(num != olddisplay && dumbdisplay.connected() && millis() % 10 < 1) { + olddisplay = num; + sevenSeg->showHexNumber(num); + } + set_raw(4, 0x00); // clear second dot +} + +void set_dec(byte num) { + byte digit1 = num / 10; + //while (digit1 > 9) { + // digit1 -= 10; + //} + byte digit2 = num; + while (digit2 > 9) { + digit2 -= 10; + } + set_digit(0, digit1); + set_digit(1, digit2); + if(num != olddisplay && dumbdisplay.connected() && millis() % 10 < 1) { + olddisplay = num; + sevenSeg->showNumber(num); + } + set_raw(4, 0x02); // set second dot +} + + +void setup() { + WiFi.noLowPowerMode(); + rp2040.enableDoubleResetBootloader(); + + pinMode(LED_BUILTIN, OUTPUT); + pinMode(32+1, OUTPUT); + digitalWrite(32+1, LOW); // set SMPS to full power mode (pin connected thru wifi chip) + digitalWrite(LED_BUILTIN, HIGH); + Serial.begin(115200); + //Serial.println("hello!"); + delay(2000); + Serial.println("Initializing RIB subsystems.."); + + + Serial.print("Enabling LED driver.."); + Wire.setSDA(20); + Wire.setSCL(21); + Wire.begin(); + Wire.beginTransmission(0x38); + Wire.write(0x01); // register: decode mode + Wire.write(0x00); // disable decode mode for all digits + Wire.write(0x3f); // intensity max + Wire.write(0x03); // scan limit 3 + Wire.write(0x01); // normal operation + Wire.endTransmission(); + Wire.beginTransmission(0x38); + Wire.write(0x07); // display mode register + Wire.write(0x01); // display test mode + Wire.endTransmission(); + delay(100); + + Wire.beginTransmission(0x38); + Wire.write(0x07); + Wire.write(0x00); // disable display test mode + Wire.endTransmission(); + Serial.println(" done"); + + + Serial.print("Initializing ADC.."); + set_hex(0x1); + SPI1.setRX(12); + SPI1.setCS(13); + SPI1.setTX(11); + SPI1.setSCK(10); + adc.begin(13); + //pinMode(13, OUTPUT); + //pinMode(11, OUTPUT); + //pinMode(10, OUTPUT); + //digitalWrite(13, HIGH); + //digitalWrite(11, HIGH); + //digitalWrite(10, HIGH); + //adc.begin(13,11,12,10); + Serial.println(" done"); + delay(20); + + Serial.print("Initializing OSMCs.."); + set_hex(0x2); + osmc_init(); + Serial.println(" done"); + delay(20); + //delay(2000); + + + bool ioex1p, ioex2p = false; + Serial.print("Initializing I/O expanders.."); + set_hex(0x3); + if(ioex1.begin()) { + delay(200); + Serial.print(" 1"); + ioex1p = true; + } else { + delay(20); + } + set_hex(0x4); + if(ioex2.begin()) { + delay(20); + Serial.print(" 2"); + ioex2p = true; + } else { + delay(20); + } + + Serial.println(" done"); + delay(20); + + Serial.print("Initializing encoders.."); + set_hex(0x5); + enc1.begin(); + enc2.begin(); + Serial.println(" done"); + delay(20); + + Serial.print("Initializing xBee radio.."); + set_hex(0x6); + SerComm.setRX(17); + SerComm.setTX(16); + SerComm.begin(57600); + comm_init(); //Initialize the communication FSM + safe.stickX = 127; + safe.stickY = 127; + safe.btnhi = 0; + safe.btnlo = 0; + safe.cksum = 0b1000000010001011; + + Serial.println(" done"); + delay(20); + + + + Serial.println("Initialization complete."); + Serial.println("Running self-tests.."); + byte pass = 0; + set_hex(0x7); + Serial.print("Checking LED driver.."); + Wire.beginTransmission(0x38); + if(Wire.endTransmission() != 0) { + Serial.println(" WARNING: LED driver not detected"); + set_hex(0xF7); + delay(500); + } else { + Serial.println(" done"); + delay(20); + pass++; + } + + // TODO + + set_hex(0x8); + Serial.print("Checking ADC.."); + byte startpass = pass; + for (size_t i = 0; i < adc.numChannels(); ++i) + { + if (adc.analogRead(i) != 0 && adc.analogRead(i) != 1023) { + pass = startpass+1; // check that at least one reading is successful to confirm MCP3008 is responding + } + } + if (pass == startpass+1) { + Serial.println(" done"); + delay(20); + } else { + Serial.println(" WARNING: ADC not detected"); + set_hex(0xF8); + delay(500); + } + Serial.print("Calibrating current sensors.."); + set_offset(); + Serial.println("done"); + + Serial.print("Checking OSMC 1.."); + set_hex(0x9); + if (get_voltage(0) > 13) { + Serial.println(" done"); + delay(20); + pass++; + } else { + Serial.println(" WARNING: OSMC 1 battery too low or OSMC not present"); + set_hex(0xF9); + delay(500); + } + set_hex(0xA); + Serial.print("Checking OSMC 2.."); + if (get_voltage(1) > 13) { + Serial.println(" done"); + delay(20); + pass++; + } else { + Serial.println(" WARNING: OSMC 2 battery too low or OSMC not present"); + set_hex(0xFA); + delay(500); + } + + set_hex(0xB); + Serial.print("Checking I/O expander 1.."); + for (int i = 0; i < 8; i++) { + ioex1.pinMode(i,OUTPUT, LOW); + } + if(ioex1p == false) { + Serial.println(" WARNING: I/O expander not detected"); + set_hex(0xFB); + delay(500); + } else { + Serial.println(" done"); + delay(20); + pass++; + } + set_hex(0xC); + Serial.print("Checking I/O expander 2.."); + for (int i = 0; i < 8; i++) { + ioex2.pinMode(i,OUTPUT, LOW); + } + if(!ioex2p == false) { + Serial.println(" WARNING: I/O expander not detected"); + set_hex(0xFC); + delay(500); + } else { + Serial.println(" done"); + delay(20); + pass++; + } + + Serial.print("Self tests complete: "); + Serial.print(pass); + Serial.println("/6 tests passed."); + Serial.println("RIB is ready to go. Starting program."); + set_blank(); + /*dumbdisplay.recordLayerSetupCommands(); + sevenSeg = dumbdisplay.create7SegmentRowLayer(2); // 2 digits + appjoystick = dumbdisplay.createJoystickLayer(255, "lr+tb", 1); // max, directions, scale in UI + appjoystick->autoRecenter(true); + appjoystick->moveToCenter(); + optionsdisplay = dumbdisplay.createLcdLayer(5, 1); + optionsdisplay->setFeedbackHandler(FeedbackHandler); + optionsdisplay->backgroundColor("lightgray"); + optionsdisplay->print("TURBO"); + dumbdisplay.configAutoPin(DD_AP_HORI_2( + appjoystick->getLayerId(), + DD_AP_VERT_2( + sevenSeg->getLayerId(), + optionsdisplay->getLayerId()))); + dumbdisplay.playbackLayerSetupCommands("basic");*/ + //dumbdisplay.configAutoPin(DD_AP_VERT); // auto vertical pin layout + setup_complete = true; + rp2040.wdt_begin(500); // start watchdog with 500ms limit. Safety feature; reset during crash to disable motors! +} + +void setup1() { + while(!setup_complete) + delay(100); +} +void print_status() { + Serial.print(get_voltage(0)); + Serial.print("V "); + Serial.print(get_current(0)); + Serial.print("A ENC1: "); + Serial.print(enc1.getCount()); + Serial.print(" ENC2: "); + Serial.println(enc2.getCount()); + + SerComm.print(get_voltage(0)); + SerComm.print("V "); + SerComm.print(get_current(0)); + SerComm.print("A ENC1: "); + SerComm.print(enc1.getCount()); + SerComm.print(" ENC2: "); + SerComm.println(enc2.getCount()); + + +} + + +void loop() { + rp2040.wdt_reset(); + comm_parse(); + if (getButton(SHOULDER_TOP_RIGHT)) + turbo = true; + else + turbo = false; + //const DDFeedback* fb; + /*if (!dumbdisplay.connected() || !WiFi.isConnected()) { + target_left_power = 0; + target_right_power = 0; + Serial.print("Connection lost"); + + } else + fb = appjoystick->getFeedback();*/ + + int zeroed_power = -1 * ((int)(astate->stickX) - 127); + int zeroed_turn = ((int)(astate->stickY) - 127); + + + if (true) { //fb != NULL) { + //int x = fb->x - 127; + //int y = - fb->y + 127; + int x = zeroed_turn; + int y = zeroed_power; + //Serial.print(x); + //Serial.print(" "); + //Serial.println(y); + + double rawdriveangle = atan2(x, y); + double driveangle = rawdriveangle * 180 / 3.1415926; + target_left_power = y; + target_right_power = y; + + target_left_power += x; + target_right_power += -x; + target_left_power = constrain(target_left_power, -127, 127); + target_right_power = constrain(target_right_power, -127, 127); + if(turbo) { + target_left_power *= 2; + target_right_power *= 2; + } + target_left_power = target_left_power * 0.675; + target_right_power = target_right_power * 0.675; + + + + + } + + if(turbo) + acceleration = 8; + else + acceleration = 3; + + if(left_cooldown > 0) + left_cooldown --; + + if(abs(target_left_power) <= 4 && abs(left_power) > 5) { + left_power = 0; + left_cooldown = 2; + } + else if(target_left_power >= left_power + acceleration && left_cooldown == 0) + left_power += acceleration; + else if(acceleration > target_left_power - left_power && left_cooldown == 0) + left_power = target_left_power; + else if(target_left_power <= left_power - acceleration && left_cooldown == 0) + left_power -= acceleration; + else if(acceleration > left_power - target_left_power && left_cooldown == 0) + left_power = target_left_power; + + if(right_cooldown > 0) + right_cooldown --; + + if(abs(target_right_power) <= 4 && abs(right_power) > 5) { + right_power = 0; + right_cooldown = 2; + } + else if(target_right_power >= right_power + acceleration && right_cooldown == 0) + right_power += acceleration; + else if(acceleration > target_right_power - right_power && right_cooldown == 0) + right_power = target_right_power; + else if(target_right_power <= right_power - acceleration && right_cooldown == 0) + right_power -= acceleration; + else if(acceleration > right_power - target_right_power && right_cooldown == 0) + right_power = target_right_power; + + int avg_speed = (abs(right_power) + abs(left_power))/2; + //SerComm.println(); + set_hex(avg_speed); + + drive_right(right_enabled, right_power); + drive_left(left_enabled, -left_power); + SerComm.println(" L: " + String(left_power) + " LT: " + String(target_left_power) + " R: " + String(right_power) + " RT: " + String(target_right_power) + " MEM FREE: "+ String(rp2040.getFreeHeap())); + + //if(left_power != target_left_power || right_power != target_right_power) + + + + //delay(1000); + //set_digit(0, 6); + //set_digit(0, 10); + //set_digit(1, 9); + //set_digit(1, 10); + //set_digit(2, 8); + //set_digit(2, 10); + //set_digit(3, 8); + //set_digit(3, 10); + //set_digit(4, 8); + //set_digit(4, 10); + /*if (mode == 0) { + set_raw(count / 8, count % 8); + if (count < 39) { + count ++; + } else { + count = 0; + mode = 1; + delay(100); + } + }*/ + //print_status(); + //drive_right(right_enabled, 10); + //drive_left(left_enabled, 10); + /*if (millis() % 3000 > 1500) { + set_mosfet(0, LOW); + set_mosfet(1, LOW); + //ioex2.digitalWrite(7, LOW); + } + if (millis() % 3000 < 1500) { + set_mosfet(0, HIGH); + set_mosfet(1, HIGH); + //ioex2.digitalWrite(7, HIGH); + + }*/ + /*if (mode == 1) { + set_dec(count); + drive_right(right_enabled, count); + //set_hex(count); + if (count < 40) { + count += 5; + } else { + //count = 0; + + mode = 2; + } + } + if (mode == 2) { + set_dec(count); + drive_right(right_enabled, count); + //set_hex(count); + if (count > 5) { + count -= 5; + } else { + //count = 0; + + mode = 1; + } + }*/ + //delay(200); + delay(50); + //DDYield(); + +} +int loopcount = 0; +void loop1() { + rp2040.wdt_reset(); + //digitalWrite(LED_BUILTIN, HIGH); + if(loopcount == 20) { + //print_status(); + loopcount = 0; + delay(25); + } + else { + delay(25); + loopcount++; + } + //SerComm.println("update"); + left_enabled = try_enable_left(left_enabled, get_voltage(1)); + right_enabled = try_enable_right(right_enabled, get_voltage(0)); + //digitalWrite(LED_BUILTIN, LOW); + delay(25); +} \ No newline at end of file diff --git a/src/packet.h b/src/packet.h new file mode 100644 index 0000000..0932e26 --- /dev/null +++ b/src/packet.h @@ -0,0 +1,18 @@ +#pragma once + +#include +#define SFRAME 0x5b +#define EFRAME 0x5d +enum comm_state { + COMM_WAIT, + COMM_RECV, + COMM_COMPLETE, + COMM_VALID, + COMM_INVALID }; +typedef struct{ +uint8_t stickX; +uint8_t stickY; +uint8_t btnhi; +uint8_t btnlo; +uint16_t cksum; +} packet_t; diff --git a/src/ribtest.code-workspace b/src/ribtest.code-workspace new file mode 100644 index 0000000..36da48d --- /dev/null +++ b/src/ribtest.code-workspace @@ -0,0 +1,12 @@ +{ + "folders": [ + { + "path": "../.." + }, + { + "name": "ribtest", + "path": ".." + } + ], + "settings": {} +} \ No newline at end of file diff --git a/src/zserio.cpp b/src/zserio.cpp new file mode 100644 index 0000000..216c971 --- /dev/null +++ b/src/zserio.cpp @@ -0,0 +1,91 @@ +#include +#include "base64.h" +#include "crc16.h" +#include "globals.h" +#include "zserio.h" + + +char comm_ok; +long ptime; +void comm_init() { + ptime = 0; + cs = COMM_WAIT; + astate = &pA; + incoming = &pB; + comm_ok=0; +} +void comm_parse() { + packet_t *tmp; + static char encstr[2 + B64_ENC_LEN(sizeof(packet_t))]; + static unsigned int recvcount=0; + char inc; + uint16_t crc; + while (SerComm.available()) { + inc = SerComm.read(); + // SerCommDbg.print(cs,DEC); + if (inc == SFRAME) { + // SerCommDbg.println("Sframe"); + cs = COMM_RECV; + recvcount = 0; + // SerCommDbg.print(cs,DEC); + } else if (inc == EFRAME && cs == COMM_RECV) { + cs = COMM_COMPLETE; + //SerCommDbg.println("Eframe"); + //length check + if(recvcount != B64_ENC_LEN(sizeof(packet_t))){ + //SerCommDbg.println("Length"); + cs = COMM_INVALID; + } + + //Check decoded size in case of base64 error + if(base64_dec_len(encstr, B64_ENC_LEN(sizeof(packet_t))) != sizeof(packet_t)){ + //SerCommDbg.println("B64"); + cs = COMM_INVALID; + } + + } else if (cs == COMM_RECV) { + //populate buffer, preventing overflows from dropped start or end bytes + if (recvcount >= B64_ENC_LEN(sizeof(packet_t))) { + //SerCommDbg.println("Overflow"); + cs = COMM_INVALID; + } else { + encstr[recvcount] = inc; + recvcount++; + } + } + + if(cs==COMM_COMPLETE){ + //SerCommDbg.println("Comm Complete"); + //Base64 decode + base64_decode((char *)incoming, encstr, B64_ENC_LEN(sizeof(packet_t))); + //Evaluate CRC16 and flip pointers if valid + crc = compute_crc((char *)incoming, sizeof(packet_t)-sizeof(uint16_t)); + if(crc == ntohs(incoming->cksum)){ +// SerCommDbg.println("vaild"); + cs=COMM_VALID; + ptime=millis(); + tmp=astate; + astate=incoming; + incoming=tmp; + comm_ok=1; + //digitalWrite(13,HIGH); + } else{ + cs=COMM_VALID; + ptime=millis(); + tmp=astate; + astate=incoming; + incoming=tmp; + comm_ok=1; + cs=COMM_INVALID; + SerCommDbg.println("Invalid RC packet received!"); + } + } + } + + if(millis()-ptime > FAILTIME){ + digitalWrite(13,LOW); + //Been too long, copy safe state over active one + memcpy(astate,&safe,sizeof(packet_t)); + comm_ok=0; + } +} diff --git a/src/zserio.h b/src/zserio.h new file mode 100644 index 0000000..449659e --- /dev/null +++ b/src/zserio.h @@ -0,0 +1,9 @@ +#pragma once + +//#define htons(x) ( ((x)<<8) | (((x)>>8)&0xFF) ) +//#define ntohs(x) htons(x) +//#define htonl(x) ( ((x)<<24 & 0xFF000000UL) | ((x)<< 8 & 0x00FF0000UL) | ((x)>> 8 & 0x0000FF00UL) | ((x)>>24 & 0x000000FFUL) ) +//#define ntohl(x) htonl(x) + +void comm_init(); +void comm_parse(); diff --git a/test/README b/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Test Runner and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html