adeck/rib-test-code
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

initial commit

Browse Source
This commit is contained in:
Cole Deck 2023-09-01 17:26:52 -05:00
commit f28f5ac12c
18 changed files with 1621 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
.gitignore vendored Normal file
View File

@ -0,0 +1,5 @@
.pio
.vscode/.browse.c_cpp.db*
.vscode/c_cpp_properties.json
.vscode/launch.json
.vscode/ipch

10
.vscode/extensions.json vendored Normal file
View File

@ -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"
]
}

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

@ -0,0 +1,9 @@
{
"C_Cpp.errorSquiggles": "disabled",
"files.associations": {
"cmath": "cpp",
"*.tcc": "cpp",
"string": "cpp",
"ranges": "cpp"
}
}

39
include/README Normal file
View File

@ -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

46
lib/README Normal file
View File

@ -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 <Foo.h>
#include <Bar.h>
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

23
platformio.ini Normal file
View File

@ -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

321
src/MCP3XXX.h Normal file
View File

@ -0,0 +1,321 @@
//
// Copyright (c) 2018 Christopher Baker <https://christopherbaker.net>
//
// SPDX-License-Identifier: MIT
//
// Modified by Amelia Deck to use SPI1 + at a low frequency for optocouplers
#pragma once
#include <Arduino.h>
#include <SPI.h>
/// \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<uint8_t NumBits,
uint8_t NumChannels,
uint32_t MaxSPIClockSpeed,
uint8_t SPITransferLength = 3>
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;

126
src/base64.c Normal file
View File

@ -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;
}

68
src/base64.h Normal file
View File

@ -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

32
src/crc16.c Normal file
View File

@ -0,0 +1,32 @@
//#include <util/crc16.h>
/* 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);
}

13
src/crc16.h Normal file
View File

@ -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

54
src/globals.h Normal file
View File

@ -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

734
src/main.cpp Normal file
View File

@ -0,0 +1,734 @@
#include <Arduino.h>
#include <WiFi.h>
#include <Wire.h>
#include "globals.h"
#include <SPI.h>
#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);
}

18
src/packet.h Normal file
View File

@ -0,0 +1,18 @@
#pragma once
#include <stdint.h>
#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;

12
src/ribtest.code-workspace Normal file
View File

@ -0,0 +1,12 @@
{
"folders": [
{
"path": "../.."
},
{
"name": "ribtest",
"path": ".."
}
],
"settings": {}
}

91
src/zserio.cpp Normal file
View File

@ -0,0 +1,91 @@
#include <Arduino.h>
#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;
}
}

9
src/zserio.h Normal file
View File

@ -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();

11
test/README Normal file
View File

@ -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