From 626df13dd27468889a16622e657040ca2f2d86ae Mon Sep 17 00:00:00 2001 From: ladyada Date: Sun, 27 May 2018 20:49:52 -0400 Subject: [PATCH] archive https://github.com/adafruit/BLE-Robot-Rover --- Ada_BLE_RC/Ada_BLE_RC.ino | 413 +++++++++++++++++++++++++++++++++++ Ada_BLE_RC/BluefruitConfig.h | 57 +++++ Ada_BLE_RC/README.md | 2 + Ada_BLE_RC/packetParser.cpp | 138 ++++++++++++ 4 files changed, 610 insertions(+) create mode 100644 Ada_BLE_RC/Ada_BLE_RC.ino create mode 100644 Ada_BLE_RC/BluefruitConfig.h create mode 100644 Ada_BLE_RC/README.md create mode 100644 Ada_BLE_RC/packetParser.cpp diff --git a/Ada_BLE_RC/Ada_BLE_RC.ino b/Ada_BLE_RC/Ada_BLE_RC.ino new file mode 100644 index 000000000..d672a7380 --- /dev/null +++ b/Ada_BLE_RC/Ada_BLE_RC.ino @@ -0,0 +1,413 @@ +/********************************************************************* + This is an example for our nRF51822 based Bluefruit LE modules + + Modified to drive a 3-wheeled BLE Robot Rover! by http://james.devi.to + + Pick one up today in the Adafruit shop! + + Adafruit invests time and resources providing this open source code, + please support Adafruit and open-source hardware by purchasing + products from Adafruit! + + MIT license, check LICENSE for more information + All text above, and the splash screen below must be included in + any redistribution +*********************************************************************/ + +#include +#include +#include +#if not defined (_VARIANT_ARDUINO_DUE_X_) + #include +#endif + +#include "Adafruit_BLE.h" +#include "Adafruit_BluefruitLE_SPI.h" +#include "Adafruit_BluefruitLE_UART.h" + +#include "BluefruitConfig.h" + +#include +#include +// #include "utility/Adafruit_PWMServoDriver.h" +// #include + +// Create the motor shield object with the default I2C address +Adafruit_MotorShield AFMS = Adafruit_MotorShield(); + +// And connect 2 DC motors to port M3 & M4 ! +Adafruit_DCMotor *L_MOTOR = AFMS.getMotor(4); +Adafruit_DCMotor *R_MOTOR = AFMS.getMotor(3); + +//not used, testing acceleration +// int accelTime = 200; + +//Name your RC here +String BROADCAST_NAME = "adafruit red robot rover"; + +String BROADCAST_CMD = String("AT+GAPDEVNAME=" + BROADCAST_NAME); + +Adafruit_BluefruitLE_SPI ble(BLUEFRUIT_SPI_CS, BLUEFRUIT_SPI_IRQ, BLUEFRUIT_SPI_RST); + + +// A small helper +void error(const __FlashStringHelper*err) { + Serial.println(err); + while (1); +} + +// function prototypes over in packetparser.cpp +uint8_t readPacket(Adafruit_BLE *ble, uint16_t timeout); +float parsefloat(uint8_t *buffer); +void printHex(const uint8_t * data, const uint32_t numBytes); + +// the packet buffer +extern uint8_t packetbuffer[]; + +char buf[60]; + +/**************************************************************************/ +/*! + @brief Sets up the HW an the BLE module (this function is called + automatically on startup) +*/ +/**************************************************************************/ +void setup(void) +{ + Serial.begin(9600); + + AFMS.begin(); // create with the default frequency 1.6KHz + + // turn on motors + L_MOTOR->setSpeed(0); + L_MOTOR->run(RELEASE); + + R_MOTOR->setSpeed(0); + R_MOTOR->run(RELEASE); + + Serial.begin(115200); + Serial.println(F("Adafruit Bluefruit Robot Controller Example")); + Serial.println(F("-----------------------------------------")); + + /* Initialize the module */ + BLEsetup(); + + +} + +int velocity = 0; + +float x, y; + +int L_restrict = 0; +int R_restrict = 0; + +unsigned long lastAccelPacket = 0; + +bool modeToggle = false; + +void loop(void) +{ + // read new packet data + uint8_t len = readPacket(&ble, BLE_READPACKET_TIMEOUT); + // if (len == 0) return; + + // Read from Accelerometer input + if( accelMode() ) { + lastAccelPacket = millis(); + modeToggle = true; + return; + } + + // Stop motors if accelerometer data is turned off (100ms timeout) + if( millis() - lastAccelPacket > 100 & modeToggle) { + L_MOTOR->run(RELEASE); + R_MOTOR->run(RELEASE); + modeToggle = false; + return; + } + + //if no accelerometer, use control pad + if( !modeToggle ) buttonMode(); + +} + + +bool accelMode(){ + if (packetbuffer[1] == 'A') { + x = parsefloat( packetbuffer + 2 ); + y = parsefloat( packetbuffer + 6 ); + + if( x <= -0.55 ){ + x += 0.55; + x *= -100.0; + L_MOTOR->run( BACKWARD ); + R_MOTOR->run( BACKWARD ); + if( x >= 45 ) x = 45; + if( x <= 0 ) x = 0; + velocity = map( x, 0, 45, 0 ,255 ); + } + else if( x >= -0.25 ){ + x+= 0.25; + x *= 100; + L_MOTOR->run( FORWARD ); + R_MOTOR->run( FORWARD ); + if( x>= 45 ) x = 45; + if( x<= 0 ) x = 0; + velocity = map( x, 0, 45, 0, 255 ); + } + else{ + L_MOTOR->run( RELEASE ); + R_MOTOR->run( RELEASE ); + velocity = 0; + } + + //account for L / R accel + + if( y >= 0.1 ){ + y -= 0.1; + y *= 100; + if( y >= 50 ) y = 50; + if( y <= 0 ) y = 0; + + L_restrict = fscale( y, 0.0, 50.0, 0.0, 100.0, -4.0 ); + } + else if( y <= -0.1 ){ + y += 0.1; + y *= -100; + if( y>= 50 ) y = 50; + if( y<= 0 ) y = 0; + + R_restrict = fscale( y, 0.0, 50.0, 0.0, 100.0, -4.0 ); + } + else{ + L_restrict = 0; + R_restrict = 0; + } + + float Lpercent = ( 100.0 - L_restrict ) / 100.00 ; + float Rpercent = ( 100.0 - R_restrict ) / 100.00 ; + + // Serial.print( x ); + // Serial.print( "\t" ); + // Serial.print( Lpercent ); + // Serial.print( "\t" ); + // Serial.print( velocity ); + // Serial.print( "\t" ); + // Serial.println( Rpercent ); + + L_MOTOR->setSpeed( velocity * Lpercent ); + R_MOTOR->setSpeed( velocity * Rpercent ); + + return true; + } + return false; +} + +bool isMoving = false; + +bool buttonMode(){ + + static unsigned long lastPress = 0; + + + // Buttons + if (packetbuffer[1] == 'B') { + + uint8_t buttnum = packetbuffer[2] - '0'; + boolean pressed = packetbuffer[3] - '0'; + + // Serial.println(buttnum); + + Serial.println(isMoving); + if (pressed) { + isMoving = true; + if(buttnum == 5){ + L_MOTOR->run(FORWARD); + R_MOTOR->run(FORWARD); + } + if(buttnum == 6){ + L_MOTOR->run(BACKWARD); + R_MOTOR->run(BACKWARD); + } + if(buttnum == 7){ + L_MOTOR->run(RELEASE); + R_MOTOR->run(FORWARD); + } + if(buttnum == 8){ + L_MOTOR->run(FORWARD); + R_MOTOR->run(RELEASE); + } + + lastPress = millis(); + + L_MOTOR->setSpeed(255); + R_MOTOR->setSpeed(255); + } + + else { + isMoving = false; + L_MOTOR->run(RELEASE); + R_MOTOR->run(RELEASE); + } + return true; + } + // if(isMoving){ + + // unsigned long timeSincePress = millis() - lastPress; + + // if(timeSincePress <= accelTime){ + + // Serial.println( timeSincePress ) ; + + // int motorSpeed = map( timeSincePress, 0, accelTime, 0, 255 ); + + // L_MOTOR->setSpeed(motorSpeed); + // R_MOTOR->setSpeed(motorSpeed); + // } + + // else{ + // // full speed ahead! + // L_MOTOR->setSpeed(255); + // R_MOTOR->setSpeed(255); + // } + // } + + return false; + +} + +void BLEsetup(){ + Serial.print(F("Initialising the Bluefruit LE module: ")); + + if ( !ble.begin(VERBOSE_MODE) ) + { + error(F("Couldn't find Bluefruit, make sure it's in CoMmanD mode & check wiring?")); + } + Serial.println( F("OK!") ); + + /* Perform a factory reset to make sure everything is in a known state */ + Serial.println(F("Performing a factory reset: ")); + if (! ble.factoryReset() ){ + error(F("Couldn't factory reset")); + } + + //Convert the name change command to a char array + BROADCAST_CMD.toCharArray(buf, 60); + + //Change the broadcast device name here! + if(ble.sendCommandCheckOK(buf)){ + Serial.println("name changed"); + } + delay(250); + + //reset to take effect + if(ble.sendCommandCheckOK("ATZ")){ + Serial.println("resetting"); + } + delay(250); + + //Confirm name change + ble.sendCommandCheckOK("AT+GAPDEVNAME"); + + /* Disable command echo from Bluefruit */ + ble.echo(false); + + Serial.println("Requesting Bluefruit info:"); + /* Print Bluefruit information */ + ble.info(); + + Serial.println(F("Please use Adafruit Bluefruit LE app to connect in Controller mode")); + Serial.println(F("Then activate/use the sensors, color picker, game controller, etc!")); + Serial.println(); + + ble.verbose(false); // debug info is a little annoying after this point! + + /* Wait for connection */ + while (! ble.isConnected()) { + delay(500); + } + + Serial.println(F("*****************")); + + // Set Bluefruit to DATA mode + Serial.println( F("Switching to DATA mode!") ); + ble.setMode(BLUEFRUIT_MODE_DATA); + + Serial.println(F("*****************")); +} + +//Logarithmic mapping function from http://playground.arduino.cc/Main/Fscale +float fscale( float inputValue, float originalMin, float originalMax, float newBegin, float newEnd, float curve){ + + float OriginalRange = 0; + float NewRange = 0; + float zeroRefCurVal = 0; + float normalizedCurVal = 0; + float rangedValue = 0; + boolean invFlag = 0; + + + // condition curve parameter + // limit range + + if (curve > 10) curve = 10; + if (curve < -10) curve = -10; + + curve = (curve * -.1) ; // - invert and scale - this seems more intuitive - postive numbers give more weight to high end on output + curve = pow(10, curve); // convert linear scale into lograthimic exponent for other pow function + + /* + Serial.println(curve * 100, DEC); // multply by 100 to preserve resolution + Serial.println(); + */ + + // Check for out of range inputValues + if (inputValue < originalMin) { + inputValue = originalMin; + } + if (inputValue > originalMax) { + inputValue = originalMax; + } + + // Zero Refference the values + OriginalRange = originalMax - originalMin; + + if (newEnd > newBegin){ + NewRange = newEnd - newBegin; + } + else + { + NewRange = newBegin - newEnd; + invFlag = 1; + } + + zeroRefCurVal = inputValue - originalMin; + normalizedCurVal = zeroRefCurVal / OriginalRange; // normalize to 0 - 1 float + + /* + Serial.print(OriginalRange, DEC); + Serial.print(" "); + Serial.print(NewRange, DEC); + Serial.print(" "); + Serial.println(zeroRefCurVal, DEC); + Serial.println(); + */ + + // Check for originalMin > originalMax - the math for all other cases i.e. negative numbers seems to work out fine + if (originalMin > originalMax ) { + return 0; + } + + if (invFlag == 0){ + rangedValue = (pow(normalizedCurVal, curve) * NewRange) + newBegin; + + } + else // invert the ranges + { + rangedValue = newBegin - (pow(normalizedCurVal, curve) * NewRange); + } + + return rangedValue; +} + diff --git a/Ada_BLE_RC/BluefruitConfig.h b/Ada_BLE_RC/BluefruitConfig.h new file mode 100644 index 000000000..57efc594d --- /dev/null +++ b/Ada_BLE_RC/BluefruitConfig.h @@ -0,0 +1,57 @@ +// COMMON SETTINGS +// ---------------------------------------------------------------------------------------------- +// These settings are used in both SW UART, HW UART and SPI mode +// ---------------------------------------------------------------------------------------------- +#define BUFSIZE 128 // Size of the read buffer for incoming data +#define VERBOSE_MODE true // If set to 'true' enables debug output +#define BLE_READPACKET_TIMEOUT 500 // Timeout in ms waiting to read a response + + +// SOFTWARE UART SETTINGS +// ---------------------------------------------------------------------------------------------- +// The following macros declare the pins that will be used for 'SW' serial. +// You should use this option if you are connecting the UART Friend to an UNO +// ---------------------------------------------------------------------------------------------- +#define BLUEFRUIT_SWUART_RXD_PIN 9 // Required for software serial! +#define BLUEFRUIT_SWUART_TXD_PIN 10 // Required for software serial! +#define BLUEFRUIT_UART_CTS_PIN 11 // Required for software serial! +#define BLUEFRUIT_UART_RTS_PIN -1 // Optional, set to -1 if unused + + +// HARDWARE UART SETTINGS +// ---------------------------------------------------------------------------------------------- +// The following macros declare the HW serial port you are using. Uncomment +// this line if you are connecting the BLE to Leonardo/Micro or Flora +// ---------------------------------------------------------------------------------------------- +#ifdef Serial1 // this makes it not complain on compilation if there's no Serial1 + #define BLUEFRUIT_HWSERIAL_NAME Serial1 +#endif + + +// SHARED UART SETTINGS +// ---------------------------------------------------------------------------------------------- +// The following sets the optional Mode pin, its recommended but not required +// ---------------------------------------------------------------------------------------------- +#define BLUEFRUIT_UART_MODE_PIN 12 // Set to -1 if unused + + +// SHARED SPI SETTINGS +// ---------------------------------------------------------------------------------------------- +// The following macros declare the pins to use for HW and SW SPI communication. +// SCK, MISO and MOSI should be connected to the HW SPI pins on the Uno when +// using HW SPI. This should be used with nRF51822 based Bluefruit LE modules +// that use SPI (Bluefruit LE SPI Friend). +// ---------------------------------------------------------------------------------------------- +#define BLUEFRUIT_SPI_CS 8 +#define BLUEFRUIT_SPI_IRQ 7 +#define BLUEFRUIT_SPI_RST 6 // Optional but recommended, set to -1 if unused + +// SOFTWARE SPI SETTINGS +// ---------------------------------------------------------------------------------------------- +// The following macros declare the pins to use for SW SPI communication. +// This should be used with nRF51822 based Bluefruit LE modules that use SPI +// (Bluefruit LE SPI Friend). +// ---------------------------------------------------------------------------------------------- +#define BLUEFRUIT_SPI_SCK 13 +#define BLUEFRUIT_SPI_MISO 12 +#define BLUEFRUIT_SPI_MOSI 11 diff --git a/Ada_BLE_RC/README.md b/Ada_BLE_RC/README.md new file mode 100644 index 000000000..655e5eb27 --- /dev/null +++ b/Ada_BLE_RC/README.md @@ -0,0 +1,2 @@ +## Adafruit Bluefruit LE Feather Robot Rover +Build & control an RC car with a smart phone! Follow the Adafruit learn guide here: https://learn.adafruit.com/bluefruit-feather-robot ! diff --git a/Ada_BLE_RC/packetParser.cpp b/Ada_BLE_RC/packetParser.cpp new file mode 100644 index 000000000..aa0201bef --- /dev/null +++ b/Ada_BLE_RC/packetParser.cpp @@ -0,0 +1,138 @@ +#include +#include +#include +#include + +#include "Adafruit_BLE.h" +#include "Adafruit_BluefruitLE_SPI.h" +#include "Adafruit_BluefruitLE_UART.h" + + +#define PACKET_ACC_LEN (15) +#define PACKET_GYRO_LEN (15) +#define PACKET_MAG_LEN (15) +#define PACKET_QUAT_LEN (19) +#define PACKET_BUTTON_LEN (5) +#define PACKET_COLOR_LEN (6) +#define PACKET_LOCATION_LEN (15) + +// READ_BUFSIZE Size of the read buffer for incoming packets +#define READ_BUFSIZE (20) + + +/* Buffer to hold incoming characters */ +uint8_t packetbuffer[READ_BUFSIZE+1]; + +/**************************************************************************/ +/*! + @brief Casts the four bytes at the specified address to a float +*/ +/**************************************************************************/ +float parsefloat(uint8_t *buffer) +{ + float f = ((float *)buffer)[0]; + return f; +} + +/**************************************************************************/ +/*! + @brief Prints a hexadecimal value in plain characters + @param data Pointer to the byte data + @param numBytes Data length in bytes +*/ +/**************************************************************************/ +void printHex(const uint8_t * data, const uint32_t numBytes) +{ + uint32_t szPos; + for (szPos=0; szPos < numBytes; szPos++) + { + Serial.print(F("0x")); + // Append leading 0 for small values + if (data[szPos] <= 0xF) + { + Serial.print(F("0")); + Serial.print(data[szPos] & 0xf, HEX); + } + else + { + Serial.print(data[szPos] & 0xff, HEX); + } + // Add a trailing space if appropriate + if ((numBytes > 1) && (szPos != numBytes - 1)) + { + Serial.print(F(" ")); + } + } + Serial.println(); +} + +/**************************************************************************/ +/*! + @brief Waits for incoming data and parses it +*/ +/**************************************************************************/ +uint8_t readPacket(Adafruit_BLE *ble, uint16_t timeout) +{ + uint16_t origtimeout = timeout, replyidx = 0; + + memset(packetbuffer, 0, READ_BUFSIZE); + + while (timeout--) { + if (replyidx >= 20) break; + if ((packetbuffer[1] == 'A') && (replyidx == PACKET_ACC_LEN)) + break; + if ((packetbuffer[1] == 'G') && (replyidx == PACKET_GYRO_LEN)) + break; + if ((packetbuffer[1] == 'M') && (replyidx == PACKET_MAG_LEN)) + break; + if ((packetbuffer[1] == 'Q') && (replyidx == PACKET_QUAT_LEN)) + break; + if ((packetbuffer[1] == 'B') && (replyidx == PACKET_BUTTON_LEN)) + break; + if ((packetbuffer[1] == 'C') && (replyidx == PACKET_COLOR_LEN)) + break; + if ((packetbuffer[1] == 'L') && (replyidx == PACKET_LOCATION_LEN)) + break; + + while (ble->available()) { + char c = ble->read(); + if (c == '!') { + replyidx = 0; + } + packetbuffer[replyidx] = c; + replyidx++; + timeout = origtimeout; + } + + if (timeout == 0) break; + delay(1); + } + + packetbuffer[replyidx] = 0; // null term + + if (!replyidx) // no data or timeout + return 0; + if (packetbuffer[0] != '!') // doesn't start with '!' packet beginning + return 0; + + // check checksum! + uint8_t xsum = 0; + uint8_t checksum = packetbuffer[replyidx-1]; + + for (uint8_t i=0; i