From bbbd57177ab36a02f9c5fb192537da5d0960c6c9 Mon Sep 17 00:00:00 2001 From: Till Harbaum Date: Fri, 11 Dec 2020 21:08:26 +0100 Subject: [PATCH] Add examples for ftDuino bluetooth adapter --- .../ftDuino-Bluetooth/1sheeld/1sheeld.ino | 71 ++ .../1sheeld/FirmataSerial.cpp | 216 +++++++ .../ftDuino-Bluetooth/1sheeld/FirmataSerial.h | 28 + .../ftDuino-Bluetooth/1sheeld/Ftduino.cpp | 606 ++++++++++++++++++ .../ftDuino-Bluetooth/1sheeld/Ftduino.h | 135 ++++ .../ftDuino-Bluetooth/1sheeld/I2cSerialBt.cpp | 79 +++ .../ftDuino-Bluetooth/1sheeld/I2cSerialBt.h | 24 + .../ftDuino-Bluetooth/1sheeld/adc_table.h | 130 ++++ .../ftDuino-Bluetooth/1sheeld/readme.txt | 31 + .../ArduinoBlue/ArduinoBlue.ino | 106 +++ .../ArduinoBlue/I2cSerialBt.cpp | 79 +++ .../ArduinoBlue/I2cSerialBt.h | 24 + .../ftDuino-Bluetooth/ArduinoBlue/readme.txt | 14 + .../ftDuino-Bluetooth/Bridge/Bridge.ino | 30 + .../ftDuino-Bluetooth/Bridge/I2cSerialBt.cpp | 79 +++ .../ftDuino-Bluetooth/Bridge/I2cSerialBt.h | 24 + .../bluetooth_config/I2cSerialBt.cpp | 79 +++ .../bluetooth_config/I2cSerialBt.h | 24 + .../bluetooth_config/bluetooth_config.ino | 502 +++++++++++++++ .../ftduinoblue_demo/I2cSerialBt.cpp | 79 +++ .../ftduinoblue_demo/I2cSerialBt.h | 24 + .../ftduinoblue_demo/ftduinoblue.cpp | 143 +++++ .../ftduinoblue_demo/ftduinoblue.h | 45 ++ .../ftduinoblue_demo/ftduinoblue_demo.ino | 163 +++++ .../txt_automation_3axis/I2cSerialBt.cpp | 79 +++ .../txt_automation_3axis/I2cSerialBt.h | 24 + .../txt_automation_3axis/ftduinoblue.cpp | 143 +++++ .../txt_automation_3axis/ftduinoblue.h | 45 ++ .../txt_automation_3axis.ino | 185 ++++++ 29 files changed, 3211 insertions(+) create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/1sheeld.ino create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/FirmataSerial.cpp create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/FirmataSerial.h create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/Ftduino.cpp create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/Ftduino.h create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/I2cSerialBt.cpp create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/I2cSerialBt.h create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/adc_table.h create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/readme.txt create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ArduinoBlue/ArduinoBlue.ino create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ArduinoBlue/I2cSerialBt.cpp create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ArduinoBlue/I2cSerialBt.h create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ArduinoBlue/readme.txt create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/Bridge/Bridge.ino create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/Bridge/I2cSerialBt.cpp create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/Bridge/I2cSerialBt.h create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/bluetooth_config/I2cSerialBt.cpp create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/bluetooth_config/I2cSerialBt.h create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/bluetooth_config/bluetooth_config.ino create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ftduinoblue_demo/I2cSerialBt.cpp create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ftduinoblue_demo/I2cSerialBt.h create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ftduinoblue_demo/ftduinoblue.cpp create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ftduinoblue_demo/ftduinoblue.h create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ftduinoblue_demo/ftduinoblue_demo.ino create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/txt_automation_3axis/I2cSerialBt.cpp create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/txt_automation_3axis/I2cSerialBt.h create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/txt_automation_3axis/ftduinoblue.cpp create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/txt_automation_3axis/ftduinoblue.h create mode 100644 ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/txt_automation_3axis/txt_automation_3axis.ino diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/1sheeld.ino b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/1sheeld.ino new file mode 100644 index 0000000..1663c39 --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/1sheeld.ino @@ -0,0 +1,71 @@ +// +// 1sheeld.ino +// +// The bluetooth modules name needs to start with 1Sheeld for this to work + + +#define CUSTOM_SETTINGS +#define INCLUDE_SEVEN_SEGMENT_SHIELD +#define INCLUDE_KEYPAD_SHIELD + +#include "Ftduino.h" +#include + +#include "I2cSerialBt.h" +I2cSerialBt btSerial; + +#include "FirmataSerial.h" +FirmataSerial firmataSerial(btSerial); + +void setup() { + Serial.begin(115200); + + // wait max 1 sec for adapter to appear on bus. This is not + // needed as begin() will wait for the device. But this way + // we can use the led as an inidictaor for problems with + // the i2c uart adapater + if(!btSerial.check(1000)) { + // fast blink with led on failure + pinMode(LED_BUILTIN, OUTPUT); + while(true) { + digitalWrite(LED_BUILTIN, HIGH); + delay(100); + digitalWrite(LED_BUILTIN, LOW); + delay(100); + } + } + + btSerial.begin(9600); + + OneSheeld.begin(firmataSerial); + + ftduino.init(); +} + +void loop() { + int keys[][3] = { + { 0,0,Ftduino::O1 }, // 1 + { 0,1,Ftduino::O2 }, // 2 + { 0,2,Ftduino::O3 }, // 3 + { 1,0,Ftduino::O4 }, // 4 + { 1,1,Ftduino::O5 }, // 5 + { 1,2,Ftduino::O6 }, // 6 + { 2,0,Ftduino::O7 }, // 7 + { 2,1,Ftduino::O8 } // 8 + }; + + for(char i=0;i<8;i++) { + // request onesheeld keypad state + if(Keypad.isRowPressed(keys[i][0]) && Keypad.isColumnPressed(keys[i][1])) + ftduino.output_set(keys[i][2], Ftduino::HI, Ftduino::MAX); + else + ftduino.output_set(keys[i][2], 0, 0); + } + + // set onesheeld seven segment number + static byte number = 0; + SevenSegment.setNumber(number++); + if(number == 10) number = 0; + + OneSheeld.delay(100); +} diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/FirmataSerial.cpp b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/FirmataSerial.cpp new file mode 100644 index 0000000..797c6f7 --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/FirmataSerial.cpp @@ -0,0 +1,216 @@ +#include "FirmataSerial.h" + +// #define DEBUG_SYSEX +// #define DEBUG_BASIC + +#define VERSION_MAJOR 1 +#define VERSION_MINOR 6 + +bool FirmataSerial::parse_sysex(byte st, byte b) { + static uint8_t uart_flag, uart_byte; // buffer to assemble uart bytes + + // end of sysex message + if(b == 0xf7) + return true; + + // start of a new message? + if(st) { + uart_flag = 0; + + switch(b) { +#ifdef DEBUG_SYSEX + case 0x5c: + Serial.print(" QUERY_UART_BAUD_RATE"); + break; + case 0x5f: + Serial.print(" REPORT_INPUT_PINS"); + break; + case 0x62: + Serial.print(" IS_ALIVE"); + break; + case 0x64: + Serial.print(" FIRMATA_MUTE"); + break; +#endif + case 0x66: +#ifdef DEBUG_SYSEX + Serial.print(" UART_DATA"); +#endif + uart_flag = 1; + break; + default: +#ifdef DEBUG_SYSEX + Serial.print(" CMD"); + Serial.print(b, HEX); +#endif + break; + } + } else { + // with a sysex command + if(uart_flag) { + // uart parsing in progress? + if(uart_flag & 1) { + uart_flag = 2; + uart_byte = b & 0x7f; // actually bit 7 must never be set, anyway + } else { + uart_flag = 1; + uart_byte |= b << 7; + + this->uart_buffer_valid = true; + this->uart_buffer = uart_byte; + } + } +#ifdef DEBUG_SYSEX + else { + Serial.print("("); + Serial.print(b, HEX); + Serial.print(")"); + } +#endif + } + return false; +} + +void FirmataSerial::parse(byte b) { + static int8_t expect = 0; + + if(expect) { + if(expect > 0) { +#ifdef DEBUG_BASIC + Serial.print(" "); + Serial.print(b, HEX); +#endif + expect--; + +#ifdef DEBUG_BASIC + if(expect == 0) + Serial.println(""); +#endif + } else { + if(parse_sysex(expect==-2, b)) { +#ifdef DEBUG_BASIC + Serial.println(" <"); +#endif + expect = 0; // done parsing sysex + } else + expect = -1; // continue parsing sysex + } + return; + } + + if((b & 0xf0) == 0xe0) { +#ifdef DEBUG_BASIC + Serial.print("analog port message"); + Serial.print(b&0x0f, DEC); + Serial.print(" "); +#endif + expect = 2; + } else if((b & 0xf0) == 0x90) { +#ifdef DEBUG_BASIC + Serial.print("digital port message"); + Serial.print(b&0x0f, DEC); + Serial.print(" "); +#endif + expect = 2; + } else if((b & 0xf0) == 0xc0) { +#ifdef DEBUG_BASIC + Serial.print("report analog port "); + Serial.print(b&0x0f, DEC); +#endif + expect = 1; + } else if((b & 0xf0) == 0xd0) { +#ifdef DEBUG_BASIC + Serial.print("report digital port "); + Serial.print(b&0x0f, DEC); +#endif + expect = 1; + } else if(b == 0xf0) { +#ifdef DEBUG_BASIC + Serial.print("sysex"); +#endif + expect = -2; + } else if(b == 0xf4) { +#ifdef DEBUG_BASIC + Serial.print("set pin mode"); +#endif + expect = 2; + } else if(b == 0xf5) { +#ifdef DEBUG_BASIC + Serial.print("set digital pin value"); +#endif + expect = 2; + } else if(b == 0xf9) { +#ifdef DEBUG_BASIC + Serial.println("version"); +#endif +#if defined(VERSION_MAJOR) && defined(VERSION_MINOR) + this->m_s->write(0xf9); + this->m_s->write(VERSION_MINOR); + this->m_s->write(VERSION_MAJOR); +#endif + expect = 0; + } else if(b == 0xff) { +#ifdef DEBUG_BASIC + Serial.println("system reset"); +#endif + expect = 0; + } + +#ifdef DEBUG_BASIC + else { + Serial.print("Unknown cmd "); + Serial.println(b, HEX); + } +#endif +} + +FirmataSerial::FirmataSerial(Stream &s) { + this->uart_buffer_valid = false; + this->m_s = &s; +} + +void FirmataSerial::begin() { } + +void FirmataSerial::end() { } + +void FirmataSerial::poll(void) { + // if there is a byte in the buffer don't read more as + // we couldn't store any result + if(this->uart_buffer_valid) + return; + + if(this->m_s->available()) + parse(this->m_s->read()); +} + +int FirmataSerial::available() { + poll(); + return this->uart_buffer_valid?1:0; +} + +int FirmataSerial::availableForWrite() { + return 1; +} + +int FirmataSerial::read() { + poll(); + this->uart_buffer_valid = false; + return this->uart_buffer; +} + +int FirmataSerial::peek() { + return this->uart_buffer; +} + +void FirmataSerial::flush() { } + +size_t FirmataSerial::write(uint8_t byte) { + // write as serial + this->m_s->write(0xf0); // sysex start + this->m_s->write(0x66); // uart data + this->m_s->write(byte & 0x7f); // LSB + this->m_s->write((byte >> 7) & 1); // MSB + this->m_s->write(0xf7); // sysex end + + return 1; +} diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/FirmataSerial.h b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/FirmataSerial.h new file mode 100644 index 0000000..a1d2a17 --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/FirmataSerial.h @@ -0,0 +1,28 @@ + +#include +#include +#include + +class FirmataSerial : public Stream +{ +public: + FirmataSerial(Stream &); + ~FirmataSerial() { end(); } + void begin(); + void end(); + int available(); + int availableForWrite(); + int read(); + int peek(); + void flush(); + size_t write(uint8_t byte); + +private: + Stream *m_s; + bool parse_sysex(byte st, byte b); + void parse(byte b); + void poll(void); + + byte uart_buffer; + bool uart_buffer_valid; +}; diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/Ftduino.cpp b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/Ftduino.cpp new file mode 100644 index 0000000..2018213 --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/Ftduino.cpp @@ -0,0 +1,606 @@ +/* + Ftduino.cpp - Library for ftDuino + + (c) 2017 by Till Harbaum +*/ + +#include + +#include +#include + +#include "Ftduino.h" +#include "adc_table.h" + +// decreasing the filter time is possible, increasing it to more +// than 255 requires some variable sizes to be expanded + +#ifndef COUNTER_FILTER +#define COUNTER_FILTER 250 // in 1/250000 sec -> 250 == 1 msec +#endif + +// the library itself contains a static instance +Ftduino ftduino; + +void Ftduino::cd4051_init() { + // PC.6 = INH, PF.0 = A, PF.1 = B, PD.5 = C + + // make all 4051 control signals outputs + DDRC |= (1<<6); // INHIBIT + PORTC |= (1<<6); // pull it high by default, disabling the 4051 + + DDRF |= (1<<0); // A + PORTF &= ~(1<<0); // = low + + DDRF |= (1<<1); // B + PORTF &= ~(1<<1); // = low + + DDRD |= (1<<5); // C + PORTD &= ~(1<<5); // = low +} + +void Ftduino::cd4051_set(char mode) { + // enable/disable pullup + // 0..7 = on for I1..I8, 8 = off + if(mode & 8) + PORTC |= (1<<6); // activate INH pin + else { + PORTC &= ~(1<<6); // de-activate INH pin + if(mode & 1) PORTF |= (1<<0); // A=1 + else PORTF &= ~(1<<0); // A=0 + if(mode & 2) PORTF |= (1<<1); // B=1 + else PORTF &= ~(1<<1); // B=0 + if(mode & 4) PORTD |= (1<<5); // C=1 + else PORTD &= ~(1<<5); // C=0 + } +} + +#define ADC_REF ((0 << REFS1) | (1 << REFS0)) // AVcc with CAP on AREF + +/* adc interrupt service routine */ +/* At 16Mhz with a adc prescaler of 128, the conversion rate is at most 8900/sec. */ +/* Since there are 8 channels and we scan every channel twice the total */ +/* conversion rate is roughly 550Hz. */ + +// non-static function called by static IRQ handler +void Ftduino::adc_interrupt_exec() { + + // second conversion successful + if(adc_state & 1) { + // read ADC value and store it + adc_val[adc_state>>1] = ADC; + input_valid |= (1<<(adc_state>>1)); + } + + // next channel, lowest channel bit is state toggle + adc_state = (adc_state+1)&15; + + // We abuse the adc irq to frequently check for events + // in the counter inputs. There must be a nicer solution + if(!(adc_state & 3)) + counter_check_pending(adc_state >> 2); + + // switch to next channel and restart conversion + if(!(adc_state & 1)) + adc_prepare(adc_state>>1); + else + ADCSRA |= (1<>1))) + ftduino.cd4051_set(ftduino.adc_state>>1); + else + ftduino.cd4051_set(0xff); + + if(!(ftduino.adc_state & 8)) { + // ADC 4..7 + ADMUX = (1 << MUX2) | ADC_REF | ( (ftduino.adc_state>>1) & 0x03); + ADCSRB = 0; // MUX5 = 0 -"- + } else { + // ADC 8..11 + ADMUX = ADC_REF | ( (ftduino.adc_state>>1) & 0x03); + ADCSRB = (1<= Ftduino::I1) && (ch <= Ftduino::I8)) { + // wait for valid data if necessary + while(!(input_valid & (1<<(ch-Ftduino::I1)))) + _delay_us(100); + + return adc_val[ch-Ftduino::I1]; + } + + return 0; // sane value, but should never happen +} + +uint16_t Ftduino::adc_get_r(uint8_t ch) { + return pgm_read_word_near(adc2r + adc_get(ch)); +} + +/* a switch is considered closed if the resistance is < 500R */ +uint16_t Ftduino::adc_get_s(uint8_t ch) { + return( adc_get_r(ch) < 500 ); +} + +uint16_t Ftduino::adc_get_v(uint8_t ch) { + return 10000l * adc_get(ch) / 1023; +} + +uint16_t Ftduino::input_get(uint8_t ch) { + // pullup enabled -> resistance or switch + if(input_pullup & (1<<(ch-Ftduino::I1))) { + if(input_analog & (1<<(ch-Ftduino::I1))) + return adc_get_r(ch); + else + return adc_get_s(ch); + } + + return adc_get_v(ch); +} + +// interrupt driven 32 bit transfers +// an empty spi tx irq does roughly 110000 4 byte transfers/sec at SPI clk = 1Mhz +// or 27500 4 byte transfers/sec at SPI clk = 250khz +// + +// an output O1-O8 can be in one of three states: +// - floating: no MOSFET enabled +// - low: only lowside MOSFET enabled +// - high: only highside MOSFET enabled +// (- both MOSFETs should never be enabled at the same time) +// PWM toggles between floating and low or floating and high + +void Ftduino::spi_interrupt_exec() { + if(!(spi_state & 3)) { + // set /SS high and low again + PORTB |= (1<<0); + PORTB &= ~(1<<0); + + // fetch next data word + spi_tx_data.w = spi_tx_in[(spi_state>>2)&(SPI_PWM_CYCLE_LEN-1)]; + } + + if(spi_state & 1) SPDR = spi_tx_data.b[(spi_state>>1) & 1]; + else SPDR = 0; + spi_state++; +} + +void Ftduino::spi_interrupt() { + ftduino.spi_interrupt_exec(); +} + +void Ftduino::output_init() { + uint8_t i; + + // SPI clock considerations + // - the higher the clock rate, the higher is the CPU load caused by the IRQ handler + // at 1Mhz SPI clock the IRQ handler consumes roughly 30% of the CPU time + // - the lower the clock rate, the lower is the PWM clock cycle and PWM resolution + // that can be achieved. + // At 1Mhz we have roughly 100.000 bytes/sec or 25.000 MC33879 cycles/sec. At a + // PWM resolution of 0..64 which results in ~400 Hz PWM frequency + + // configure /SS, SCK and MOSI as output + // drive /SS it high + DDRB |= (1<<0) | (1<<1) | (1<<2); + PORTB |= (1<<0); + + // enable SPI, enable interrupts, MSB first, Master mode, + // mode 1 (SCK low when idle, data valid on falling edge) + // and clock = FCPU/64 = 250khz + SPCR = (1< highside S2, lowside D8 + // O2/O6 -> highside S6, lowside D1 + // O3/O7 -> highside S4, lowside D7 + // O4/O8 -> highside S5, lowside D3 + + // Never enable at the same time: + // channel 2 and 8, 6 and 1, 4 and 7, 5 and 3 + + // O1-O4 = first 33879 in chain + // O5-O8 = second 33879 in chain + + uint32_t set_mask = 0, clr_mask = 0; + + // mode can be 0 (floating), 1 (high) or 2 (low) + if(mode == 1) { + set_mask = mc33879_highside_map[port&3]; // highside to be set + clr_mask = mc33879_lowside_map[port&3]; // lowside to be cleared + } else if(mode == 2) { + set_mask = mc33879_lowside_map[port&3]; // lowside to be set + clr_mask = mc33879_highside_map[port&3]; // highside to be cleared + } else { + set_mask = 0; // nothing to be set + clr_mask = mc33879_highside_map[port&3] | // highside and lowside to be cleared + mc33879_lowside_map[port&3]; + } + + // ports O1..O4 (0-3) are in MSB, ports O5-O8 (4-7) are in MSB of lower 16 bit + if(!(port & 4)) { + set_mask <<= 8; + clr_mask <<= 8; + } + + // set 0..32 entries in the PWN table according the requested PWM + for(uint8_t i=0;i 4*COUNTER_FILTER) + counter_timer_exceeded(counter); + } +} + +void Ftduino::counter_timer_exceeded(uint8_t c) { + // get current port state + bool state = counter_get_pin_state(c); + + // save last accepted state and only count if + // the current state differs from the accepted one. + // otherwise very short spikes may e.g. counted + + // check if state of counter input was high and has fallen + if(counter_in_state & (1<> (2*c)) & 3; + + // count event if it has the desired edge + if((mode == Ftduino::C_EDGE_ANY) || + (!state && (mode == Ftduino::C_EDGE_FALLING)) || + ( state && (mode == Ftduino::C_EDGE_RISING)) ) + counter_val[c]++; + + // this counter timer has been processed + counter_event_time[c] = 0; +} + +uint16_t Ftduino::counter_get(uint8_t ch) { + if((ch >= Ftduino::C1) && (ch <= Ftduino::C4)) { + counter_check_pending(ch - Ftduino::C1); + return counter_val[ch-Ftduino::C1]; + } + + return 0; // sane value, but should never happen +} + +void Ftduino::counter_clear(uint8_t ch) { + counter_val[ch-Ftduino::C1] = 0; +} + +void Ftduino::counter_set_mode(uint8_t ch, uint8_t mode) { + char ch_idx = (ch-Ftduino::C1)<<1; // CH1 = 0, CH2 = 2, ... + + counter_modes &= ~(3 << ch_idx); // clear mode bits + counter_modes |= mode << ch_idx; // set new mode bits +} + +bool Ftduino::counter_get_state(uint8_t ch) { + return !counter_get_pin_state(ch-Ftduino::C1); +} + +// this irq fires whenever anything on one of the four +// counter ports +void Ftduino::ext_interrupt_exec(uint8_t counter) { + // implementation using the micros() function + counter_check_pending(counter); + counter_event_time[counter] = micros(); +} + +void Ftduino::ext_interrupt2() { + ftduino.ext_interrupt_exec(0); +} + +void Ftduino::ext_interrupt3() { + ftduino.ext_interrupt_exec(1); +} + +void Ftduino::pc_interrupt() { + // The hardware won't tell us which pin fired this interrupt. + // so we need to keep track of that outselves + static uint8_t portb_state = 0xff; // assume both pins are pulled high + uint8_t pinb = PINB; + + if((pinb ^ portb_state) & (1<<5)) + ftduino.ext_interrupt_exec(2); + + if((pinb ^ portb_state) & (1<<6)) + ftduino.ext_interrupt_exec(3); + + portb_state = pinb; +} + +void Ftduino::counter_init(void) { + + // C1/C2 are PD.2/PD.3 and C3/C4 are on PB.5/PB.6 + DDRD &= ~((1 << 2) | (1 << 3)); // counter ports C1 and C2 are inputs + PORTD &= ~((1 << 2) | (1 << 3)); // disable internal pullup, we use external + DDRB &= ~((1 << 5) | (1 << 6)); // counter ports C3 and C4 are inputs + PORTB &= ~((1 << 5) | (1 << 6)); // disable internal pullup, we use external + + // port change interrupts are not used as they cannot easily be made to cope with + // input signal bouncing + + // no counter is currently being used to control an encoder motor and all four motor + // brakes are on + counter4motor = 0xf0; + + // default mode is to not count at all + counter_modes = (Ftduino::C_EDGE_NONE << 6) | (Ftduino::C_EDGE_NONE << 4) | + (Ftduino::C_EDGE_NONE << 2) | (Ftduino::C_EDGE_NONE << 0); + + // generate irq on both edges for C1/INT2 and C1/INT3 + EICRA = (1< OCR1A + TCCR1B = (1< 250kHz + + // initially no counter input is assumed to be active, so we assmume they + // are pulled high + counter_in_state = 0xff; + + // clear all counters + for(char i=0;i<4;i++) + counter_val[i] = 0; +} + +void Ftduino::init() { + // initialization takes place in the init function and not in the + // constructor since the arduino wiring.c itself changes hw + // settings after the constructor has run + + // --------------------------------------------------------- + + // prepare ADC conversion for I1..I8 + adc_init(); + + // init and disable pulldown on C1. It is not nee + pulldown_c1_init(); + + // prepare spi service for M1..M4/O1..O8 + output_init(); + + counter_init(); +} + +Ftduino::Ftduino() { } diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/Ftduino.h b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/Ftduino.h new file mode 100644 index 0000000..87cb858 --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/Ftduino.h @@ -0,0 +1,135 @@ +/* + Ftduino.h - Library for ftduino + + (c) 2017 by Till Harbaum +*/ + +#ifndef ftduino_h +#define ftduino_h + +#include "Arduino.h" + +#define STRINGIFY(name) #name +#define CLASS_IRQ(name, vector) \ + static void name(void) asm(STRINGIFY(vector)) \ + __attribute__ ((signal, __INTR_ATTRS)) + +extern const PROGMEM unsigned short adc2r[]; + +// the PWM rate is roughly SPI-CLK/36/CYCLE_LEN +// if spi_state is 8 bit, SPI_PWM_CYCLE_LEN must be at most 256 +#define SPI_PWM_CYCLE_LEN 64 // must be a power of 2 + +class Ftduino { + public: + Ftduino(); + + void init(); + + // constant names for the counter inputs + static const uint8_t C1 = 0, C2 = 1, C3 = 2, C4 = 3; + + // constant names for the inputs + static const uint8_t I1 = 0, I2 = 1, I3 = 2, I4 = 3; + static const uint8_t I5 = 4, I6 = 5, I7 = 6, I8 = 7; + + // input modes + static const uint8_t RESISTANCE = 0, VOLTAGE = 1, SWITCH = 2; + + // constant names for the outputs + static const uint8_t O1 = 0, O2 = 1, O3 = 2, O4 = 3; + static const uint8_t O5 = 4, O6 = 5, O7 = 6, O8 = 7; + static const uint8_t M1 = 0, M2 = 1, M3 = 2, M4 = 3; + + // input counter modes + static const uint8_t C_EDGE_NONE = 0, C_EDGE_RISING = 1, C_EDGE_FALLING = 2, C_EDGE_ANY = 3; + + // max/ḿin PWM values and level namesVOLTAGE + static const uint8_t MAX = SPI_PWM_CYCLE_LEN, ON = SPI_PWM_CYCLE_LEN, OFF = 0; // name for extreme PWM values + static const uint8_t HI = 1, LO = 2; // OFF = 0 also applies + + // motor modes + static const uint8_t LEFT = 1, RIGHT = 2, BRAKE = 3; + + void input_set_mode(uint8_t ch, uint8_t mode); + uint16_t input_get(uint8_t ch); + + void output_set(uint8_t port, uint8_t mode, uint8_t pwm); + void motor_set(uint8_t port, uint8_t mode, uint8_t pwm); + void motor_counter(uint8_t port, uint8_t mode, uint8_t pwm, uint16_t counter); + bool motor_counter_active(uint8_t port); + void motor_counter_set_brake(uint8_t port, bool on); + + void counter_set_mode(uint8_t ch, uint8_t mode); + uint16_t counter_get(uint8_t ch); + void counter_clear(uint8_t ch); + bool counter_get_state(uint8_t ch); + + private: + + // --------------- ADC ------------- + void adc_init(); + + void adc_prepare(uint8_t ch); + uint16_t adc_get(uint8_t ch); + uint16_t adc_get_r(uint8_t ch); + uint16_t adc_get_v(uint8_t ch); + uint16_t adc_get_s(uint8_t ch); + + // ADC related variables + uint8_t adc_state; + volatile uint16_t adc_val[8]; + + // ADC irq handler + void adc_interrupt_exec(); + CLASS_IRQ(adc_interrupt, ADC_vect); + + // --------------- 4051 ------------- + void cd4051_init(); + void cd4051_set(char mode); + + uint8_t input_pullup = 0xff; // all pullups enabled by default, a pullup enabled means "resistance mode" + uint8_t input_analog = 0x00; // flag whether input is digital or analog. All inouts are digital by default + uint8_t input_valid = 0x00; // a valid value has been received for this channel + + // ---------- outputs ---------------- + void output_init(); + + void spi_interrupt_exec(); + CLASS_IRQ(spi_interrupt, SPI_STC_vect); + + uint8_t spi_state = 0; // byte/word counter for spi transmission + uint16_t spi_tx_in[SPI_PWM_CYCLE_LEN]; // set outside interrupt + + // repeatedly sent during interupt: + union { + uint16_t w; // repeated during interupt + uint8_t b[2]; + } spi_tx_data; + + void pulldown_c1_init(); + + // ------- counter inputs ------------ + void counter_init(void); + void counter_timer_exceeded(uint8_t c); + bool counter_get_pin_state(uint8_t c); + + // time when last event has been seen + uint32_t counter_event_time[4] = { 0,0,0,0 }; + void counter_check_pending(uint8_t ch); + + uint8_t counter_in_state; + + void ext_interrupt_exec(uint8_t c); + CLASS_IRQ(ext_interrupt2, INT2_vect); + CLASS_IRQ(ext_interrupt3, INT3_vect); + CLASS_IRQ(pc_interrupt, PCINT0_vect); + + volatile uint8_t counter4motor; + volatile uint16_t counter_val[4]; + uint8_t counter_modes; +}; + +extern Ftduino ftduino; + +#endif diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/I2cSerialBt.cpp b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/I2cSerialBt.cpp new file mode 100644 index 0000000..ec6cd8c --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/I2cSerialBt.cpp @@ -0,0 +1,79 @@ +#include "I2cSerialBt.h" +#include +#include +#define ADDR 5 // make configurable + +I2cSerialBt::I2cSerialBt() { } + +bool I2cSerialBt::check(int timeout_msec) { + Wire.begin(); + + uint32_t timeout = millis(); + do { + Wire.beginTransmission(ADDR); + if(!Wire.endTransmission()) + return true; + } + while(millis() - timeout < timeout_msec); + + return false; +} + +uint8_t I2cSerialBt::registerRead(char r) { + Wire.beginTransmission(ADDR); + Wire.write(r); + Wire.endTransmission(false); + Wire.requestFrom(ADDR,1); + return Wire.read(); +} + +void I2cSerialBt::registerWrite(char r, uint8_t d) { + Wire.beginTransmission(ADDR); + Wire.write(r); + Wire.write(d); + Wire.endTransmission(); +} + +void I2cSerialBt::begin(uint32_t baud) { + Wire.begin(); + + // wait for i2c adapter to show up on bus + do + Wire.beginTransmission(ADDR); + while(Wire.endTransmission()); + + // set baud rate + registerWrite(0, baud/1200); +} + +void I2cSerialBt::key(bool on) { + registerWrite(1, on?0:1); +} + +void I2cSerialBt::end() { } + +int I2cSerialBt::available() { + // check i2c client for data + return registerRead(0); +} + +int I2cSerialBt::availableForWrite() { + // check for space in clients tx buffer + return registerRead(1); +} + +int I2cSerialBt::read() { + return registerRead(2); +} + +// peek does not work (yet) ... +int I2cSerialBt::peek() { return 0; } + +void I2cSerialBt::flush() { } + +size_t I2cSerialBt::write(uint8_t byte) { + registerWrite(2, byte); + return 1; +} + +// using Print::write; diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/I2cSerialBt.h b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/I2cSerialBt.h new file mode 100644 index 0000000..ab31909 --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/I2cSerialBt.h @@ -0,0 +1,24 @@ +#include +#include + +class I2cSerialBt : public Stream +{ +public: + I2cSerialBt(); + ~I2cSerialBt() { end(); } + void begin(uint32_t baud); + void end(); + + bool check(int timeout); + void key(bool on); + int available(); + int availableForWrite(); + int read(); + int peek(); + void flush(); + size_t write(uint8_t byte); + +private: + uint8_t registerRead(char r); + void registerWrite(char r, uint8_t d); +}; diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/adc_table.h b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/adc_table.h new file mode 100644 index 0000000..65c0af0 --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/adc_table.h @@ -0,0 +1,130 @@ +const PROGMEM unsigned short adc2r[] = { + 0, 6, 12, 19, 25, 32, 38, 45, + 52, 58, 65, 72, 78, 85, 92, 99, + 106, 112, 119, 126, 133, 140, 147, 154, + 161, 168, 175, 183, 190, 197, 204, 212, + 219, 226, 234, 241, 248, 256, 263, 271, + 279, 286, 294, 302, 309, 317, 325, 333, + 340, 348, 356, 364, 372, 380, 388, 396, + 404, 413, 421, 429, 437, 446, 454, 463, + 471, 479, 488, 497, 505, 514, 522, 531, + 540, 549, 558, 566, 575, 584, 593, 602, + 612, 621, 630, 639, 648, 658, 667, 677, + 686, 696, 705, 715, 724, 734, 744, 754, + 763, 773, 783, 793, 803, 814, 824, 834, + 844, 854, 865, 875, 886, 896, 907, 917, + 928, 939, 950, 961, 971, 982, 993, 1005, + 1016, 1027, 1038, 1050, 1061, 1072, 1084, 1096, + 1107, 1119, 1131, 1142, 1154, 1166, 1178, 1190, + 1203, 1215, 1227, 1240, 1252, 1265, 1277, 1290, + 1303, 1315, 1328, 1341, 1354, 1367, 1380, 1394, + 1407, 1420, 1434, 1448, 1461, 1475, 1489, 1503, + 1517, 1531, 1545, 1559, 1573, 1588, 1602, 1617, + 1632, 1646, 1661, 1676, 1691, 1706, 1722, 1737, + 1752, 1768, 1783, 1799, 1815, 1831, 1847, 1863, + 1879, 1895, 1912, 1928, 1945, 1962, 1979, 1996, + 2013, 2030, 2047, 2065, 2082, 2100, 2118, 2136, + 2154, 2172, 2190, 2209, 2227, 2246, 2265, 2283, + 2302, 2322, 2341, 2360, 2380, 2400, 2420, 2440, + 2460, 2480, 2501, 2521, 2542, 2563, 2584, 2605, + 2626, 2648, 2670, 2692, 2714, 2736, 2758, 2781, + 2803, 2826, 2849, 2872, 2896, 2919, 2943, 2967, + 2991, 3016, 3040, 3065, 3090, 3115, 3140, 3166, + 3191, 3217, 3243, 3270, 3296, 3323, 3350, 3378, + 3405, 3433, 3461, 3489, 3517, 3546, 3575, 3604, + 3633, 3663, 3693, 3723, 3754, 3784, 3815, 3847, + 3878, 3910, 3942, 3975, 4007, 4041, 4074, 4108, + 4141, 4176, 4210, 4245, 4281, 4316, 4352, 4388, + 4425, 4462, 4499, 4537, 4575, 4614, 4653, 4692, + 4732, 4772, 4812, 4853, 4894, 4936, 4978, 5021, + 5064, 5107, 5151, 5196, 5241, 5286, 5332, 5378, + 5425, 5473, 5521, 5569, 5618, 5668, 5718, 5769, + 5820, 5872, 5924, 5977, 6031, 6085, 6140, 6196, + 6252, 6309, 6367, 6426, 6485, 6545, 6605, 6667, + 6729, 6792, 6855, 6920, 6985, 7052, 7119, 7187, + 7256, 7326, 7396, 7468, 7541, 7615, 7689, 7765, + 7842, 7920, 7999, 8079, 8161, 8243, 8327, 8412, + 8498, 8586, 8675, 8765, 8857, 8950, 9044, 9140, + 9238, 9337, 9437, 9540, 9644, 9749, 9857, 9966, + 10077, 10190, 10305, 10422, 10541, 10662, 10785, 10910, + 11038, 11168, 11301, 11436, 11573, 11713, 11856, 12001, + 12150, 12301, 12455, 12613, 12773, 12937, 13105, 13276, + 13450, 13628, 13810, 13996, 14187, 14381, 14580, 14783, + 14992, 15205, 15423, 15646, 15875, 16109, 16349, 16596, + 16848, 17107, 17373, 17646, 17927, 18215, 18511, 18815, + 19128, 19450, 19781, 20122, 20474, 20836, 21210, 21595, + 21993, 22404, 22828, 23267, 23721, 24191, 24677, 25181, + 25704, 26246, 26809, 27394, 28002, 28635, 29294, 29981, + 30697, 31446, 32228, 33046, 33904, 34802, 35746, 36737, + 37780, 38879, 40039, 41265, 42562, 43937, 45398, 46952, + 48609, 50379, 52274, 54308, 56498, 58860, 61418, 64195, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, + 65535, 65535, 65535, 65535, 65535, 65535, 65535 +}; \ No newline at end of file diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/readme.txt b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/readme.txt new file mode 100644 index 0000000..1b718d7 --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/1sheeld/readme.txt @@ -0,0 +1,31 @@ +1Sheeld for ftDuino +------------------- + +This is a 1Sheeld demo sketch for the ftDuino and the ftDuino +bluetooth adapter. It's supposed to be used in conjunction with the +1Sheeld smartphone app. This app is available for Android and +IOS. Only the Android version has been tested with this sketch. + +This sketch requires the OneSheeld library to be installed in the +Arduino IDE. You can install it using the library manager. + +This sketch makes use of the "Keypad" and "7 Segement" features if 1Sheeld. +The sketch allows to control the outputs O1 to O8 of the ftDuino using +the keypad keys 1 to 8. And the 7 segement is used as a counter by the +ftDuino. + +1Sheeld will only detect the ftDuino if the Bluetooth name starts with +"1Sheeld". Use e.g. the ftDuino bluetooth_config sketch to set the name +to 1Sheeld like so: + +-=- ftDuino bluetooth configuration -=- +For more information see http://ftduino.de +I²C uart bridge found. +Checking for bluetooth module at 9600 baud ... OK +Checking for baud value ... OK, found HM-11 +Type 'HELP' for more info. +CMD> name 1Sheeld +Name changed to: '1Sheeld' +CMD> + +Afterwards the module will be detected by the 1Sheeld app. diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ArduinoBlue/ArduinoBlue.ino b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ArduinoBlue/ArduinoBlue.ino new file mode 100644 index 0000000..91dbe42 --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ArduinoBlue/ArduinoBlue.ino @@ -0,0 +1,106 @@ +// +// ArduinoBlue +// +// ftDuino example for ArduinoBlue https://sites.google.com/stonybrook.edu/arduinoble +// This demo needs the ArduinoBlue library. Please install it using the library manager. +// + +#include +#include +#include "I2cSerialBt.h" +I2cSerialBt btSerial; + +ArduinoBlue phone(btSerial); + +int prevThrottle = 49; +int prevSteering = 49; +int throttle, steering, sliderVal, button, sliderId; + +void setup() +{ + ftduino.init(); + Serial.begin(9600); + + // wait max 1 sec for adapter to appear on bus. This is not + // needed as begin() will wait for the device. But this way + // we can use the led as an inidictaor for problems with + // the i2c uart adapater + if(!btSerial.check(1000)) { + // fast blink with led on failure + pinMode(LED_BUILTIN, OUTPUT); + while(true) { + digitalWrite(LED_BUILTIN, HIGH); + delay(100); + digitalWrite(LED_BUILTIN, LOW); + delay(100); + } + } + + btSerial.begin(9600); + btSerial.key(0); +}; + +void loop() +{ + // ID of the button pressed pressed. + button = phone.getButton(); + + // Returns the text data sent from the phone. + // After it returns the latest data, empty string "" is sent in subsequent. + // calls until text data is sent again. + String str = phone.getText(); + + // Throttle and steering values go from 0 to 99. + // When throttle and steering values are at 99/2 = 49, the joystick is at center. + throttle = phone.getThrottle(); + steering = phone.getSteering(); + + // ID of the slider moved. + sliderId = phone.getSliderId(); + + // Slider value goes from 0 to 200. + sliderVal = phone.getSliderVal(); + + // Display button data whenever its pressed. + if (button != -1) { + Serial.print("Button: "); + Serial.println(button); + } + + // Display slider data when slider moves + if (sliderId != -1) { + Serial.print("Slider ID: "); + Serial.print(sliderId); + Serial.print("\tValue: "); + Serial.println(sliderVal); + } + + // Display throttle and steering data if steering or throttle value is changed + if (prevThrottle != throttle || prevSteering != steering) { + Serial.print("Throttle: "); Serial.print(throttle); Serial.print("\tSteering: "); Serial.println(steering); + prevThrottle = throttle; + prevSteering = steering; + + // use throttle to control M1 + if(throttle >= 50) // 50 ... 99 + ftduino.motor_set(Ftduino::M1, Ftduino::LEFT, Ftduino::MAX * (throttle-50) / 49); + else if(throttle <= 48) // 0 ... 48 + ftduino.motor_set(Ftduino::M1, Ftduino::RIGHT, Ftduino::MAX * (48-throttle) / 48); + else // 49 + ftduino.motor_set(Ftduino::M1, Ftduino::OFF, Ftduino::MAX); + } + + // If a text from the phone was sent print it to the serial monitor + if (str != "") { + Serial.println(str); + } + + // Send string from serial command line to the phone. This will alert the user. + if (Serial.available()) { + Serial.write("send: "); + String str = Serial.readString(); + phone.sendMessage(str); // phone.sendMessage(str) sends the text to the phone. + Serial.print(str); + Serial.write('\n'); + } +}; diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ArduinoBlue/I2cSerialBt.cpp b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ArduinoBlue/I2cSerialBt.cpp new file mode 100644 index 0000000..ec6cd8c --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ArduinoBlue/I2cSerialBt.cpp @@ -0,0 +1,79 @@ +#include "I2cSerialBt.h" +#include +#include +#define ADDR 5 // make configurable + +I2cSerialBt::I2cSerialBt() { } + +bool I2cSerialBt::check(int timeout_msec) { + Wire.begin(); + + uint32_t timeout = millis(); + do { + Wire.beginTransmission(ADDR); + if(!Wire.endTransmission()) + return true; + } + while(millis() - timeout < timeout_msec); + + return false; +} + +uint8_t I2cSerialBt::registerRead(char r) { + Wire.beginTransmission(ADDR); + Wire.write(r); + Wire.endTransmission(false); + Wire.requestFrom(ADDR,1); + return Wire.read(); +} + +void I2cSerialBt::registerWrite(char r, uint8_t d) { + Wire.beginTransmission(ADDR); + Wire.write(r); + Wire.write(d); + Wire.endTransmission(); +} + +void I2cSerialBt::begin(uint32_t baud) { + Wire.begin(); + + // wait for i2c adapter to show up on bus + do + Wire.beginTransmission(ADDR); + while(Wire.endTransmission()); + + // set baud rate + registerWrite(0, baud/1200); +} + +void I2cSerialBt::key(bool on) { + registerWrite(1, on?0:1); +} + +void I2cSerialBt::end() { } + +int I2cSerialBt::available() { + // check i2c client for data + return registerRead(0); +} + +int I2cSerialBt::availableForWrite() { + // check for space in clients tx buffer + return registerRead(1); +} + +int I2cSerialBt::read() { + return registerRead(2); +} + +// peek does not work (yet) ... +int I2cSerialBt::peek() { return 0; } + +void I2cSerialBt::flush() { } + +size_t I2cSerialBt::write(uint8_t byte) { + registerWrite(2, byte); + return 1; +} + +// using Print::write; diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ArduinoBlue/I2cSerialBt.h b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ArduinoBlue/I2cSerialBt.h new file mode 100644 index 0000000..ab31909 --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ArduinoBlue/I2cSerialBt.h @@ -0,0 +1,24 @@ +#include +#include + +class I2cSerialBt : public Stream +{ +public: + I2cSerialBt(); + ~I2cSerialBt() { end(); } + void begin(uint32_t baud); + void end(); + + bool check(int timeout); + void key(bool on); + int available(); + int availableForWrite(); + int read(); + int peek(); + void flush(); + size_t write(uint8_t byte); + +private: + uint8_t registerRead(char r); + void registerWrite(char r, uint8_t d); +}; diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ArduinoBlue/readme.txt b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ArduinoBlue/readme.txt new file mode 100644 index 0000000..d5c8917 --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ArduinoBlue/readme.txt @@ -0,0 +1,14 @@ +ArduinoBlue for ftDuino +----------------------- + +This is a 1Sheeld demo sketch for the ftDuino and the ftDuino +bluetooth interface. It's supposed to be used in conjunction with the +ArduinoBlue smartphone app. This app is available for Android and +IOS. Only the Android version has been tested with this sketch. + +This sketch requires the ArduinoBlue library to be installed in the +Arduino IDE. You can install it using the library manager. + +This sketch makes use of throttle function (drive forward and +backward) to control a motor in ftDuino port M1. + diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/Bridge/Bridge.ino b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/Bridge/Bridge.ino new file mode 100644 index 0000000..025b02f --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/Bridge/Bridge.ino @@ -0,0 +1,30 @@ +#include +#include "I2cSerialBt.h" +I2cSerialBt btSerial; + +void setup() { + Serial.begin(115200); + while(!Serial); + btSerial.begin(9600); + Wire.setClock(400000); + btSerial.key(0); + +#if 0 + delay(1000); + btSerial.key(1); // force 9600 baud + delay(1500); + btSerial.key(0); +#endif +} + +void loop() { + + // send if there's data to be sent and if the receiver is able to + // accept data + while(Serial.available() && btSerial.availableForWrite()) + btSerial.write(Serial.read()); + + if(btSerial.available() && Serial.availableForWrite()) + Serial.write(btSerial.read()); + +} diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/Bridge/I2cSerialBt.cpp b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/Bridge/I2cSerialBt.cpp new file mode 100644 index 0000000..ec6cd8c --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/Bridge/I2cSerialBt.cpp @@ -0,0 +1,79 @@ +#include "I2cSerialBt.h" +#include +#include +#define ADDR 5 // make configurable + +I2cSerialBt::I2cSerialBt() { } + +bool I2cSerialBt::check(int timeout_msec) { + Wire.begin(); + + uint32_t timeout = millis(); + do { + Wire.beginTransmission(ADDR); + if(!Wire.endTransmission()) + return true; + } + while(millis() - timeout < timeout_msec); + + return false; +} + +uint8_t I2cSerialBt::registerRead(char r) { + Wire.beginTransmission(ADDR); + Wire.write(r); + Wire.endTransmission(false); + Wire.requestFrom(ADDR,1); + return Wire.read(); +} + +void I2cSerialBt::registerWrite(char r, uint8_t d) { + Wire.beginTransmission(ADDR); + Wire.write(r); + Wire.write(d); + Wire.endTransmission(); +} + +void I2cSerialBt::begin(uint32_t baud) { + Wire.begin(); + + // wait for i2c adapter to show up on bus + do + Wire.beginTransmission(ADDR); + while(Wire.endTransmission()); + + // set baud rate + registerWrite(0, baud/1200); +} + +void I2cSerialBt::key(bool on) { + registerWrite(1, on?0:1); +} + +void I2cSerialBt::end() { } + +int I2cSerialBt::available() { + // check i2c client for data + return registerRead(0); +} + +int I2cSerialBt::availableForWrite() { + // check for space in clients tx buffer + return registerRead(1); +} + +int I2cSerialBt::read() { + return registerRead(2); +} + +// peek does not work (yet) ... +int I2cSerialBt::peek() { return 0; } + +void I2cSerialBt::flush() { } + +size_t I2cSerialBt::write(uint8_t byte) { + registerWrite(2, byte); + return 1; +} + +// using Print::write; diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/Bridge/I2cSerialBt.h b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/Bridge/I2cSerialBt.h new file mode 100644 index 0000000..ab31909 --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/Bridge/I2cSerialBt.h @@ -0,0 +1,24 @@ +#include +#include + +class I2cSerialBt : public Stream +{ +public: + I2cSerialBt(); + ~I2cSerialBt() { end(); } + void begin(uint32_t baud); + void end(); + + bool check(int timeout); + void key(bool on); + int available(); + int availableForWrite(); + int read(); + int peek(); + void flush(); + size_t write(uint8_t byte); + +private: + uint8_t registerRead(char r); + void registerWrite(char r, uint8_t d); +}; diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/bluetooth_config/I2cSerialBt.cpp b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/bluetooth_config/I2cSerialBt.cpp new file mode 100644 index 0000000..ec6cd8c --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/bluetooth_config/I2cSerialBt.cpp @@ -0,0 +1,79 @@ +#include "I2cSerialBt.h" +#include +#include +#define ADDR 5 // make configurable + +I2cSerialBt::I2cSerialBt() { } + +bool I2cSerialBt::check(int timeout_msec) { + Wire.begin(); + + uint32_t timeout = millis(); + do { + Wire.beginTransmission(ADDR); + if(!Wire.endTransmission()) + return true; + } + while(millis() - timeout < timeout_msec); + + return false; +} + +uint8_t I2cSerialBt::registerRead(char r) { + Wire.beginTransmission(ADDR); + Wire.write(r); + Wire.endTransmission(false); + Wire.requestFrom(ADDR,1); + return Wire.read(); +} + +void I2cSerialBt::registerWrite(char r, uint8_t d) { + Wire.beginTransmission(ADDR); + Wire.write(r); + Wire.write(d); + Wire.endTransmission(); +} + +void I2cSerialBt::begin(uint32_t baud) { + Wire.begin(); + + // wait for i2c adapter to show up on bus + do + Wire.beginTransmission(ADDR); + while(Wire.endTransmission()); + + // set baud rate + registerWrite(0, baud/1200); +} + +void I2cSerialBt::key(bool on) { + registerWrite(1, on?0:1); +} + +void I2cSerialBt::end() { } + +int I2cSerialBt::available() { + // check i2c client for data + return registerRead(0); +} + +int I2cSerialBt::availableForWrite() { + // check for space in clients tx buffer + return registerRead(1); +} + +int I2cSerialBt::read() { + return registerRead(2); +} + +// peek does not work (yet) ... +int I2cSerialBt::peek() { return 0; } + +void I2cSerialBt::flush() { } + +size_t I2cSerialBt::write(uint8_t byte) { + registerWrite(2, byte); + return 1; +} + +// using Print::write; diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/bluetooth_config/I2cSerialBt.h b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/bluetooth_config/I2cSerialBt.h new file mode 100644 index 0000000..ab31909 --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/bluetooth_config/I2cSerialBt.h @@ -0,0 +1,24 @@ +#include +#include + +class I2cSerialBt : public Stream +{ +public: + I2cSerialBt(); + ~I2cSerialBt() { end(); } + void begin(uint32_t baud); + void end(); + + bool check(int timeout); + void key(bool on); + int available(); + int availableForWrite(); + int read(); + int peek(); + void flush(); + size_t write(uint8_t byte); + +private: + uint8_t registerRead(char r); + void registerWrite(char r, uint8_t d); +}; diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/bluetooth_config/bluetooth_config.ino b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/bluetooth_config/bluetooth_config.ino new file mode 100644 index 0000000..eb33079 --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/bluetooth_config/bluetooth_config.ino @@ -0,0 +1,502 @@ +/* + * bluetooth_config.ino + * + * Bluetooth configuration tool for ftDuino bluetooth modules + */ + +#include +#include + +#include "I2cSerialBt.h" +I2cSerialBt btSerial; + +uint32_t cmd_timer = 0; +enum cmd_e { CMD_NONE=0, CMD_CHECK, CMD_RESET, CMD_BAUD, CMD_NAME, CMD_VERSION } cmd = CMD_NONE; +enum module_type_e { MODULE_NONE, MODULE_HM11, MODULE_HM17, MODULE_UNKNOWN } module_type = MODULE_NONE; + +static const long baud_vals_hm11[] = { 9600, 19200, 38400, 57600, 115200, 4800, 2400, 1200, 230400, -1 }; +static const long baud_vals_hm17[] = { 1200, 2400, 4800, 9600, 19200, 38400, 57600, 115200, 230400, -1 }; + +void halt() { + Serial.println(F("Press a key to retry ...")); + while(!Serial.available()); + wdt_enable(WDTO_15MS); + for(;;); +} + +void setup() { + + // setup connection to host and wait for host + Serial.begin(115200); + while(!Serial); + + Serial.println(F("\n\n\n-=- ftDuino bluetooth configuration -=-")); + Serial.println(F("For more information see http://ftduino.de")); + + if(!btSerial.check(1000)) { + Serial.println(F("ERROR: No ftDuino bluetooth adapter found on i²c address 5")); + Serial.println(F("Please check that the module is properly connected and restart your ftDuino.")); + halt(); + } + + Serial.println(F("I²C uart bridge found.")); + + baud_detect(); + + Serial.println(F("Type 'HELP' for more info.")); + show_prompt(); +} + +// flush anything that might be in the queue from the bt modules +void bt_flush() { + delay(50); + while(btSerial.available()) { btSerial.read(); delay(50); } +} + +bool check_baud_value(long baud) { + char buffer[16] = ""; + char fill = 0; + + Serial.print(F("Checking for baud value ... ")); + + bt_flush(); + btSerial.println(F("AT+BAUD?")); + + // wait for reply (with timeout) + uint32_t timeout = millis(); + while(millis() - timeout < 1000) { + if(btSerial.available()) { + char c = btSerial.read(); + if(c > 31) { + if(fill < sizeof(buffer)-1) + buffer[fill++] = c; + } else if(c == '\n') { + buffer[fill++] = 0; + bt_flush(); + + if(strlen(buffer) < 8) { + Serial.print(F("FAILED (reply too short '")); + Serial.print(buffer); + Serial.println(F("')")); + return false; + } + + if(strncasecmp(buffer, "OK+Get:", 7) != 0) { + Serial.print(F("FAILED (unexpected reply '")); + Serial.print(buffer); + Serial.println(F("')")); + return false; + } + + if(buffer[7] < '0' || buffer[7] > '8') { + Serial.print(F("FAILED (unexpected value '")); + Serial.print(buffer[7]); + Serial.println(F("')")); + return false; + } + + // check if this is the expected value + char value = buffer[7] - '0'; + if((baud_vals_hm11[value] != baud) && (baud_vals_hm17[value] != baud)) { + Serial.print(F("FAILED (values don't match '")); + Serial.print(value, DEC); + Serial.print(F(" != ")); + Serial.print(baud, DEC); + Serial.println(F("')")); + Serial.println(F("ERROR: Detected module is not a HM-11, HM-17 or HM-19.")); + Serial.println(F(" Baud rate settings disabled.")); + module_type = MODULE_UNKNOWN; + return true; + } + + if(baud_vals_hm11[value] == baud) { + Serial.println(F("OK, found HM-11")); + module_type = MODULE_HM11; + } else { + Serial.println(F("OK, found HM-17/HM-19")); + module_type = MODULE_HM17; + } + + if(baud == 230400) { + Serial.println(F("Warning: The detected baud rate is valid for HM-11,")); + Serial.println(F(" HM-17 and HM-19. BAUD rate setting may be wrong")); + } + + return true; + } + } + } + + Serial.println(F("FAILED (reply timeout)")); + bt_flush(); + return false; +} + +long baud_detect_rate() { + // try to figure baud rate out + long baud = -1; + for(char i = 0; (baud < 0) && baud_vals_hm11[i] > 0;i++) { + Serial.print(F("Checking for bluetooth module at ")); + Serial.print(baud_vals_hm11[i], DEC); + Serial.print(F(" baud ...")); + + if(baud_check(baud_vals_hm11[i])) { + Serial.println(F(" OK")); + baud = baud_vals_hm11[i]; + } else + Serial.println(F(" FAILED")); + } + + if(baud >= 0) { + // check baud value of module and compare it against detected rate. + if(!check_baud_value(baud)) + halt(); + } + + return baud; +} + +void baud_detect() { + if(baud_detect_rate() < 0) { + Serial.println(F("No module in AT mode found. Trying to force by sys_key ...")); + + btSerial.key(true); + delay(1500); + btSerial.key(false); + delay(1000); + + if(baud_detect_rate() < 0) { + Serial.println(F("ERROR: No bluetooth module found!")); + halt(); + } + } +} + +char baud_check(long rate) { + char fill = 0, buffer[4]=""; + + btSerial.begin(rate); + Wire.setClock(400000); + + // flush buffer + bt_flush(); + + // send "AT" + btSerial.println("AT"); + uint32_t timeout = millis(); + + // wait for "OK" + while((strncasecmp(buffer, "OK", 2) != 0 && (millis() - timeout) < 250)) + if(fill < 3 && btSerial.available()) + buffer[fill++] = btSerial.read(); + + // flush buffer + bt_flush(); + + // return true if "OK" was found in buffer + return(strncasecmp(buffer, "OK", 2) == 0); +} + +void cmd_done() { + cmd = CMD_NONE; + cmd_timer = 0; + show_prompt(); +} + +void send_reset() { + send_at_cmd(CMD_RESET, (char*)""); +} + +void send_baud(char *parms) { + if(module_type != MODULE_HM11 && module_type != MODULE_HM17) + return; + + if(!parms) { + send_at_cmd(CMD_BAUD, NULL); + return; + } + + long b = atol(parms); + + char req_baud = -1; + if(module_type == MODULE_HM11) { + for(char i = 0; baud_vals_hm11[i] > 0;i++) + if(baud_vals_hm11[i] == b) + req_baud = i; + } else { + for(char i = 0; baud_vals_hm17[i] > 0;i++) + if(baud_vals_hm17[i] == b) + req_baud = i; + } + + if(req_baud < 0) { + Serial.println(F("ERROR: Unsupported baud rate!")); + cmd_done(); + return; + } + + parms[0] = '0' + req_baud; + parms[1] = 0; + send_at_cmd(CMD_BAUD, parms); +} + +void send_version() { + send_at_cmd(CMD_VERSION, NULL); +} + +void send_name(char *parms) { + if(!parms) { + send_at_cmd(CMD_NAME, NULL); + return; + } + + if(strlen(parms) > 13) { + Serial.println(F("ERROR: New name too long. The name must have at most 13 characters.")); + cmd_done(); + return; + } + + send_at_cmd(CMD_NAME, parms); +} + +void send_check() { + btSerial.println("AT"); + cmd = CMD_CHECK; + cmd_timer = millis(); +} + +void send_at_cmd(cmd_e c, char *parms) { + btSerial.print(F("AT+")); + if(c == CMD_BAUD) + btSerial.print(F("BAUD")); + else if(c == CMD_RESET) + btSerial.print(F("RESET")); + else if(c == CMD_NAME) + btSerial.print(F("NAME")); + else if(c == CMD_VERSION) + btSerial.print(F("VERS")); + else { + Serial.println(F("ERROR: Unknown command code")); + cmd_done(); + return; + } + + if(!parms) btSerial.println(F("?")); + else btSerial.println(parms); + + cmd = c; + cmd_timer = millis(); +} + +void show_prompt() { + Serial.print(F("CMD> ")); +} + +void print_help() { + Serial.println(F("Available commands:")); + Serial.println(F("HELP - show this help")); + Serial.println(F("CHECK - check communication")); + if(module_type == MODULE_HM11 || module_type == MODULE_HM17) + Serial.println(F("BAUD - show/set baud rate")); + Serial.println(F("NAME - show/set device name")); + Serial.println(F("VERSION - show firmware version")); + Serial.println(F("RESET - reset the module")); + Serial.println(F("KEY - assert SYS KEY for 1.5 sec")); + if(module_type != MODULE_HM11 && module_type != MODULE_HM17) + Serial.println(F("Baud rate setting are disabled for this unknown device!")); +} + +void parse_user(char c) { + static char buffer[32+1]; // max 32 bytes + term char + static char fill = 0; + + if(c >= 32 && fill < sizeof(buffer)-1) { + buffer[fill++] = c; + Serial.write(c); + } else if(c == '\r') { + Serial.println(""); + buffer[fill++] = 0; // terminate command + fill = 0; + + char *cmd_begin = buffer; + while(*cmd_begin && *cmd_begin == ' ') cmd_begin++; + char *cmd_end = cmd_begin; + while(*cmd_end && *cmd_end != ' ') cmd_end++; + + char *parms = NULL; + if(*cmd_end != 0) { + parms = cmd_end; + while(*parms && *parms == ' ') parms++; + if(!*parms) + parms = NULL; // there were only white spaces after the command + + // remove all trailing spaces + char *pend = parms; + while(*pend) pend++; + while(pend > parms && (*pend == 0 || *pend == ' ')) + *pend-- = 0; + } + + // terminate command + *cmd_end = 0; + + if(strcasecmp(cmd_begin, "HELP") == 0) { + print_help(); + show_prompt(); + } else if(strcasecmp(cmd_begin, "CHECK") == 0) { + send_check(); + } else if(strcasecmp(cmd_begin, "RESET") == 0) { + send_reset(); + } else if(strcasecmp(cmd_begin, "KEY") == 0) { + Serial.println(F("Activating key!")); + btSerial.key(true); + delay(1500); + Serial.println(F("Releasing key!")); + btSerial.key(false); + show_prompt(); + } else if(strcasecmp(cmd_begin, "NAME") == 0) { + send_name(parms); + } else if(strcasecmp(cmd_begin, "VERSION") == 0) { + send_version(); + } else if(((module_type == MODULE_HM11) || + (module_type == MODULE_HM17)) && + strcasecmp(cmd_begin, "BAUD") == 0) { + send_baud(parms); + } else { + Serial.print(F("ERROR: Unknown command '")); + Serial.print(cmd_begin); + Serial.println(F("'!")); + show_prompt(); + } + } +} + +void parse_bt(char c) { + static char buffer[32+1]; // max 32 bytes + term char + static char fill = 0; + + if(c >= 32 && fill < sizeof(buffer)-1) { + buffer[fill++] = c; + } else if(c == '\r') { + buffer[fill++] = 0; // terminate command + fill = 0; + + // check if reply begins with "OK+" + if(strncasecmp(buffer, "OK", 2) == 0) { + // if the pending command is CMD_CHECK, then the OK is all we need + if(cmd == CMD_CHECK) + Serial.println(F("Module is ok")); + else { + // check if this is a 'Set:' reply + if(strncasecmp(buffer+2, "+Set:", 5) == 0) { + if(cmd == CMD_BAUD) { + char val = buffer[7]-'0'; + if(val < 0 || val > 8) { + Serial.print(F("ERROR: Unexpected baud value: ")); + Serial.println(val, DEC); + } else { + Serial.print(F("BAUD rate changed to ")); + Serial.print(val, DEC); + Serial.print(F(": ")); + if(module_type == MODULE_HM11) + Serial.print(baud_vals_hm11[val], DEC); + else if(module_type == MODULE_HM17) + Serial.print(baud_vals_hm17[val], DEC); + else { + Serial.print("value"); + Serial.print(val, DEC); + } + Serial.println(F(". A reset is needed to activate it.")); + } + } else if(cmd == CMD_NAME) { + char *name = buffer+7; + + Serial.print(F("Name changed to: '")); + Serial.print(name); + Serial.println(F("'")); + } else { + Serial.print(F("Set value to: ")); + Serial.println(buffer+7); + } + } + + // check if this is a 'Get:' reply + else if(strncasecmp(buffer+2, "+Get:", 5) == 0) { + if(cmd == CMD_BAUD) { + char val = buffer[7]-'0'; + if(val < 0 || val > 8) { + Serial.print(F("ERROR: Unexpected baud value: ")); + Serial.println(val, DEC); + } else { + Serial.print(F("BAUD value is ")); + Serial.print(val, DEC); + Serial.print(F(": ")); + if(module_type == MODULE_HM11) + Serial.print(baud_vals_hm11[val], DEC); + else if(module_type == MODULE_HM17) + Serial.print(baud_vals_hm17[val], DEC); + else { + Serial.print("value"); + Serial.print(val, DEC); + } + + Serial.println(F(" baud.")); + } + } else if(cmd == CMD_VERSION) { + // hm-17 and hm-19 report with "Get:" + Serial.print(F("Firmware version is '")); + Serial.print(buffer+7); + Serial.println(F("'.")); + } else { + Serial.print(F("Got a value back: ")); + Serial.println(buffer+7); + } + } + + else if(strncasecmp(buffer+2, "+reset", 6) == 0) { + Serial.println(F("Reset successful. Re-checking for module")); + delay(1000); + baud_detect(); + } + + else if(strncasecmp(buffer+2, "+name:", 6) == 0) { + char *name = buffer+8; + while(*name == ' ') name++; + + Serial.print(F("Name is: '")); + Serial.print(name); + Serial.println("'"); + } + + else { + Serial.print(F("Reply code: ")); + Serial.println(buffer+2); + } + } + } else if(cmd == CMD_VERSION) { + // hm-11 doesn't even send 'ok' + Serial.print(F("Firmware version is '")); + Serial.print(buffer); + Serial.println(F("'.")); + } else { + Serial.print(F("ERROR: Unexpected reply: '")); + Serial.print(buffer); + Serial.println(F("'")); + } + cmd_done(); + } +} + +void loop() { + while(Serial.available() && !cmd_timer) + parse_user(Serial.read()); + + while(btSerial.available()) + parse_bt(btSerial.read()); + + // check for 2 seconds command timeout + if(cmd_timer && (millis()- cmd_timer > 2000)) { + Serial.println(F("ERROR: Timeout while waiting for reply from module")); + cmd_done(); + } +} diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ftduinoblue_demo/I2cSerialBt.cpp b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ftduinoblue_demo/I2cSerialBt.cpp new file mode 100644 index 0000000..ec6cd8c --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ftduinoblue_demo/I2cSerialBt.cpp @@ -0,0 +1,79 @@ +#include "I2cSerialBt.h" +#include +#include +#define ADDR 5 // make configurable + +I2cSerialBt::I2cSerialBt() { } + +bool I2cSerialBt::check(int timeout_msec) { + Wire.begin(); + + uint32_t timeout = millis(); + do { + Wire.beginTransmission(ADDR); + if(!Wire.endTransmission()) + return true; + } + while(millis() - timeout < timeout_msec); + + return false; +} + +uint8_t I2cSerialBt::registerRead(char r) { + Wire.beginTransmission(ADDR); + Wire.write(r); + Wire.endTransmission(false); + Wire.requestFrom(ADDR,1); + return Wire.read(); +} + +void I2cSerialBt::registerWrite(char r, uint8_t d) { + Wire.beginTransmission(ADDR); + Wire.write(r); + Wire.write(d); + Wire.endTransmission(); +} + +void I2cSerialBt::begin(uint32_t baud) { + Wire.begin(); + + // wait for i2c adapter to show up on bus + do + Wire.beginTransmission(ADDR); + while(Wire.endTransmission()); + + // set baud rate + registerWrite(0, baud/1200); +} + +void I2cSerialBt::key(bool on) { + registerWrite(1, on?0:1); +} + +void I2cSerialBt::end() { } + +int I2cSerialBt::available() { + // check i2c client for data + return registerRead(0); +} + +int I2cSerialBt::availableForWrite() { + // check for space in clients tx buffer + return registerRead(1); +} + +int I2cSerialBt::read() { + return registerRead(2); +} + +// peek does not work (yet) ... +int I2cSerialBt::peek() { return 0; } + +void I2cSerialBt::flush() { } + +size_t I2cSerialBt::write(uint8_t byte) { + registerWrite(2, byte); + return 1; +} + +// using Print::write; diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ftduinoblue_demo/I2cSerialBt.h b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ftduinoblue_demo/I2cSerialBt.h new file mode 100644 index 0000000..ab31909 --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ftduinoblue_demo/I2cSerialBt.h @@ -0,0 +1,24 @@ +#include +#include + +class I2cSerialBt : public Stream +{ +public: + I2cSerialBt(); + ~I2cSerialBt() { end(); } + void begin(uint32_t baud); + void end(); + + bool check(int timeout); + void key(bool on); + int available(); + int availableForWrite(); + int read(); + int peek(); + void flush(); + size_t write(uint8_t byte); + +private: + uint8_t registerRead(char r); + void registerWrite(char r, uint8_t d); +}; diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ftduinoblue_demo/ftduinoblue.cpp b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ftduinoblue_demo/ftduinoblue.cpp new file mode 100644 index 0000000..2d90c56 --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ftduinoblue_demo/ftduinoblue.cpp @@ -0,0 +1,143 @@ +#include "ftduinoblue.h" +#include + +FtduinoBlue::FtduinoBlue(Stream &s, const char *l) { + this->mStream = &s; + this->layout = l; + this->mOutSum = 0; + this->buffer_fill = 0; + this->m_callback = NULL; +} + +void FtduinoBlue::setCallback(void (*cb)(struct reply *)) { + this->m_callback = cb; +} + +int FtduinoBlue::available() { + return this->mStream->available(); +} + +int FtduinoBlue::read() { + return this->mStream->read(); +} + +size_t FtduinoBlue::write(uint8_t b) { + if(b>=32) mOutSum += b; // ignore all control characters (especially the \r and \n at the end of the line) + // according to + // https://www.arduino.cc/reference/en/language/functions/communication/serial/println/ + // println always sends \r\n. So we insert the checksum before the \r + if(b == '\r') { + this->mStream->write(':'); + if(mOutSum < 16) this->mStream->write('0'); + this->mStream->print(mOutSum, HEX); + mOutSum = 0; + } + return this->mStream->write(b); +} + +int FtduinoBlue::peek() { + return this->mStream->peek(); +} + +char FtduinoBlue::parseHexDigit(char a) { + if(a >= '0' && a <= '9') return a-'0'; + if(a >= 'a' && a <= 'f') return 10+a-'a'; + if(a >= 'A' && a <= 'F') return 10+a-'A'; + return 0; +} + +void FtduinoBlue::handle(void) { + // parse everything coming from Serial1 (bluetooth side) + while(available()) { + uint8_t c = read(); + + // buffer all characters but the control characters + if(c >= 32) { + if(buffer_fill < sizeof(buffer)) + buffer[buffer_fill++] = c; + } else if((c == '\n' || c == '\r')) { + // verify checksum if present + if(buffer_fill > 3 && buffer[buffer_fill-3] == ':') { + // extract checksum + uint8_t csum_in = + 16 * parseHexDigit(buffer[buffer_fill-2]) + + parseHexDigit(buffer[buffer_fill-1]); + + // calculate checksum + uint8_t csum_calc = 0; + for(char i=0;i +#include + +// A very simple wrapper class that appends a checksum to +// every message sent. +class FtduinoBlue: public Stream { +public: + static const uint8_t FTDB_STATE = 0; + static const uint8_t FTDB_LABEL = 1; + static const uint8_t FTDB_BUTTON = 2; + static const uint8_t FTDB_SWITCH = 3; + static const uint8_t FTDB_JOYSTICK = 4; + static const uint8_t FTDB_SLIDER = 5; + + struct reply { + uint8_t type, id; + union { + bool state; + int slider; + struct { + int8_t x, y; + } joystick; + }; + } mReply; + + FtduinoBlue(Stream &s, const char *l); + int available(); + int read(); + size_t write(uint8_t b); + int peek(); + void handle(void); + void setCallback(void (*)(struct reply *)); +private: + char parseHexDigit(char a); + void parseCommand(char *buffer); + int parseParameter(char **idx); + + void (*m_callback)(struct reply *); + + Stream *mStream; + uint8_t mOutSum = 0; + char buffer[32]; + uint8_t buffer_fill = 0; + const char *layout; +}; diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ftduinoblue_demo/ftduinoblue_demo.ino b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ftduinoblue_demo/ftduinoblue_demo.ino new file mode 100644 index 0000000..7ea4baa --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/ftduinoblue_demo/ftduinoblue_demo.ino @@ -0,0 +1,163 @@ +/* + * ftduinoblue_demo.ino + * + * This is a demo sketch for ftDuinoBlue. It allows you to create a custom + * remote android user interface for your project right inside you arduino + * sketch. No android programming required. + * + * For more info see http://ftduino.de/blue + */ + +#include +#include +#include "ftduinoblue.h" + +// use i2c bluetooth adapter +#include "I2cSerialBt.h" +I2cSerialBt btSerial; +#include + +// the layout to be sent to the ftDuinoBlue app +const char layout[] PROGMEM = + "" + "LED on/off" + "" + "" + "" + "" + ""; + +FtduinoBlue ftdblue(btSerial, layout); + +void setup() { + Serial.begin(9600); // some debug output is done on Serial (USB) + + ftduino.init(); + + // register callback for ftduinoblue + ftdblue.setCallback(ftduinoblue_callback); + + // wait max 1 sec for adapter to appear on bus. This is not + // needed as begin() will wait for the device. But this way + // we can use the led as an inidictaor for problems with + // the i2c uart adapater + if(!btSerial.check(1000)) { + // fast blink with led on failure + pinMode(LED_BUILTIN, OUTPUT); + while(true) { + digitalWrite(LED_BUILTIN, HIGH); + delay(100); + digitalWrite(LED_BUILTIN, LOW); + delay(100); + } + } + + // initialize i2c uart to 9600 baud + btSerial.begin(9600); + + // prepare led + pinMode(LED_BUILTIN, OUTPUT); + digitalWrite(LED_BUILTIN, LOW); +}; + +// led config. This has a startup default and can be changed +// by the user via bluetooth. This is also reported back to +// the user app to provide the correct initial values should +// the user reconnect with the app +static char ledChanged = true; +static char ledState = true; // LED is on +static uint8_t ledBlinkSpeed = 50; // ~ 2 Hz +static uint8_t ledBrightness = 128; // 50% brightness + +void ftduinoblue_callback(struct FtduinoBlue::reply *r) { + switch(r->type) { + case FtduinoBlue::FTDB_STATE: + Serial.println("STATE"); + + ftdblue.print("SWITCH 1 "); + ftdblue.println(ledState?"ON":"OFF"); + ftdblue.print("SLIDER 3 "); + ftdblue.println(ledBrightness); + ftdblue.print("SLIDER 5 "); + ftdblue.println(ledBlinkSpeed); + break; + + case FtduinoBlue::FTDB_BUTTON: + Serial.print("BUTTON "); + Serial.print(r->id, DEC); + Serial.print(" "); + Serial.println(r->state?"DOWN":"UP"); + break; + + case FtduinoBlue::FTDB_SWITCH: + Serial.print("SWITCH "); + Serial.print(r->id, DEC); + Serial.print(" "); + Serial.println(r->state?"ON":"OFF"); + + // make sure the led reacts on switch 1 + if(r->id == 1) { + ledChanged = true; + ledState = r->state; + } + break; + + case FtduinoBlue::FTDB_SLIDER: + Serial.print("SLIDER "); + Serial.print(r->id, DEC); + Serial.print(" "); + Serial.println(r->slider, DEC); + + if(r->id == 3) { + ledBrightness = r->slider; + ledChanged = true; // user has changed something -> led needs to be updated + } + if(r->id == 5) { + ledBlinkSpeed = r->slider; + ledChanged = true; // user has changed something -> led needs to be updated + } + break; + + case FtduinoBlue::FTDB_JOYSTICK: + Serial.print("JOYSTICK "); + Serial.print(r->id, DEC); + Serial.print(" "); + Serial.print(r->joystick.x, DEC); + Serial.print(" "); + Serial.println(r->joystick.y, DEC); + break; + + default: + Serial.print("UNKNOWN "); + Serial.println(r->type, DEC); + break; + } +} + +void loop() { + + // led blink timer + static uint32_t led_tick = 0; + static bool led_state = false; + + // led is updated on timer or by user change + if(ledChanged || (millis() - led_tick) > 50+10*(100-ledBlinkSpeed)) { + // check if led update was triggered by user + if(!ledChanged) { + // no. So it was the timer -> handle timer + led_state = !led_state; + led_tick = millis(); + } else + ledChanged = false; + + // update state of physical led. Set analog brightness if + // a) led is enabled by user and b) led is actually in the on state + // of the blinking. Switch it off otherwise + if(led_state && ledState) analogWrite(LED_BUILTIN, ledBrightness); + else digitalWrite(LED_BUILTIN, LOW); + if(led_state && ledState) ftduino.output_set(Ftduino::O1, Ftduino::HI, ledBrightness/4); + else ftduino.output_set(Ftduino::O1, Ftduino::OFF, 0); + } + + ftdblue.handle(); +}; diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/txt_automation_3axis/I2cSerialBt.cpp b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/txt_automation_3axis/I2cSerialBt.cpp new file mode 100644 index 0000000..ec6cd8c --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/txt_automation_3axis/I2cSerialBt.cpp @@ -0,0 +1,79 @@ +#include "I2cSerialBt.h" +#include +#include +#define ADDR 5 // make configurable + +I2cSerialBt::I2cSerialBt() { } + +bool I2cSerialBt::check(int timeout_msec) { + Wire.begin(); + + uint32_t timeout = millis(); + do { + Wire.beginTransmission(ADDR); + if(!Wire.endTransmission()) + return true; + } + while(millis() - timeout < timeout_msec); + + return false; +} + +uint8_t I2cSerialBt::registerRead(char r) { + Wire.beginTransmission(ADDR); + Wire.write(r); + Wire.endTransmission(false); + Wire.requestFrom(ADDR,1); + return Wire.read(); +} + +void I2cSerialBt::registerWrite(char r, uint8_t d) { + Wire.beginTransmission(ADDR); + Wire.write(r); + Wire.write(d); + Wire.endTransmission(); +} + +void I2cSerialBt::begin(uint32_t baud) { + Wire.begin(); + + // wait for i2c adapter to show up on bus + do + Wire.beginTransmission(ADDR); + while(Wire.endTransmission()); + + // set baud rate + registerWrite(0, baud/1200); +} + +void I2cSerialBt::key(bool on) { + registerWrite(1, on?0:1); +} + +void I2cSerialBt::end() { } + +int I2cSerialBt::available() { + // check i2c client for data + return registerRead(0); +} + +int I2cSerialBt::availableForWrite() { + // check for space in clients tx buffer + return registerRead(1); +} + +int I2cSerialBt::read() { + return registerRead(2); +} + +// peek does not work (yet) ... +int I2cSerialBt::peek() { return 0; } + +void I2cSerialBt::flush() { } + +size_t I2cSerialBt::write(uint8_t byte) { + registerWrite(2, byte); + return 1; +} + +// using Print::write; diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/txt_automation_3axis/I2cSerialBt.h b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/txt_automation_3axis/I2cSerialBt.h new file mode 100644 index 0000000..ab31909 --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/txt_automation_3axis/I2cSerialBt.h @@ -0,0 +1,24 @@ +#include +#include + +class I2cSerialBt : public Stream +{ +public: + I2cSerialBt(); + ~I2cSerialBt() { end(); } + void begin(uint32_t baud); + void end(); + + bool check(int timeout); + void key(bool on); + int available(); + int availableForWrite(); + int read(); + int peek(); + void flush(); + size_t write(uint8_t byte); + +private: + uint8_t registerRead(char r); + void registerWrite(char r, uint8_t d); +}; diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/txt_automation_3axis/ftduinoblue.cpp b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/txt_automation_3axis/ftduinoblue.cpp new file mode 100644 index 0000000..2d90c56 --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/txt_automation_3axis/ftduinoblue.cpp @@ -0,0 +1,143 @@ +#include "ftduinoblue.h" +#include + +FtduinoBlue::FtduinoBlue(Stream &s, const char *l) { + this->mStream = &s; + this->layout = l; + this->mOutSum = 0; + this->buffer_fill = 0; + this->m_callback = NULL; +} + +void FtduinoBlue::setCallback(void (*cb)(struct reply *)) { + this->m_callback = cb; +} + +int FtduinoBlue::available() { + return this->mStream->available(); +} + +int FtduinoBlue::read() { + return this->mStream->read(); +} + +size_t FtduinoBlue::write(uint8_t b) { + if(b>=32) mOutSum += b; // ignore all control characters (especially the \r and \n at the end of the line) + // according to + // https://www.arduino.cc/reference/en/language/functions/communication/serial/println/ + // println always sends \r\n. So we insert the checksum before the \r + if(b == '\r') { + this->mStream->write(':'); + if(mOutSum < 16) this->mStream->write('0'); + this->mStream->print(mOutSum, HEX); + mOutSum = 0; + } + return this->mStream->write(b); +} + +int FtduinoBlue::peek() { + return this->mStream->peek(); +} + +char FtduinoBlue::parseHexDigit(char a) { + if(a >= '0' && a <= '9') return a-'0'; + if(a >= 'a' && a <= 'f') return 10+a-'a'; + if(a >= 'A' && a <= 'F') return 10+a-'A'; + return 0; +} + +void FtduinoBlue::handle(void) { + // parse everything coming from Serial1 (bluetooth side) + while(available()) { + uint8_t c = read(); + + // buffer all characters but the control characters + if(c >= 32) { + if(buffer_fill < sizeof(buffer)) + buffer[buffer_fill++] = c; + } else if((c == '\n' || c == '\r')) { + // verify checksum if present + if(buffer_fill > 3 && buffer[buffer_fill-3] == ':') { + // extract checksum + uint8_t csum_in = + 16 * parseHexDigit(buffer[buffer_fill-2]) + + parseHexDigit(buffer[buffer_fill-1]); + + // calculate checksum + uint8_t csum_calc = 0; + for(char i=0;i +#include + +// A very simple wrapper class that appends a checksum to +// every message sent. +class FtduinoBlue: public Stream { +public: + static const uint8_t FTDB_STATE = 0; + static const uint8_t FTDB_LABEL = 1; + static const uint8_t FTDB_BUTTON = 2; + static const uint8_t FTDB_SWITCH = 3; + static const uint8_t FTDB_JOYSTICK = 4; + static const uint8_t FTDB_SLIDER = 5; + + struct reply { + uint8_t type, id; + union { + bool state; + int slider; + struct { + int8_t x, y; + } joystick; + }; + } mReply; + + FtduinoBlue(Stream &s, const char *l); + int available(); + int read(); + size_t write(uint8_t b); + int peek(); + void handle(void); + void setCallback(void (*)(struct reply *)); +private: + char parseHexDigit(char a); + void parseCommand(char *buffer); + int parseParameter(char **idx); + + void (*m_callback)(struct reply *); + + Stream *mStream; + uint8_t mOutSum = 0; + char buffer[32]; + uint8_t buffer_fill = 0; + const char *layout; +}; diff --git a/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/txt_automation_3axis/txt_automation_3axis.ino b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/txt_automation_3axis/txt_automation_3axis.ino new file mode 100644 index 0000000..3f6db9d --- /dev/null +++ b/ftduino/libraries/Ftduino/examples/Bluetooth/ftDuino-Bluetooth/txt_automation_3axis/txt_automation_3axis.ino @@ -0,0 +1,185 @@ +/* + * txt_automation_3achs.ino + * + * Control sketch for the fischertechnik 3-axis robot driven by + * the ftDuino + ftDuino bluetooth module using ftDuinoBlue. + * + * For more info see http://ftduino.de/blue + */ + +#include +#include +#include "ftduinoblue.h" + +// use i2c bluetooth adapter +#include "I2cSerialBt.h" +I2cSerialBt btSerial; +#include + +#define BUTTON_COLOR_1 "#009688" +#define BUTTON_COLOR_2 "#889600" +#define BUTTON_COLOR_E "#800000" + +#if 1 // vertical arrangement +// the layout to be sent to the ftDuinoBlue app +const char layout[] PROGMEM = +"" +" " +" " +" " +" " +" " +" " +" " +" " +" " +" " +" " +""; +#endif + +FtduinoBlue ftdblue(btSerial, layout); + +void setup() { + Serial.begin(9600); // some debug output is done on Serial (USB) + + ftduino.init(); + + // inputs I1-I4 are the endstop switches + ftduino.input_set_mode(Ftduino::I1, Ftduino::SWITCH); + ftduino.input_set_mode(Ftduino::I2, Ftduino::SWITCH); + ftduino.input_set_mode(Ftduino::I3, Ftduino::SWITCH); + ftduino.input_set_mode(Ftduino::I4, Ftduino::SWITCH); + + // register callback for ftduinoblue + ftdblue.setCallback(ftduinoblue_callback); + + // wait max 1 sec for adapter to appear on bus. This is not + // needed as begin() will wait for the device. But this way + // we can use the led as an inidictaor for problems with + // the i2c uart adapater + if(!btSerial.check(1000)) { + // fast blink with led on failure + pinMode(LED_BUILTIN, OUTPUT); + while(true) { + digitalWrite(LED_BUILTIN, HIGH); + delay(100); + digitalWrite(LED_BUILTIN, LOW); + delay(100); + } + } + + // initialize i2c uart to 9600 baud + btSerial.begin(9600); + + // prepare led + pinMode(LED_BUILTIN, OUTPUT); + digitalWrite(LED_BUILTIN, LOW); +}; + +// current state of motors +static uint8_t motor_state[4] = { 0,0,0,0 }; + +// button ids per motor, e.g. buttons 3 and 4 control first motor M1 +static const uint8_t id[4][2] = { { 3,4 }, { 7,8 }, { 2,5 }, { 11,10 } }; + +void process_motor(uint8_t i, FtduinoBlue::reply *r) { + + // check if the reported id belongs to one of the + // two buttons used for this motor + if(r->id == id[i][0] || r->id == id[i][1]) { + uint8_t m = motor_state[i]; // read current motor state + + if(r->id == id[i][0]) { // motor left? + if(r->state) m |= 1; + else m &= ~1; + } + if(r->id == id[i][1]) { // motor right? + if(r->state) m |= 2; + else m &= ~2; + } + + // don't drive into active endstop + if(m == 1 && ftduino.input_get(Ftduino::I1+i)) + m = 0; + + if(m != motor_state[i]) { + if(m == 1) ftduino.motor_set(Ftduino::M1+i, Ftduino::LEFT, Ftduino::MAX); + else if(m == 2) ftduino.motor_set(Ftduino::M1+i, Ftduino::RIGHT, Ftduino::MAX); + else ftduino.motor_set(Ftduino::M1+i, Ftduino::BRAKE, Ftduino::MAX); + motor_state[i] = m; + } + } +} + +void process_motors(FtduinoBlue::reply *r) { + for(char i=0;i<4;i++) + process_motor(i, r); +} + +void ftduinoblue_callback(struct FtduinoBlue::reply *r) { + switch(r->type) { + case FtduinoBlue::FTDB_STATE: + Serial.println("STATE"); + // update state of endstop buttons if necessary + for(char i=0;i<4;i++) { + if(ftduino.input_get(Ftduino::I1+i)) { + ftdblue.print("BGCOLOR "); ftdblue.print(id[i][0], DEC); ftdblue.println(" "BUTTON_COLOR_E); + ftdblue.print("DISABLE "); ftdblue.println(id[i][0], DEC); + } + } + break; + + case FtduinoBlue::FTDB_BUTTON: + process_motors(r); + break; + + case FtduinoBlue::FTDB_SWITCH: + Serial.print("SWITCH "); + Serial.print(r->id, DEC); + Serial.print(" "); + Serial.println(r->state?"ON":"OFF"); + break; + + case FtduinoBlue::FTDB_SLIDER: + Serial.print("SLIDER "); + Serial.print(r->id, DEC); + Serial.print(" "); + Serial.println(r->slider, DEC); + break; + } +} + +void loop() { + // keep track of endstop states + static uint8_t endstop_state[4] = { 0,0,0,0}; + + // permanently monitor the endstops + for(char i=0;i<4;i++) { + + // check if endstop state has changed + uint8_t endstop = ftduino.input_get(Ftduino::I1+i); + if(endstop != endstop_state[i]) { + // colorize buttons via endstop state + ftdblue.print("BGCOLOR "); ftdblue.print(id[i][0], DEC); + ftdblue.println(endstop?" "BUTTON_COLOR_E:((id[i][0]<5)?" " BUTTON_COLOR_1:" " BUTTON_COLOR_2)); + + // and enable/disable them + if(endstop) ftdblue.print("DISABLE "); + else ftdblue.print("ENABLE "); + ftdblue.println(id[i][0], DEC); + + // if endstop active and motor running in direction of + // endstop, then stop the motor + if(endstop && motor_state[i] == 1) { + ftduino.motor_set(Ftduino::M1+i, Ftduino::BRAKE, Ftduino::MAX); + motor_state[i] = 0; + } + + // save new state + endstop_state[i] = endstop; + } + } + + ftdblue.handle(); +};