├── .gitattributes ├── .gitmodules ├── library.properties ├── .travis.yml ├── examples ├── PWMsimple │ └── PWMsimple.ino ├── readSpeed │ └── readSpeed.ino └── scheduledMessages │ └── scheduledMessages.ino ├── src ├── protocolFunctions.h ├── protocolFunctions.c ├── HoverboardAPI.h └── HoverboardAPI.cpp ├── LICENSE ├── scripts └── update_arduino_branch.sh ├── README.md └── keywords.txt /.gitattributes: -------------------------------------------------------------------------------- 1 | scripts/* linguist-vendored -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "src/hbprotocol"] 2 | path = src/hbprotocol 3 | url = https://github.com/bipropellant/hbprotocol.git 4 | -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=HoverboardAPI 2 | version=0.1.1 3 | author=phail 4 | maintainer=phail 5 | sentence=Hoverboard API controller 6 | paragraph=A Hoverboard API controller Interface. 7 | category=Communication 8 | url=https://github.com/bipropellant/bipropellant-hoverboard-api 9 | architectures=* 10 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: c 2 | sudo: false 3 | cache: 4 | directories: 5 | - ~/arduino_ide 6 | - ~/.arduino15/packages/ 7 | git: 8 | depth: false 9 | quiet: true 10 | env: 11 | global: 12 | - PRETTYNAME="Bipropellant Hoverboard API" 13 | before_install: 14 | - source <(curl -SLs https://raw.githubusercontent.com/adafruit/travis-ci-arduino/master/install.sh) 15 | 16 | script: 17 | - build_io_platforms 18 | 19 | 20 | deploy: 21 | provider: script 22 | script: bash scripts/update_arduino_branch.sh 23 | on: 24 | branch: master 25 | repo: bipropellant/bipropellant-hoverboard-api 26 | -------------------------------------------------------------------------------- /examples/PWMsimple/PWMsimple.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Minimalistic example to to get the wheels turning. 3 | Look at HoverboardAPI.c to learn which other functions are available. 4 | 5 | Further information on https://github.com/bipropellant 6 | */ 7 | 8 | 9 | #include 10 | 11 | 12 | int serialWrapper(unsigned char *data, int len) { 13 | return (int) Serial1.write(data,len); 14 | } 15 | HoverboardAPI hoverboard = HoverboardAPI(serialWrapper); 16 | 17 | 18 | 19 | void setup() { 20 | Serial1.begin(115200); 21 | } 22 | 23 | void loop() { 24 | hoverboard.sendPWM(100, 30, PROTOCOL_SOM_NOACK); 25 | delay(30); 26 | } -------------------------------------------------------------------------------- /src/protocolFunctions.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "hbprotocol/protocol.h" 4 | #include 5 | 6 | #ifdef __cplusplus 7 | extern "C" { 8 | #endif 9 | 10 | //////////////////////////////////////////////////////////////////////////////////////////// 11 | // initialize protocol and register functions 12 | extern int setup_protocol(PROTOCOL_STAT *s); 13 | extern unsigned long latency; 14 | 15 | 16 | 17 | extern volatile PROTOCOL_ELECTRICAL_PARAMS electrical_measurements; 18 | extern volatile PROTOCOL_HALL_DATA_STRUCT HallData[2]; 19 | extern volatile PROTOCOL_SPEED_DATA SpeedData; 20 | 21 | 22 | void fn_SubscribeData ( PROTOCOL_STAT *s, PARAMSTAT *param, unsigned char cmd, PROTOCOL_MSG3full *msg ); 23 | 24 | 25 | #ifdef __cplusplus 26 | } 27 | #endif 28 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Bipropellant 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /scripts/update_arduino_branch.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | setup_git() { 4 | git config --global user.email "travis@travis-ci.org" 5 | git config --global user.name "Travis CI" 6 | } 7 | 8 | flatten_submodule() { 9 | git checkout -b master_arduino 10 | 11 | git rm --cached src/hbprotocol # delete reference to submodule HEAD (no trailing slash) 12 | git rm .gitmodules # if you have more than one submodules, 13 | # you need to edit this file instead of deleting! 14 | rm -rf src/hbprotocol/.git # make sure you have backup!! 15 | rm -rf src/hbprotocol/examples 16 | rm -rf src/hbprotocol/README.md 17 | rm -rf scripts 18 | rm -rf .travis.yml 19 | # mv src/hbprotocol/* src/ 20 | # rm -rf src/hbprotocol 21 | git add . 22 | # git mv src/* ./ 23 | 24 | git commit --message "Auto convert to Arduino Library ($TRAVIS_BUILD_NUMBER)" 25 | } 26 | 27 | upload_files() { 28 | git remote add upload https://${GITHUB_TOKEN}@github.com/bipropellant/bipropellant-hoverboard-api.git > /dev/null 2>&1 29 | git push --quiet --set-upstream upload master_arduino -f 30 | } 31 | 32 | setup_git 33 | flatten_submodule 34 | upload_files 35 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # bipropellant-hoverboard-api 2 | 3 | C++ Wrapper around the [bipropellant hoverboard UART protocol](https://github.com/bipropellant/bipropellant-protocol). 4 | An Arduino compatible branch is automatically generated: master_arduino. (This branch removes the submodule dependency.) 5 | 6 | Feel free to write Pull Requests for added features and examples. 7 | 8 | For now, only functions are implemented which were needed by the Author. Many more are possible. 9 | 10 | See https://github.com/bipropellant/bipropellant-protocol/blob/fe96935cb2f550110ffaaa6d74852bedd79e25c7/protocol.c#L266-L307 for possible information which could be accessed via this protocol. 11 | 12 | ## Implemented Features 13 | * Setting Motor PWM, also separately for left and right motor 14 | * Reading Motor Speeds 15 | * Setting Motor speeds, separately for left and right motor 16 | * Setting PID control values 17 | * Reading electrical Parameters (Voltage, Current..) 18 | * Subscribe to automatic updates of values 19 | * Schedule recurring transmission of values 20 | * Register callback function when values are received 21 | 22 | See https://github.com/bipropellant/bipropellant-hoverboard-api/blob/master/src/HoverboardAPI.cpp for detailed information of implemented functions. 23 | -------------------------------------------------------------------------------- /examples/readSpeed/readSpeed.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Read back speed. 3 | Look at HoverboardAPI.c to learn which other functions are available. 4 | 5 | Further information on https://github.com/bipropellant 6 | */ 7 | 8 | 9 | #include 10 | 11 | 12 | // Setup HoverboardAPI to use Serial1 13 | int serialWrapper(unsigned char *data, int len) { 14 | return (int) Serial1.write(data,len); 15 | } 16 | HoverboardAPI hoverboard = HoverboardAPI(serialWrapper); 17 | 18 | 19 | 20 | void setup() { 21 | Serial.begin(115200); // Serial for debugging 22 | Serial1.begin(115200); // Connection to hoverboard 23 | } 24 | 25 | void loop() { 26 | 27 | // Send PWM Value 100 with steer 30 to hoverboard. (7% duty cycle for wheel1, 13% duty cycle for wheel2) 28 | hoverboard.sendPWM(100, 30, PROTOCOL_SOM_NOACK); 29 | 30 | // Request read of Hall Data (=Motor Speed) 31 | hoverboard.requestRead(HoverboardAPI::Codes::sensHall, PROTOCOL_SOM_NOACK); 32 | 33 | // Print Speed on debug Serial. Since there was no delay between requesting and printing, this value is probably one cycle old. 34 | Serial.print("Motor Speed: "); 35 | Serial.print(hoverboard.getSpeed_kmh()); 36 | Serial.print("km/h (average from "); 37 | Serial.print(hoverboard.getSpeed0_mms()); 38 | Serial.print("mm/s and "); 39 | Serial.print(hoverboard.getSpeed1_mms()); 40 | Serial.println("mm/s)"); 41 | 42 | 43 | // Delay loop() for 30 ms use the time to send and receive UART protocol every 100 microseconds 44 | // This way messages which are received can be processed and scheduled messages can be sent. 45 | unsigned long start = millis(); 46 | while (millis() < start + 30){ 47 | 48 | // Read and Process Incoming data 49 | int i=0; 50 | while(Serial1.available() && i++ < 1024) { // read maximum 1024 bytes at once. 51 | hoverboard.protocolPush( Serial1.read() ); 52 | } 53 | 54 | // Send Messages from queue, re-send failed messages and process scheduled transmissions 55 | hoverboard.protocolTick(); 56 | delayMicroseconds(100); 57 | } 58 | 59 | } 60 | -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map For HoverboardAPI Library 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | 9 | HoverboardAPI KEYWORD1 10 | PARAMSTAT_FN KEYWORD1 11 | 12 | ####################################### 13 | # Methods and Functions (KEYWORD2) 14 | ####################################### 15 | 16 | protocolPush KEYWORD2 17 | protocolTick KEYWORD2 18 | updateParamHandler KEYWORD2 19 | updateParamVariable KEYWORD2 20 | scheduleTransmission KEYWORD2 21 | requestRead KEYWORD2 22 | scheduleRead KEYWORD2 23 | sendPWM KEYWORD2 24 | sendPWMData KEYWORD2 25 | sendEnable KEYWORD2 26 | sendBuzzer KEYWORD2 27 | sendCounterReset KEYWORD2 28 | getBatteryVoltage KEYWORD2 29 | getMotorAmpsAvg KEYWORD2 30 | getTxBufferLevel KEYWORD2 31 | getSpeed_kmh KEYWORD2 32 | getSteer_kmh KEYWORD2 33 | getSpeed_mms KEYWORD2 34 | getSteer_mms KEYWORD2 35 | getSpeed0_kmh KEYWORD2 36 | getSpeed1_kmh KEYWORD2 37 | getSpeed0_mms KEYWORD2 38 | getSpeed1_mms KEYWORD2 39 | resetCounters KEYWORD2 40 | printStats KEYWORD2 41 | 42 | ####################################### 43 | # Constants (LITERAL1) 44 | ####################################### 45 | 46 | Codes::protocolVersion LITERAL1 47 | Codes::protocolSubscriptions LITERAL1 48 | Codes::protocolCountSum LITERAL1 49 | Codes::protocolCountACK LITERAL1 50 | Codes::protocolCountnoACK LITERAL1 51 | Codes::sensHall LITERAL1 52 | Codes::sensElectrical LITERAL1 53 | Codes::enableMotors LITERAL1 54 | Codes::disablePoweroff LITERAL1 55 | Codes::debugOut LITERAL1 56 | Codes::setPointPWMData LITERAL1 57 | Codes::setPointPWM LITERAL1 58 | Codes::setBuzzer LITERAL1 59 | -------------------------------------------------------------------------------- /src/protocolFunctions.c: -------------------------------------------------------------------------------- 1 | #include "hbprotocol/protocol.h" 2 | #include 3 | #include 4 | 5 | extern unsigned long millis(void); 6 | 7 | //////////////////////////////////////////////////////////////////////////////////////////// 8 | // Variable & Functions for 0x02 hall data 9 | 10 | volatile PROTOCOL_HALL_DATA_STRUCT HallData[2]; 11 | 12 | 13 | //////////////////////////////////////////////////////////////////////////////////////////// 14 | // Variable & Functions for 0x08 electrical_measurements 15 | 16 | volatile PROTOCOL_ELECTRICAL_PARAMS electrical_measurements; 17 | 18 | 19 | //////////////////////////////////////////////////////////////////////////////////////////// 20 | // Variable & Functions for 0x03 speed data 21 | 22 | volatile PROTOCOL_SPEED_DATA SpeedData; 23 | 24 | 25 | //////////////////////////////////////////////////////////////////////////////////////////// 26 | // Variable & Functions for 0x27 Ping 27 | // Sends back the received Message as a readresponse. (Sender can send a timestamp to measure latency) 28 | unsigned long latency = 0; 29 | 30 | void fn_customPing ( PROTOCOL_STAT *s, PARAMSTAT *param, unsigned char cmd, PROTOCOL_MSG3full *msg ) { 31 | 32 | // Keep functionality to reply to pings. 33 | fn_ping( s, param, cmd, msg); 34 | 35 | switch (cmd) { 36 | 37 | case PROTOCOL_CMD_READVALRESPONSE: 38 | { 39 | if(msg) { 40 | unsigned long timeSent; 41 | memcpy( &timeSent, msg->content, sizeof(timeSent)); 42 | 43 | latency = millis() - timeSent; 44 | } 45 | } 46 | } 47 | } 48 | 49 | //////////////////////////////////////////////////////////////////////////////////////////// 50 | // Variable & Functions for 0x__ Paddelparameters 51 | 52 | PARAMSTAT paddelParameters = 53 | { 0x70, "PaddelParametes", NULL, UI_NONE, NULL, 0, fn_defaultProcessing }; 54 | 55 | //////////////////////////////////////////////////////////////////////////////////////////// 56 | // initialize protocol and register functions 57 | int setup_protocol( PROTOCOL_STAT *s ) { 58 | 59 | int errors = 0; 60 | 61 | errors += setParamVariable( s, 0x08, UI_NONE, (void *)&electrical_measurements, sizeof(PROTOCOL_ELECTRICAL_PARAMS)); 62 | setParamHandler(s, 0x08, fn_defaultProcessing); 63 | 64 | errors += setParamVariable( s, 0x02, UI_NONE, (void *)&HallData, sizeof(HallData)); 65 | setParamHandler(s, 0x02, fn_defaultProcessing); 66 | 67 | errors += setParamVariable( s, 0x03, UI_NONE, (void *)&SpeedData, sizeof(SpeedData)); 68 | setParamHandler(s, 0x03, fn_defaultProcessing); 69 | 70 | setParamHandler(s, 0x27, fn_customPing); 71 | 72 | setParam(s, &paddelParameters); 73 | 74 | return errors; 75 | 76 | } 77 | -------------------------------------------------------------------------------- /examples/scheduledMessages/scheduledMessages.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Scheduled transmission and reading of data. 3 | Look at HoverboardAPI.c to learn which other functions are available. 4 | 5 | Further information on https://github.com/bipropellant 6 | */ 7 | 8 | 9 | #include 10 | 11 | 12 | // Setup HoverboardAPI to use Serial1 13 | int serialWrapper(unsigned char *data, int len) { 14 | return (int) Serial1.write(data,len); 15 | } 16 | HoverboardAPI hoverboard = HoverboardAPI(serialWrapper); 17 | 18 | // Variable for PWM 19 | PROTOCOL_PWM_DATA PWMData; 20 | PROTOCOL_BUZZER_DATA buzzer = {8, 0, 50}; 21 | 22 | void setup() { 23 | Serial.begin(115200); // Serial for debugging 24 | Serial1.begin(115200); // Connection to hoverboard 25 | 26 | // Request Hall sensor data every 100ms 27 | hoverboard.scheduleRead(HoverboardAPI::Codes::sensHall, -1, 100); 28 | 29 | // Request Electrical Information every second 30 | hoverboard.scheduleRead(hoverboard.Codes::sensElectrical, -1, 1000); 31 | 32 | // Register Variable and send PWM values periodically 33 | hoverboard.updateParamVariable(HoverboardAPI::Codes::setPointPWM, &PWMData, sizeof(PWMData)); 34 | hoverboard.scheduleTransmission(HoverboardAPI::Codes::setPointPWM, -1, 30); 35 | 36 | // Send PWM to 10% duty cycle for wheel1, 15% duty cycle for wheel2. Wheel2 is running backwards. 37 | PWMData.pwm[0] = 100.0; 38 | PWMData.pwm[1] = -150.0; 39 | 40 | // Register Variable and send Buzzer values periodically 41 | hoverboard.updateParamVariable(HoverboardAPI::Codes::setBuzzer, &buzzer, sizeof(buzzer)); 42 | hoverboard.scheduleTransmission(HoverboardAPI::Codes::setBuzzer, -1, 200); 43 | 44 | // Set maxium PWM to 400, Minimum to -400 and threshold to 30. Require ACK (Message will be resend when not Acknowledged) 45 | hoverboard.sendPWMData(0, 0, 400, -400, 30, PROTOCOL_SOM_ACK); 46 | 47 | } 48 | 49 | void loop() { 50 | 51 | 52 | // Print Speed on debug Serial. 53 | Serial.print("Motor Speed: "); 54 | Serial.print(hoverboard.getSpeed_kmh()); 55 | Serial.println("km/h (average wheel1 and wheel2"); 56 | 57 | // Print battery Voltage on debug Serial. 58 | Serial.print("Battery Voltage: "); 59 | Serial.print(hoverboard.getBatteryVoltage()); 60 | Serial.println("V"); 61 | 62 | // Create annoying melody 63 | buzzer.buzzerFreq++; 64 | 65 | 66 | // Delay loop() for 300 ms use the time to send and receive UART protocol every 100 microseconds 67 | // This way messages which are received can be processed and scheduled messages can be sent. 68 | unsigned long start = millis(); 69 | while (millis() < start + 300){ 70 | 71 | // Read and Process Incoming data 72 | int i=0; 73 | while(Serial1.available() && i++ < 1024) { // read maximum 1024 bytes at once. 74 | hoverboard.protocolPush( Serial1.read() ); 75 | } 76 | 77 | // Send Messages from queue, re-send failed messages and process scheduled transmissions 78 | hoverboard.protocolTick(); 79 | delayMicroseconds(100); 80 | } 81 | 82 | } -------------------------------------------------------------------------------- /src/HoverboardAPI.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "hbprotocol/protocol.h" 4 | #ifdef ARDUINO 5 | #include "Stream.h" 6 | #endif 7 | 8 | class HoverboardAPI 9 | { 10 | public: 11 | 12 | enum Codes { 13 | protocolVersion = 0xFE, 14 | protocolSubscriptions = 0x22, 15 | protocolCountSum = 0x23, 16 | protocolCountACK = 0x24, 17 | protocolCountnoACK = 0x25, 18 | text = 0x26, 19 | ping = 0x27, 20 | sensHall = 0x02, 21 | setSpeed = 0x03, 22 | sensElectrical = 0x08, 23 | enableMotors = 0x09, 24 | disablePoweroff = 0x0A, 25 | debugOut = 0x0B, 26 | setPointPWMData = 0x0D, 27 | setPointPWM = 0x0E, 28 | setBuzzer = 0x21, 29 | setSpeedKp = 0x85, 30 | setSpeedKi = 0x86, 31 | setSpeedKd = 0x87, 32 | setSpeedIncrLimit = 0x88, 33 | adcSettings = 0x90, 34 | flashMagic = 0x80, 35 | paddleParameters = 0x80 36 | }; 37 | 38 | //commonly used functions ************************************************************************** 39 | HoverboardAPI(); // * constructor. 40 | HoverboardAPI(int (*send_serial_data)( unsigned char *data, int len )); // * constructor. 41 | void setSendSerialData(int (*send_serial_data)( unsigned char *data, int len )); 42 | 43 | 44 | void protocolPush(unsigned char byte); 45 | void protocolTick(); 46 | PARAMSTAT_FN updateParamHandler(Codes code, PARAMSTAT_FN callback); 47 | int updateParamVariable(Codes code, void *ptr, int len); 48 | 49 | void scheduleTransmission(Codes code, int count, unsigned int period, char som = PROTOCOL_SOM_NOACK); 50 | 51 | void requestRead(Codes code, char som = PROTOCOL_SOM_NOACK); 52 | void scheduleRead(Codes code, int count, unsigned int period, char som = PROTOCOL_SOM_NOACK); 53 | 54 | 55 | void sendPWM(int16_t pwm, int16_t steer = 0, char som = PROTOCOL_SOM_NOACK); 56 | void sendPing(char som = PROTOCOL_SOM_ACK); 57 | int sendText(char *message, unsigned char som = PROTOCOL_SOM_ACK); 58 | void receiveText(char *message); 59 | 60 | void sendDifferentialPWM(int16_t left_cmd, int16_t right_cmd, char som = PROTOCOL_SOM_NOACK); 61 | void sendPWMData(int16_t pwm, int16_t steer = 0, int speed_max_power = 600, int speed_min_power = -600, int speed_minimum_pwm = 10, char som = PROTOCOL_SOM_ACK); 62 | void sendSpeedData(double left_speed, double right_speed, int16_t max_power, int16_t min_speed, char som = PROTOCOL_SOM_NOACK); 63 | void sendPIDControl(int16_t Kp, int16_t Ki, int16_t Kd, int16_t speed_increment, char som = PROTOCOL_SOM_NOACK); 64 | void sendRawData(unsigned char cmd, unsigned char code, unsigned char *content, unsigned char len, unsigned char som); 65 | void protocolPost(PROTOCOL_MSG3full *msg); 66 | 67 | 68 | void sendEnable(uint8_t newEnable, char som = PROTOCOL_SOM_ACK); 69 | void sendBuzzer(uint8_t buzzerFreq = 4, uint8_t buzzerPattern = 0, uint16_t buzzerLen = 100, char som = PROTOCOL_SOM_NOACK); 70 | void sendCounterReset(char som = PROTOCOL_SOM_ACK); 71 | 72 | float getBatteryVoltage(); 73 | float getMotorAmpsAvg(uint8_t motor); 74 | int getTxBufferLevel(); 75 | double getSpeed_kmh(); 76 | double getSteer_kmh(); 77 | double getSpeed_mms(); 78 | double getSteer_mms(); 79 | double getSpeed0_kmh(); 80 | double getSpeed1_kmh(); 81 | double getSpeed0_mms(); 82 | double getSpeed1_mms(); 83 | 84 | double getPosition0_mm(); 85 | double getPosition1_mm(); 86 | 87 | void resetCounters(); 88 | void printStats(); 89 | 90 | PROTOCOL_STAT s; 91 | 92 | }; 93 | 94 | -------------------------------------------------------------------------------- /src/HoverboardAPI.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************************** 2 | * HoverboardAPI Library - Version 0.0.3 3 | **********************************************************************************************/ 4 | 5 | #include "HoverboardAPI.h" 6 | #include "hbprotocol/protocol.h" 7 | #include "hbprotocol/protocol_private.h" 8 | #include "protocolFunctions.h" 9 | #include 10 | #include 11 | 12 | 13 | 14 | /*Constructor (...)********************************************************* 15 | * Hand over function pointer where to send data. 16 | * 17 | * Arduino example: 18 | * 19 | * int serialWrapper(unsigned char *data, int len) 20 | * { 21 | * return (int) Serial.write(data,len); 22 | * } 23 | * HoverboardAPI hoverboard = HoverboardAPI(serialWrapper); 24 | * 25 | ***************************************************************************/ 26 | 27 | #ifdef ARDUINO 28 | extern "C" 29 | { 30 | extern void delay(uint32_t ms); 31 | extern unsigned long millis(void); 32 | } 33 | #else 34 | #include 35 | #include 36 | 37 | void delay(uint32_t ms) { 38 | usleep (ms*1000); 39 | } 40 | 41 | static uint64_t ts_start = 0; 42 | 43 | unsigned long millis() 44 | { 45 | struct timespec ts; 46 | 47 | if (ts_start == 0) 48 | { 49 | clock_gettime(CLOCK_MONOTONIC_RAW, &ts); 50 | ts_start = (uint64_t)ts.tv_sec * (uint64_t)1000 + (uint64_t)(ts.tv_nsec / 1000000L) ; 51 | } 52 | 53 | clock_gettime(CLOCK_MONOTONIC_RAW, &ts); 54 | uint64_t ts_now = (uint64_t)ts.tv_sec * (uint64_t)1000 + (uint64_t)(ts.tv_nsec / 1000000L) ; 55 | 56 | return (uint32_t)(ts_now - ts_start); 57 | } 58 | #endif 59 | 60 | uint32_t tickWrapper(void) 61 | { 62 | return (uint32_t) millis(); 63 | } 64 | 65 | HoverboardAPI::HoverboardAPI(int (*send_serial_data)( unsigned char *data, int len )) 66 | { 67 | HoverboardAPI(); 68 | setSendSerialData(send_serial_data); 69 | } 70 | 71 | void HoverboardAPI::setSendSerialData(int (*send_serial_data)( unsigned char *data, int len )) 72 | { 73 | s.send_serial_data = send_serial_data; 74 | s.send_serial_data_wait = send_serial_data; 75 | } 76 | 77 | HoverboardAPI::HoverboardAPI() 78 | { 79 | if(protocol_init(&s) != 0) while(1) {}; 80 | setup_protocol(&s); 81 | s.allow_ascii = 0; // do not allow ASCII parsing. 82 | protocol_GetTick = tickWrapper; 83 | protocol_Delay = delay; 84 | } 85 | 86 | /*************************************************************************** 87 | * Input function. Feed with Serial.read(). 88 | ***************************************************************************/ 89 | void HoverboardAPI::protocolPush(unsigned char byte) 90 | { 91 | protocol_byte(&s, byte); 92 | } 93 | 94 | /*************************************************************************** 95 | * Triggers message sending from Buffer and scheduled messages. 96 | ***************************************************************************/ 97 | void HoverboardAPI::protocolTick() 98 | { 99 | protocol_tick(&s); 100 | } 101 | 102 | /*************************************************************************** 103 | * Returns local TX Buffer level 104 | ***************************************************************************/ 105 | int HoverboardAPI::getTxBufferLevel() 106 | { 107 | return (mpTxQueued(&s.ack.TxBuffer) + mpTxQueued(&s.noack.TxBuffer)); 108 | } 109 | 110 | /*************************************************************************** 111 | * Sets a callback Function to handle Protocol Read or Write events 112 | ***************************************************************************/ 113 | PARAMSTAT_FN HoverboardAPI::updateParamHandler(Codes code, PARAMSTAT_FN callback) 114 | { 115 | PARAMSTAT_FN old = getParamHandler(&s, code); 116 | setParamHandler(&s, code, callback); 117 | return old; 118 | } 119 | 120 | /*************************************************************************** 121 | * Sets a Variable into which data is stored on receive and taken from for 122 | * scheduled sending. 123 | ***************************************************************************/ 124 | int HoverboardAPI::updateParamVariable(Codes code, void *ptr, int len) 125 | { 126 | if(s.params[code] != NULL) 127 | { 128 | return setParamVariable(&s, code, s.params[code]->ui_type, ptr, len); 129 | } 130 | return 1; 131 | } 132 | 133 | /*************************************************************************** 134 | * Print Protocol Statistics. Remote Data has to be requested first. 135 | ***************************************************************************/ 136 | void HoverboardAPI::printStats() 137 | { 138 | // char buffer [100]; 139 | extern PROTOCOLCOUNT ProtocolcountData; 140 | 141 | // /* 142 | // snprintf ( buffer, 100, "ACK RX: %4li TX: %4li RXmissing: %4li TXretries: %4i ", s.ack.counters.rx, s.ack.counters.tx, s.ack.counters.rxMissing, s.ack.counters.txRetries); 143 | // port.print(buffer); 144 | // snprintf ( buffer, 100, "NOACK RX: %4li TX: %4li RXmissing: %4li TXretries: %4i ", s.noack.counters.rx, s.noack.counters.tx, s.noack.counters.rxMissing, s.noack.counters.txRetries); 145 | // port.print(buffer); 146 | // snprintf ( buffer, 100, "Received RX: %4li TX: %4li RXmissing: %4li TXretries: %4i ", ProtocolcountData.rx, ProtocolcountData.tx, ProtocolcountData.rxMissing, ProtocolcountData.txRetries); 147 | // port.print(buffer); 148 | // */ 149 | 150 | // printf ( "Local RX: %4d TX: %4d RXmissing: %4d ", s.ack.counters.rx + s.noack.counters.rx, s.ack.counters.tx + s.noack.counters.tx, s.ack.counters.rxMissing + s.noack.counters.rxMissing); 151 | // printf ( "Remote RX: %4d TX: %4d RXmissing: %4d ", ProtocolcountData.rx, ProtocolcountData.tx, ProtocolcountData.rxMissing); 152 | // printf ( "Missed Local->Remote %4d (%4d) Remote->Local %4d (%4d)", s.ack.counters.tx + s.noack.counters.tx - ProtocolcountData.rx, ProtocolcountData.rxMissing, ProtocolcountData.tx - s.ack.counters.rx - s.noack.counters.rx, s.ack.counters.rxMissing + s.noack.counters.rxMissing); 153 | // printf("\n"); 154 | } 155 | 156 | /*************************************************************************** 157 | * Triggers a readout of data from the hoverboard 158 | * It is necessary to set a callback if something should happen when the 159 | * data arrives. Otherwise the data can just be read from the variable. 160 | ***************************************************************************/ 161 | void HoverboardAPI::requestRead(Codes code, char som) 162 | { 163 | sendRawData( 164 | PROTOCOL_CMD_READVAL, 165 | code, 166 | NULL, 167 | 0, 168 | som 169 | ); 170 | } 171 | 172 | /*************************************************************************** 173 | * returns electrical Measurements. Readout has to be requested before with 174 | * requestRead or scheduling. 175 | ***************************************************************************/ 176 | float HoverboardAPI::getBatteryVoltage() 177 | { 178 | return electrical_measurements.batteryVoltage; 179 | } 180 | 181 | float HoverboardAPI::getMotorAmpsAvg(uint8_t motor) 182 | { 183 | if(motor > sizeof(electrical_measurements.motors)/sizeof(electrical_measurements.motors[0])) return -1.0; 184 | return electrical_measurements.motors[motor].dcAmpsAvg; 185 | } 186 | 187 | /*************************************************************************** 188 | * Schedules periodic transmission of value from control to hoverboard 189 | * count -1 for indefinetely 190 | ***************************************************************************/ 191 | void HoverboardAPI::scheduleTransmission(Codes code, int count, unsigned int period, char som) 192 | { 193 | PROTOCOL_MSG3full newMsg; 194 | memset((void*)&newMsg,0x00,sizeof(PROTOCOL_MSG3full)); 195 | 196 | PROTOCOL_SUBSCRIBEDATA *SubscribeData = (PROTOCOL_SUBSCRIBEDATA *) &newMsg.content; 197 | SubscribeData->code = code; 198 | SubscribeData->count = count; 199 | SubscribeData->period = period; 200 | SubscribeData->next_send_time = 0; 201 | SubscribeData->som = som; 202 | 203 | // Use native Subscription function to fill in array. 204 | newMsg.cmd = PROTOCOL_CMD_READVALRESPONSE; // This should prevent further processing 205 | newMsg.code = Codes::protocolSubscriptions; 206 | newMsg.SOM = PROTOCOL_SOM_NOACK; 207 | newMsg.lenPayload = sizeof(PROTOCOL_SUBSCRIBEDATA); 208 | 209 | fn_SubscribeData( &s, s.params[Codes::protocolSubscriptions], PROTOCOL_CMD_READVALRESPONSE, &newMsg ); 210 | } 211 | 212 | /*************************************************************************** 213 | * Triggers a periodic readout of data from the hoverboard 214 | * It is necessary to set a callback if something should happen when the 215 | * data arrives. Otherwise the data can just be read from the variable. 216 | ***************************************************************************/ 217 | void HoverboardAPI::scheduleRead(Codes code, int count, unsigned int period, char som) 218 | { 219 | PROTOCOL_SUBSCRIBEDATA writesubscribe; 220 | writesubscribe.code = code; 221 | writesubscribe.count = count; 222 | writesubscribe.period = period; 223 | writesubscribe.som = som; 224 | 225 | sendRawData( 226 | PROTOCOL_CMD_WRITEVAL, 227 | Codes::protocolSubscriptions, 228 | (unsigned char *) &writesubscribe, 229 | sizeof(writesubscribe), 230 | som 231 | ); 232 | } 233 | 234 | /*************************************************************************** 235 | * Sends PWM values to hoverboard 236 | ***************************************************************************/ 237 | void HoverboardAPI::sendPWM(int16_t pwm, int16_t steer, char som) 238 | { 239 | PROTOCOL_PWM_DATA writespeed; 240 | writespeed.pwm[0] = pwm + steer; 241 | writespeed.pwm[1] = pwm - steer; 242 | 243 | sendRawData( 244 | PROTOCOL_CMD_WRITEVAL, 245 | Codes::setPointPWM, 246 | (unsigned char *) &writespeed, 247 | sizeof(writespeed.pwm), 248 | som 249 | ); 250 | } 251 | 252 | void HoverboardAPI::sendDifferentialPWM(int16_t left_cmd, int16_t right_cmd, char som) 253 | { 254 | PROTOCOL_PWM_DATA writespeed; 255 | writespeed.pwm[0] = left_cmd; 256 | writespeed.pwm[1] = right_cmd; 257 | 258 | sendRawData( 259 | PROTOCOL_CMD_WRITEVAL, 260 | Codes::setPointPWM, 261 | (unsigned char *) &writespeed, 262 | sizeof(writespeed.pwm), 263 | som 264 | ); 265 | } 266 | 267 | /*************************************************************************** 268 | * Sends PWM values and Limits to hoverboard 269 | ***************************************************************************/ 270 | void HoverboardAPI::sendPWMData(int16_t pwm, 271 | int16_t steer, 272 | int speed_max_power, 273 | int speed_min_power, 274 | int speed_minimum_pwm, 275 | char som) 276 | { 277 | PROTOCOL_PWM_DATA writespeed; 278 | writespeed.pwm[0] = pwm + steer; 279 | writespeed.pwm[1] = pwm - steer; 280 | writespeed.speed_max_power = speed_max_power; 281 | writespeed.speed_min_power = speed_min_power; 282 | writespeed.speed_minimum_pwm = speed_minimum_pwm; 283 | 284 | sendRawData( 285 | PROTOCOL_CMD_WRITEVAL, 286 | Codes::setPointPWMData, 287 | (unsigned char *) &writespeed, 288 | sizeof(writespeed), 289 | som 290 | ); 291 | } 292 | 293 | /*************************************************************************** 294 | * Sends speed control to hoverboard 295 | ***************************************************************************/ 296 | void HoverboardAPI::sendSpeedData(double left_speed, double right_speed, int16_t max_power, int16_t min_speed, char som) 297 | { 298 | PROTOCOL_SPEED_DATA writespeed; 299 | writespeed.wanted_speed_mm_per_sec[0] = left_speed * 1000; 300 | writespeed.wanted_speed_mm_per_sec[1] = right_speed * 1000; 301 | writespeed.speed_max_power = max_power; 302 | writespeed.speed_min_power = -max_power; 303 | writespeed.speed_minimum_speed = min_speed; 304 | 305 | sendRawData( 306 | PROTOCOL_CMD_WRITEVAL, 307 | Codes::setSpeed, 308 | (unsigned char *) &writespeed, 309 | sizeof(writespeed.wanted_speed_mm_per_sec) + sizeof(writespeed.speed_max_power) + sizeof(writespeed.speed_min_power) + sizeof(writespeed.speed_minimum_speed), 310 | som 311 | ); 312 | } 313 | 314 | void HoverboardAPI::sendPIDControl(int16_t Kp, int16_t Ki, int16_t Kd, int16_t speed_increment, char som) 315 | { 316 | sendRawData( 317 | PROTOCOL_CMD_WRITEVAL, 318 | Codes::setSpeedKp, 319 | (unsigned char *) &Kp, 320 | sizeof(Kp), 321 | som 322 | ); 323 | 324 | sendRawData( 325 | PROTOCOL_CMD_WRITEVAL, 326 | Codes::setSpeedKi, 327 | (unsigned char *) &Ki, 328 | sizeof(Ki), 329 | som 330 | ); 331 | 332 | sendRawData( 333 | PROTOCOL_CMD_WRITEVAL, 334 | Codes::setSpeedKd, 335 | (unsigned char *) &Kd, 336 | sizeof(Kd), 337 | som 338 | ); 339 | 340 | sendRawData( 341 | PROTOCOL_CMD_WRITEVAL, 342 | Codes::setSpeedIncrLimit, 343 | (unsigned char *) &speed_increment, 344 | sizeof(speed_increment), 345 | som 346 | ); 347 | } 348 | 349 | /*************************************************************************** 350 | * Sends Buzzer data to hoverboard 351 | ***************************************************************************/ 352 | void HoverboardAPI::sendBuzzer(uint8_t buzzerFreq, uint8_t buzzerPattern, uint16_t buzzerLen, char som) 353 | { 354 | PROTOCOL_BUZZER_DATA writebuzzer; 355 | writebuzzer.buzzerFreq = buzzerFreq; 356 | writebuzzer.buzzerPattern = buzzerPattern; 357 | writebuzzer.buzzerLen = buzzerLen; 358 | 359 | sendRawData( 360 | PROTOCOL_CMD_WRITEVAL, 361 | Codes::setBuzzer, 362 | (unsigned char *) &writebuzzer, 363 | sizeof(writebuzzer), 364 | som 365 | ); 366 | } 367 | 368 | /*************************************************************************** 369 | * Sends enable to hoverboard 370 | ***************************************************************************/ 371 | void HoverboardAPI::sendEnable(uint8_t newEnable, char som) 372 | { 373 | sendRawData( 374 | PROTOCOL_CMD_WRITEVAL, 375 | Codes::enableMotors, 376 | (unsigned char *) &newEnable, 377 | sizeof(newEnable), 378 | som 379 | ); 380 | } 381 | 382 | /*************************************************************************** 383 | * Reset statistic counters 384 | ***************************************************************************/ 385 | void HoverboardAPI::sendCounterReset(char som) 386 | { 387 | PROTOCOLCOUNT writeprotocolcount; 388 | writeprotocolcount.rx = 0; 389 | writeprotocolcount.rxMissing = 0; 390 | writeprotocolcount.tx = 0; 391 | writeprotocolcount.txRetries = 0; 392 | writeprotocolcount.txFailed = 0; 393 | writeprotocolcount.unwantedacks = 0; 394 | writeprotocolcount.unwantednacks = 0; 395 | writeprotocolcount.unknowncommands = 0; 396 | writeprotocolcount.unplausibleresponse = 0; 397 | 398 | sendRawData( 399 | PROTOCOL_CMD_WRITEVAL, 400 | Codes::protocolCountSum, 401 | (unsigned char *) &writeprotocolcount, 402 | sizeof(writeprotocolcount), 403 | som 404 | ); 405 | } 406 | 407 | /*************************************************************************** 408 | * Reset statistic counters 409 | ***************************************************************************/ 410 | void HoverboardAPI::resetCounters() 411 | { 412 | s.ack.counters.rx = 0; 413 | s.ack.counters.rxMissing = 0; 414 | s.ack.counters.tx = 0; 415 | s.ack.counters.txRetries = 0; 416 | s.ack.counters.txFailed = 0; 417 | s.ack.counters.unwantedacks = 0; 418 | s.ack.counters.unwantednacks = 0; 419 | s.ack.counters.unknowncommands = 0; 420 | s.ack.counters.unplausibleresponse = 0; 421 | 422 | s.noack.counters.rx = 0; 423 | s.noack.counters.rxMissing = 0; 424 | s.noack.counters.tx = 0; 425 | s.noack.counters.txRetries = 0; 426 | s.noack.counters.txFailed = 0; 427 | s.noack.counters.unwantedacks = 0; 428 | s.noack.counters.unwantednacks = 0; 429 | s.noack.counters.unknowncommands = 0; 430 | s.noack.counters.unplausibleresponse = 0; 431 | } 432 | 433 | /*************************************************************************** 434 | * returns hall data. Readout has to be requested before with 435 | * requestRead or scheduling. 436 | ***************************************************************************/ 437 | double HoverboardAPI::getSpeed_kmh() 438 | { 439 | return (HallData[0].HallSpeed_mm_per_s + HallData[1].HallSpeed_mm_per_s) / 2.0 * 3600.0 / 1000000.0; 440 | } 441 | 442 | double HoverboardAPI::getSteer_kmh() 443 | { 444 | return (HallData[0].HallSpeed_mm_per_s * 3600.0 / 1000000.0 )- getSpeed_kmh(); 445 | } 446 | 447 | double HoverboardAPI::getSpeed_mms() 448 | { 449 | return (HallData[0].HallSpeed_mm_per_s + HallData[1].HallSpeed_mm_per_s) / 2.0; 450 | } 451 | 452 | double HoverboardAPI::getSteer_mms() 453 | { 454 | return HallData[0].HallSpeed_mm_per_s - getSpeed_mms(); 455 | } 456 | 457 | double HoverboardAPI::getSpeed0_mms() 458 | { 459 | return HallData[0].HallSpeed_mm_per_s; 460 | } 461 | 462 | double HoverboardAPI::getSpeed1_mms() 463 | { 464 | return HallData[1].HallSpeed_mm_per_s; 465 | } 466 | 467 | double HoverboardAPI::getSpeed0_kmh() 468 | { 469 | return HallData[0].HallSpeed_mm_per_s * 3600.0 / 1000000.0; 470 | } 471 | 472 | double HoverboardAPI::getSpeed1_kmh() 473 | { 474 | return HallData[1].HallSpeed_mm_per_s * 3600.0 / 1000000.0; 475 | } 476 | 477 | double HoverboardAPI::getPosition0_mm() 478 | { 479 | return HallData[0].HallPosn_mm; 480 | } 481 | 482 | double HoverboardAPI::getPosition1_mm() 483 | { 484 | return HallData[1].HallPosn_mm; 485 | } 486 | 487 | /*************************************************************************** 488 | * Sends Raw Data to hoverboard 489 | ***************************************************************************/ 490 | void HoverboardAPI::sendRawData( 491 | unsigned char msgCmd, 492 | unsigned char msgCode, 493 | unsigned char *content, 494 | unsigned char len, 495 | unsigned char msgSom) 496 | { 497 | // Compose new Message 498 | PROTOCOL_MSG3full msg = 499 | { 500 | SOM : msgSom, 501 | cmd : msgCmd, 502 | CI : 0, 503 | lenPayload : len, 504 | code : msgCode 505 | }; 506 | 507 | if(len <= sizeof( ((PROTOCOL_MSG3full *)0)->content ) ) memcpy(&msg.content, content, len); 508 | 509 | protocol_post(&s, &msg); 510 | } 511 | 512 | /*************************************************************************** 513 | * Sends Raw msg to hoverboard 514 | ***************************************************************************/ 515 | void HoverboardAPI::protocolPost(PROTOCOL_MSG3full *msg) 516 | { 517 | protocol_post(&s, msg); 518 | } 519 | 520 | /*************************************************************************** 521 | * Sends Text msg to hoverboard 522 | ***************************************************************************/ 523 | int HoverboardAPI::sendText(char *message, unsigned char som) 524 | { 525 | return protocol_send_text(&s, message, som); 526 | } 527 | 528 | /*************************************************************************** 529 | * Fakes Receiving a Message from hoverboard (Useful for debugging) 530 | ***************************************************************************/ 531 | void HoverboardAPI::receiveText(char *message) 532 | { 533 | if( (s.params[0x26]) && (strlen(message) <= s.params[0x26]->len ) ) 534 | { 535 | PROTOCOL_MSG3full newMsg; 536 | memset((void*)&newMsg,0x00,sizeof(PROTOCOL_MSG3full)); 537 | 538 | newMsg.SOM = PROTOCOL_SOM_NOACK; 539 | newMsg.lenPayload = strlen(message) + 1; // +1 for Null character \0 540 | 541 | newMsg.cmd = PROTOCOL_CMD_READVALRESPONSE; 542 | newMsg.code = 0x26; // 0x26 for text 543 | strcpy( (char *) newMsg.content, message); 544 | 545 | protocol_process_message(&s, &newMsg); 546 | } 547 | } 548 | 549 | /*************************************************************************** 550 | * Sends a ping to the hoverboard containing a timestamp 551 | ***************************************************************************/ 552 | void HoverboardAPI::sendPing(char som) 553 | { 554 | unsigned long time = millis(); 555 | 556 | sendRawData( 557 | PROTOCOL_CMD_WRITEVAL, 558 | Codes::ping, 559 | (unsigned char *) &time, 560 | sizeof(time), 561 | som 562 | ); 563 | } --------------------------------------------------------------------------------