This commit is contained in:
ladyada
parent
3e904801bc
commit
626df13dd2
4 changed files with 610 additions and 0 deletions
413
Ada_BLE_RC/Ada_BLE_RC.ino
Normal file
413
Ada_BLE_RC/Ada_BLE_RC.ino
Normal file
|
|
@ -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 <string.h>
|
||||
#include <Arduino.h>
|
||||
#include <SPI.h>
|
||||
#if not defined (_VARIANT_ARDUINO_DUE_X_)
|
||||
#include <SoftwareSerial.h>
|
||||
#endif
|
||||
|
||||
#include "Adafruit_BLE.h"
|
||||
#include "Adafruit_BluefruitLE_SPI.h"
|
||||
#include "Adafruit_BluefruitLE_UART.h"
|
||||
|
||||
#include "BluefruitConfig.h"
|
||||
|
||||
#include <Wire.h>
|
||||
#include <Adafruit_MotorShield.h>
|
||||
// #include "utility/Adafruit_PWMServoDriver.h"
|
||||
// #include <Servo.h>
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
57
Ada_BLE_RC/BluefruitConfig.h
Normal file
57
Ada_BLE_RC/BluefruitConfig.h
Normal file
|
|
@ -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
|
||||
2
Ada_BLE_RC/README.md
Normal file
2
Ada_BLE_RC/README.md
Normal file
|
|
@ -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 !
|
||||
138
Ada_BLE_RC/packetParser.cpp
Normal file
138
Ada_BLE_RC/packetParser.cpp
Normal file
|
|
@ -0,0 +1,138 @@
|
|||
#include <string.h>
|
||||
#include <Arduino.h>
|
||||
#include <SPI.h>
|
||||
#include <SoftwareSerial.h>
|
||||
|
||||
#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<replyidx-1; i++) {
|
||||
xsum += packetbuffer[i];
|
||||
}
|
||||
xsum = ~xsum;
|
||||
|
||||
// Throw an error message if the checksum's don't match
|
||||
if (xsum != checksum)
|
||||
{
|
||||
Serial.print("Checksum mismatch in packet : ");
|
||||
printHex(packetbuffer, replyidx+1);
|
||||
return 0;
|
||||
}
|
||||
|
||||
// checksum passed!
|
||||
return replyidx;
|
||||
}
|
||||
|
||||
Loading…
Reference in a new issue