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
Download Patch File Download Diff File Expand all files Collapse all files

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