diff --git a/controller/controller.ino b/controller/controller.ino index ede5075..37e8c7c 100644 --- a/controller/controller.ino +++ b/controller/controller.ino @@ -23,7 +23,7 @@ #define START_FRAME 0xAAAA // [-] Start frme definition for reliable serial communication //#define DEBUG -#define PARAMETEROUTPUT +//#define PARAMETEROUTPUT uint8_t error = 0; #define IMU_NO_CHANGE 2 //IMU values did not change for too long uint8_t imu_no_change_counter = 0; @@ -386,6 +386,7 @@ void loop() { set_steer = (int16_t)( ((int16_t)(lastnrfdata.steer) - NRFDATA_CENTER) * 1000 / 127 ); //align to compass + /* double yawdiff = (setYaw - 180) - (yaw - 180); //following angle difference works only for angles [-180,180]. yaw here is [0,360] yawdiff += (yawdiff > 180) ? -360 : (yawdiff < -180) ? 360 : 0; //yawdiff/=2; @@ -406,6 +407,7 @@ void loop() { magalign_multiplier = constrain(magalign_multiplier, 0.0, 1.0); //safety constrain again set_steer = set_steer * (1 - magalign_multiplier) + set_steer_mag * magalign_multiplier; + */ setYaw = setYaw * magalign_multiplier + yaw * (1 - magalign_multiplier); //if magalign_multiplier 0, setYaw equals current yaw @@ -679,8 +681,8 @@ void ReceiveSerial2() Serial.print(" 4: "); Serial.print(Feedback.speedL); Serial.print(" 5: "); Serial.print(Feedback.speedR_meas); Serial.print(" 6: "); Serial.print(Feedback.speedL_meas); - Serial.print(" 7: "); Serial.print(Feedback.batVoltage); - Serial.print(" 8: "); Serial.println(Feedback.boardTemp); + //Serial.print(" 7: "); Serial.print(Feedback.batVoltage); + //Serial.print(" 8: "); Serial.println(Feedback.boardTemp); Serial.print(" 9: "); Serial.print(Feedback.curL_DC); //in A, in hoverbrett negative sign for forward Serial.print(" 10: "); Serial.println(Feedback.curR_DC); //in A, in hoverbrett negative sign for forward } else { diff --git a/controller_pio/.gitignore b/controller_pio/.gitignore new file mode 100644 index 0000000..03f4a3c --- /dev/null +++ b/controller_pio/.gitignore @@ -0,0 +1 @@ +.pio diff --git a/controller_pio/.travis.yml b/controller_pio/.travis.yml new file mode 100644 index 0000000..7c486f1 --- /dev/null +++ b/controller_pio/.travis.yml @@ -0,0 +1,67 @@ +# Continuous Integration (CI) is the practice, in software +# engineering, of merging all developer working copies with a shared mainline +# several times a day < https://docs.platformio.org/page/ci/index.html > +# +# Documentation: +# +# * Travis CI Embedded Builds with PlatformIO +# < https://docs.travis-ci.com/user/integration/platformio/ > +# +# * PlatformIO integration with Travis CI +# < https://docs.platformio.org/page/ci/travis.html > +# +# * User Guide for `platformio ci` command +# < https://docs.platformio.org/page/userguide/cmd_ci.html > +# +# +# Please choose one of the following templates (proposed below) and uncomment +# it (remove "# " before each line) or use own configuration according to the +# Travis CI documentation (see above). +# + + +# +# Template #1: General project. Test it using existing `platformio.ini`. +# + +# language: python +# python: +# - "2.7" +# +# sudo: false +# cache: +# directories: +# - "~/.platformio" +# +# install: +# - pip install -U platformio +# - platformio update +# +# script: +# - platformio run + + +# +# Template #2: The project is intended to be used as a library with examples. +# + +# language: python +# python: +# - "2.7" +# +# sudo: false +# cache: +# directories: +# - "~/.platformio" +# +# env: +# - PLATFORMIO_CI_SRC=path/to/test/file.c +# - PLATFORMIO_CI_SRC=examples/file.ino +# - PLATFORMIO_CI_SRC=path/to/test/directory +# +# install: +# - pip install -U platformio +# - platformio update +# +# script: +# - platformio ci --lib="." --board=ID_1 --board=ID_2 --board=ID_N diff --git a/controller_pio/.vscode/c_cpp_properties.json b/controller_pio/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..33db116 --- /dev/null +++ b/controller_pio/.vscode/c_cpp_properties.json @@ -0,0 +1,142 @@ +{ + "configurations": [ + { + "name": "!!! WARNING !!! AUTO-GENERATED FILE, PLEASE DO NOT MODIFY IT AND USE https://docs.platformio.org/page/projectconf/section_env_build.html#build-flags" + }, + { + "name": "Linux", + "includePath": [ + "/media/fisch/HDD/Projects/hoverbrett/controller_pio/include", + "/media/fisch/HDD/Projects/hoverbrett/controller_pio/src", + "/media/fisch/HDD/Projects/hoverbrett/controller_pio/.pio/libdeps/genericSTM32F103C8/RF24_ID433", + "/media/fisch/HDD/Projects/hoverbrett/controller_pio/.pio/libdeps/genericSTM32F103C8/RF24_ID433/utility", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/SPI/src", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/cores/maple", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple/include", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple/stm32f1/include", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple/usb/stm32f1", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple/usb/usb_lib", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/variants/generic_stm32f103c", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/A_STM32_Examples", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Adafruit_GFX_AS", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Adafruit_ILI9341", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Adafruit_ILI9341_STM", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Adafruit_SSD1306", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/EEPROM", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Ethernet_STM/src", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS701", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS701/utility", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS821", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS821/utility", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS900", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Lcd7920_STM", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/LiquidCrystal", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/MapleCoOS", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/MapleCoOS/utility", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/MapleCoOS116", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/MapleCoOS116/utility", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/OLED_I2C", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/OneWireSTM/src", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/RTClock/src", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/SDIO", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/STM32ADC/src", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Serasidis_EtherCard_STM/src", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Serasidis_VS1003B_STM/src", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Serasidis_XPT2046_touch/src", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Servo/src", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Touch-Screen-Library_STM", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/USBComposite", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/WS2812B/src", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Wire", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Wire/utility", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/WireSlave/src", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/stm_fft", + "/home/fisch/.platformio/packages/tool-unity", + "" + ], + "browse": { + "limitSymbolsToIncludedHeaders": true, + "path": [ + "/media/fisch/HDD/Projects/hoverbrett/controller_pio/include", + "/media/fisch/HDD/Projects/hoverbrett/controller_pio/src", + "/media/fisch/HDD/Projects/hoverbrett/controller_pio/.pio/libdeps/genericSTM32F103C8/RF24_ID433", + "/media/fisch/HDD/Projects/hoverbrett/controller_pio/.pio/libdeps/genericSTM32F103C8/RF24_ID433/utility", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/SPI/src", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/cores/maple", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple/include", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple/stm32f1/include", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple/usb/stm32f1", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple/usb/usb_lib", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/variants/generic_stm32f103c", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/A_STM32_Examples", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Adafruit_GFX_AS", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Adafruit_ILI9341", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Adafruit_ILI9341_STM", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Adafruit_SSD1306", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/EEPROM", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Ethernet_STM/src", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS701", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS701/utility", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS821", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS821/utility", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS900", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Lcd7920_STM", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/LiquidCrystal", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/MapleCoOS", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/MapleCoOS/utility", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/MapleCoOS116", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/MapleCoOS116/utility", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/OLED_I2C", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/OneWireSTM/src", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/RTClock/src", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/SDIO", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/STM32ADC/src", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Serasidis_EtherCard_STM/src", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Serasidis_VS1003B_STM/src", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Serasidis_XPT2046_touch/src", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Servo/src", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Touch-Screen-Library_STM", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/USBComposite", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/WS2812B/src", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Wire", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Wire/utility", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/WireSlave/src", + "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/stm_fft", + "/home/fisch/.platformio/packages/tool-unity", + "" + ] + }, + "defines": [ + "PLATFORMIO=40304", + "STM32F103xB", + "STM32F1", + "ARDUINO=10808", + "ARDUINO_ARCH_STM32", + "ARDUINO_ARCH_STM32F1", + "ARDUINO_GENERIC_STM32F103C", + "MCU_STM32F103C8", + "__STM32F1__", + "BOARD_generic_stm32f103c", + "F_CPU=72000000L", + "VECT_TAB_ADDR=0x8000000", + "ERROR_LED_PORT=GPIOC", + "ERROR_LED_PIN=13", + "CONFIG_MAPLE_MINI_NO_DISABLE_DEBUG=1", + "" + ], + "intelliSenseMode": "clang-x64", + "cStandard": "c11", + "cppStandard": "c++11", + "compilerPath": "/home/fisch/.platformio/packages/toolchain-gccarmnoneeabi@1.70201.0/bin/arm-none-eabi-gcc", + "compilerArgs": [ + "-mcpu=cortex-m3", + "-mthumb", + "-march=armv7-m", + "" + ] + } + ], + "version": 4 +} diff --git a/controller_pio/.vscode/extensions.json b/controller_pio/.vscode/extensions.json new file mode 100644 index 0000000..0f0d740 --- /dev/null +++ b/controller_pio/.vscode/extensions.json @@ -0,0 +1,7 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ] +} diff --git a/controller_pio/.vscode/launch.json b/controller_pio/.vscode/launch.json new file mode 100644 index 0000000..4d6ddbc --- /dev/null +++ b/controller_pio/.vscode/launch.json @@ -0,0 +1,34 @@ +// AUTOMATICALLY GENERATED FILE. PLEASE DO NOT MODIFY IT MANUALLY + +// PIO Unified Debugger +// +// Documentation: https://docs.platformio.org/page/plus/debugging.html +// Configuration: https://docs.platformio.org/page/projectconf/section_env_debug.html + +{ + "version": "0.2.0", + "configurations": [ + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug", + "executable": "/media/fisch/HDD/Projects/hoverbrett/controller_pio/.pio/build/genericSTM32F103C8/firmware.elf", + "toolchainBinDir": "/home/fisch/.platformio/packages/toolchain-gccarmnoneeabi@1.70201.0/bin", + "svdPath": "/home/fisch/.platformio/platforms/ststm32/misc/svd/STM32F103xx.svd", + "preLaunchTask": { + "type": "PlatformIO", + "task": "Pre-Debug" + }, + "internalConsoleOptions": "openOnSessionStart" + }, + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug (skip Pre-Debug)", + "executable": "/media/fisch/HDD/Projects/hoverbrett/controller_pio/.pio/build/genericSTM32F103C8/firmware.elf", + "toolchainBinDir": "/home/fisch/.platformio/packages/toolchain-gccarmnoneeabi@1.70201.0/bin", + "svdPath": "/home/fisch/.platformio/platforms/ststm32/misc/svd/STM32F103xx.svd", + "internalConsoleOptions": "openOnSessionStart" + } + ] +} \ No newline at end of file diff --git a/controller_pio/include/README b/controller_pio/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/controller_pio/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/controller_pio/lib/README b/controller_pio/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/controller_pio/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/controller_pio/platformio.ini b/controller_pio/platformio.ini new file mode 100644 index 0000000..0aaa5cc --- /dev/null +++ b/controller_pio/platformio.ini @@ -0,0 +1,21 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:genericSTM32F103C8] +platform = ststm32 +board = genericSTM32F103C8 +framework = arduino + +upload_protocol = serial + +monitor_speed = 115200 + +lib_deps = + RF24@>=1.3.4 \ No newline at end of file diff --git a/controller_pio/src/main.cpp b/controller_pio/src/main.cpp new file mode 100644 index 0000000..a23a1f1 --- /dev/null +++ b/controller_pio/src/main.cpp @@ -0,0 +1,658 @@ +#include + +void ReceiveSerial2(); +void SendSerial2(int16_t uSpeedLeft, int16_t uSpeedRight); + +//https://github.com/rogerclarkmelbourne/Arduino_STM32 in arduino/hardware +//Board: Generic STM32F103C series +//Upload method: serial +//20k RAM 64k Flash + +// RX ist A10, TX ist A9 (3v3 level) +//to flash set boot0 (the one further away from reset button) to 1 and press reset, flash, program executes immediately +//set boot0 back to 0 to run program on powerup + +//Flashing the hoverbrett controller: +/* + * connect uart adapter to serial port cable (the one with more red heatshrink) + * (disconnect xt30 power connector) + * set jumper on usb uart adapter to output 5V + * hold boot0 button (black, the outermost) while powering up (or restarting with small button next to it) + * flash + */ + +//PA2 may be defective on my bluepill + +#define SERIAL_CONTROL_BAUD 38400 // [-] Baud rate for HoverSerial (used to communicate with the hoverboard) +#define SERIAL_BAUD 115200 // [-] Baud rate for built-in Serial (used for the Serial Monitor) +#define START_FRAME 0xAAAA // [-] Start frme definition for reliable serial communication + +//#define DEBUG +//#define PARAMETEROUTPUT +uint8_t error = 0; +#define IMU_NO_CHANGE 2 //IMU values did not change for too long +uint8_t imu_no_change_counter = 0; + +#define PIN_LED PC13 + +#define PIN_VBAT PA0 //battery voltage after voltage divider +//#define VBAT_DIV_FACTOR 0.010700 //how much voltage (V) equals one adc unit. measured at 40V and averaged +#define VBAT_DIV_FACTOR 0.01399535423925667828 //how much voltage (V) equals one adc unit. 3444=48.2V +#define PIN_CURRENT PA1 //output of hall sensor for current measurement +#define CURRENT_OFFSET 2048 //adc reading at 0A, with CJMCU-758 typically at Vcc/2. measured with actual voltage supply in hoverbrett +#define CURRENT_FACTOR 0.38461538461538461538 //how much current (A) equals one adc unit. 2045-2032=13 at 5A +float vbat=0; //battery voltage +float ibat=0; //battery current +long last_adcupdated=0; +#define ADC_UPDATEPERIOD 10 //in ms + + +#define SENDPERIOD 20 //ms. delay for sending speed and steer data to motor controller via serial + +//Status information sending +#define PARAMETERSENDPERIOD 50 //delay for sending stat data via nrf24 +long last_parametersend=0; + +#define CONTROLUPDATEPERIOD 10 +long last_controlupdate = 0; + +#define PIN_GAMETRAK_LENGTH PA1 //yellow (connector) / orange (gametrak module wires): length +#define PIN_GAMETRAK_VERTICAL PA3 //orange / red: vertical +#define PIN_GAMETRAK_HORIZONTAL PA4 //blue / yellow: horizontal + + +#define GT_LENGTH_OFFSET 4090 //adc offset value (rolled up value) +#define GT_LENGTH_MIN 220 //length in mm at which adc values start to change +#define GT_LENGTH_SCALE -0.73 //(adcvalue-offset)*scale = length[mm] (+length_min) +//2720 at 1000mm+220mm -> 1370 for 1000mm -> +#define GT_LENGTH_MAXLENGTH 2500 //maximum length in [mm]. maximum string length is around 2m80 +uint16_t gt_length=0; //0=rolled up, 1unit = 1mm + +#define GT_VERTICAL_CENTER 2048 //adc value for center position +#define GT_VERTICAL_RANGE 2047 //adc value difference from center to maximum (30 deg) +int8_t gt_vertical=0; //0=center. joystick can rotate +-30 degrees. -127 = -30 deg +//left = -30 deg, right= 30deg + +#define GT_HORIZONTAL_CENTER 2048 //adc value for center position +#define GT_HORIZONTAL_RANGE 2047 //adc value difference from center to maximum (30 deg) +int8_t gt_horizontal=0; //0=center + +uint16_t gt_length_set=1000; //set length to keep [mm] +#define GT_LENGTH_MINDIFF 10 //[mm] threshold, do not move within gt_length_set-GT_LENGTH_MINDIFF and gt_length_set+GT_LENGTH_MINDIFF +float gt_speed_p=0.7; //value to multipy difference [mm] with -> out_speed +float gt_speedbackward_p=0.7; +float gt_steer_p=2.0; +#define GT_SPEED_LIMIT 300 //maximum out_speed value + +#define GT_SPEEDBACKWARD_LIMIT 100//maximum out_speed value (for backward driving) - +#define GT_STEER_LIMIT 300 //maximum out_steer value +- +#define GT_LENGTH_MAXIMUMDIFFBACKWARD -200 //[mm]. if gt_length_set=1000 and GT_LENGTH_MAXIMUMDIFFBACKWARD=-200 then only drives backward if lenght is greater 800 + + +//#include +//https://github.com/fookingthg/GY85 +//ITG3200 and ADXL345 from https://github.com/jrowberg/i2cdevlib/tree/master/Arduino +//https://github.com/mechasolution/Mecha_QMC5883L //because QMC5883 on GY85 instead of HMC5883, source: https://circuitdigest.com/microcontroller-projects/digital-compass-with-arduino-and-hmc5883l-magnetometer +//in qmc5883L library read values changed from uint16_t to int16_t + +#define IMUUPDATEPERIOD 10 //ms +//long last_imuupdated = 0; +//#define MAX_YAWCHANGE 90 //in degrees, if exceeded in one update intervall error will be triggered + +/* +IMUGY85 imu; +double ax, ay, az, gx, gy, gz, roll, pitch, yaw, mx, my, mz, ma; +double old_ax, old_ay, old_az, old_gx, old_gy, old_gz, old_roll, old_pitch, old_yaw, old_mx, old_my, old_mz, old_ma; + +double setYaw = 0; +float magalign_multiplier = 0; //how much the magnetometer should influence steering, 0=none, 1=stay aligned +*/ + +// Lenovo Trackpoint pinout +//from left to right. pins at bottom. chips on top +//1 GND (black) +//2 Data +//3 Clock +//4 Reset +//5 +5V (red) +//6 Right BTN +//7 Middle BTN +//8 Left BTN +//pinout: https://martin-prochnow.de/projects/thinkpad_keyboard +//see also https://github.com/feklee/usb-trackpoint/blob/master/code/code.ino + +#include +#include "nRF24L01.h" +#include "RF24.h" + +RF24 radio(PB0, PB1); //ce, cs +//SCK D13 (Pro mini), A5 (bluepill) +//Miso D12 (Pro mini), A6 (bluepill) +//Mosi D11 (Pro mini), A7 (bluepill) + +// Radio pipe addresses for the 2 nodes to communicate. +const uint64_t pipes[2] = { 0xF0F0F0F0E1LL, 0xF0F0F0F0D2LL }; +#define NRF24CHANNEL 75 + +struct nrfdata { + uint8_t steer; + uint8_t speed; + uint8_t commands; //bit 0 set = motor enable + uint8_t checksum; +}; + +nrfdata lastnrfdata; +long last_nrfreceive = 0; //last time values were received and checksum ok +long nrf_delay = 0; +#define MAX_NRFDELAY 100 //ms. maximum time delay at which vehicle will disarm + +boolean radiosendOk=false; + + +//command variables +boolean motorenabled = false; //set by nrfdata.commands + + + + +long last_send = 0; + +int16_t out_speedl = 0; //between -1000 and 1000 +int16_t out_speedr = 0; +int16_t lastsend_out_speedl = 0; //last value transmitted to motor controller +int16_t lastsend_out_speedr = 0; +int16_t set_speed = 0; +int16_t set_steer = 0; +uint8_t out_checksum = 0; //0= disable motors, 255=reserved, 1<=checksum<255 +#define NRFDATA_CENTER 127 + +//boolean armed = false; +boolean lastpacketOK = false; + +//Gametrak +//boolean armed_gt = false; + +uint8_t controlmode=0; +#define MODE_DISARMED 0 +#define MODE_RADIONRF 1 +#define MODE_GAMETRAK 2 + + + +// Global variables +uint8_t idx = 0; // Index for new data pointer +uint16_t bufStartFrame; // Buffer Start Frame +byte *p; // Pointer declaration for the new received data +byte incomingByte; +byte incomingBytePrev; + +typedef struct{ + uint16_t start; + int16_t speedLeft; + int16_t speedRight; + uint16_t checksum; +} SerialCommand; +SerialCommand Command; + +typedef struct{ + uint16_t start; + int16_t cmd1; + int16_t cmd2; + int16_t speedL; + int16_t speedR; + int16_t speedL_meas; + int16_t speedR_meas; + int16_t batVoltage; + int16_t boardTemp; + int16_t curL_DC; + int16_t curR_DC; + uint16_t checksum; +} SerialFeedback; +SerialFeedback Feedback; +SerialFeedback NewFeedback; + + + + +void setup() { + + Serial.begin(SERIAL_BAUD); //Debug and Program. A9=TX1, A10=RX1 (3v3 level) + + + Serial2.begin(SERIAL_CONTROL_BAUD); //control. B10=TX3, B11=RX3 (Serial2 is Usart 3) + //Serial1 may be dead on my board? + + + pinMode(PIN_LED, OUTPUT); + digitalWrite(PIN_LED, HIGH); + + + pinMode(PIN_VBAT,INPUT_ANALOG); + pinMode(PIN_CURRENT,INPUT_ANALOG); + + pinMode(PIN_GAMETRAK_LENGTH,INPUT_ANALOG); + pinMode(PIN_GAMETRAK_VERTICAL,INPUT_ANALOG); + pinMode(PIN_GAMETRAK_HORIZONTAL,INPUT_ANALOG); + + + #ifdef DEBUG + Serial.println("Initializing nrf24"); + #endif + + + + radio.begin(); + + + radio.setDataRate( RF24_250KBPS ); //set to slow data rate. default was 1MBPS + //radio.setDataRate( RF24_1MBPS ); + + radio.setChannel(NRF24CHANNEL); //0 to 124 (inclusive) + + radio.setRetries(15, 15); // optionally, increase the delay between retries & # of retries + radio.setPayloadSize(8); // optionally, reduce the payload size. seems to improve reliability + + radio.openWritingPipe(pipes[0]); //write on pipe 0 + radio.openReadingPipe(1, pipes[1]); //read on pipe 1 + + radio.startListening(); + + + + + #ifdef DEBUG + Serial.println("Initialized"); + #endif + + +} + +void loop() { + + + ReceiveSerial2(); // Check for new received data + + + if (millis() - last_adcupdated > ADC_UPDATEPERIOD) { //update analog readings + vbat=analogRead(PIN_VBAT)*VBAT_DIV_FACTOR; + ibat=(analogRead(PIN_CURRENT)-CURRENT_OFFSET)*CURRENT_FACTOR; + + + gt_length = constrain((analogRead(PIN_GAMETRAK_LENGTH)-GT_LENGTH_OFFSET)*GT_LENGTH_SCALE +GT_LENGTH_MIN, 0,GT_LENGTH_MAXLENGTH); + if (gt_length<=GT_LENGTH_MIN){ + gt_length=0; //if below minimum measurable length set to 0mm + } + gt_vertical = constrain(map(analogRead(PIN_GAMETRAK_VERTICAL)-GT_VERTICAL_CENTER, +GT_VERTICAL_RANGE,-GT_VERTICAL_RANGE,-127,127),-127,127); //left negative + gt_horizontal = constrain(map(analogRead(PIN_GAMETRAK_HORIZONTAL)-GT_HORIZONTAL_CENTER, +GT_HORIZONTAL_RANGE,-GT_HORIZONTAL_RANGE,-127,127),-127,127); //down negative + + last_adcupdated = millis(); + + /* + Serial.print("gt_length="); + Serial.print(gt_length); + Serial.print(", gt_vertical="); + Serial.print(gt_vertical); + Serial.print(", gt_horizontal="); + Serial.println(gt_horizontal);*/ + + /* + Serial.print("PIN_GAMETRAK_LENGTH="); + Serial.print(analogRead(PIN_GAMETRAK_LENGTH)); + Serial.print(", PIN_GAMETRAK_VERTICAL="); + Serial.print(analogRead(PIN_GAMETRAK_VERTICAL)); + Serial.print(", PIN_GAMETRAK_HORIZONTAL="); + Serial.println(analogRead(PIN_GAMETRAK_HORIZONTAL)); + */ + } + + //NRF24 + nrf_delay = millis() - last_nrfreceive; //update nrf delay + if ( radio.available() ) + { + //Serial.println("radio available ..."); + + lastpacketOK = false; //initialize with false, if checksum ok gets set to true + digitalWrite(PIN_LED, !digitalRead(PIN_LED)); + radio.read( &lastnrfdata, sizeof(nrfdata) ); + + if (lastnrfdata.speed == NRFDATA_CENTER && lastnrfdata.steer == NRFDATA_CENTER) { //arm only when centered + controlmode = MODE_RADIONRF;//set radionrf mode at first received packet + } + + + + + uint8_t calcchecksum = (uint8_t)((lastnrfdata.steer + 3) * (lastnrfdata.speed + 13)); + if (lastnrfdata.checksum == calcchecksum) { //checksum ok? + lastpacketOK = true; + last_nrfreceive = millis(); + + //parse commands + motorenabled = (lastnrfdata.commands & (1 << 0))>>0; //check bit 0 + } + +/* +#ifdef DEBUG + Serial.print("Received:"); + Serial.print(" st="); + Serial.print(lastnrfdata.steer); + Serial.print(", sp="); + Serial.print(lastnrfdata.speed); + Serial.print(", c="); + Serial.print(lastnrfdata.commands); + Serial.print(", chks="); + Serial.print(lastnrfdata.checksum); + + Serial.print("nrfdelay="); + Serial.print(nrf_delay); + Serial.println(); +#endif +*/ + + //y positive = forward + //x positive = right + + /* + setYaw+=((int16_t)(lastnrfdata.steer)-NRFDATA_CENTER)*10/127; + while (setYaw<0){ + setYaw+=360; + } + while (setYaw>=360){ + setYaw-=360; + }*/ + + /* + Serial.print("setYaw="); + Serial.print(setYaw); + Serial.print(" Yaw="); + Serial.println(yaw);*/ + + + } + + + + + if (controlmode == MODE_RADIONRF && nrf_delay >= MAX_NRFDELAY) { //too long since last sucessful nrf receive + controlmode = MODE_DISARMED; + #ifdef DEBUG + Serial.println("nrf_delay>=MAX_NRFDELAY, disarmed!"); + #endif + } + if (controlmode == MODE_RADIONRF) { //is armed in nrf mode + + + if (lastpacketOK) { //if lastnrfdata is valid + if (millis() - last_controlupdate > CONTROLUPDATEPERIOD) { + last_controlupdate = millis(); + + //out_speed=(int16_t)( (lastnrfdata.y-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX ); + //out_steer=(int16_t)( -(lastnrfdata.x-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX ); + set_speed = (int16_t)( ((int16_t)(lastnrfdata.speed) - NRFDATA_CENTER) * 1000 / 127 ); //-1000 to 1000 + set_steer = (int16_t)( ((int16_t)(lastnrfdata.steer) - NRFDATA_CENTER) * 1000 / 127 ); + + //align to compass + /* + double yawdiff = (setYaw - 180) - (yaw - 180); //following angle difference works only for angles [-180,180]. yaw here is [0,360] + yawdiff += (yawdiff > 180) ? -360 : (yawdiff < -180) ? 360 : 0; + //yawdiff/=2; + int yawdiffsign = 1; + if (yawdiff < 0) { + yawdiffsign = -1; + } + yawdiff = yawdiff * yawdiff; //square + yawdiff = constrain(yawdiff * 1 , 0, 800); + yawdiff *= yawdiffsign; //redo sign + int16_t set_steer_mag = (int16_t)( yawdiff ); + + float new_magalign_multiplier = map( abs((int16_t)(lastnrfdata.steer) - NRFDATA_CENTER), 2, 10, 1.0, 0.0); //0=normal steering, 1=only mag steering + new_magalign_multiplier = 0; //Force mag off + new_magalign_multiplier = constrain(new_magalign_multiplier, 0.0, 1.0); + + magalign_multiplier = min(new_magalign_multiplier, min(1.0, magalign_multiplier + 0.01)); //go down fast, slowly increase + magalign_multiplier = constrain(magalign_multiplier, 0.0, 1.0); //safety constrain again + + set_steer = set_steer * (1 - magalign_multiplier) + set_steer_mag * magalign_multiplier; + */ + + //calculate speed l and r from speed and steer + #define SPEED_COEFFICIENT_NRF 1 // higher value == stronger + #define STEER_COEFFICIENT_NRF 0.5 // higher value == stronger + out_speedl = constrain(set_speed * SPEED_COEFFICIENT_NRF + set_steer * STEER_COEFFICIENT_NRF, -1500, 1500); + out_speedr = constrain(set_speed * SPEED_COEFFICIENT_NRF - set_steer * STEER_COEFFICIENT_NRF, -1500, 1500); + + /* + Serial.print("Out steer="); + Serial.println(out_steer);*/ + } + }//if pastpacket not ok, keep last out_steer and speed values until disarmed + + +#ifdef DEBUG + if (!lastpacketOK) { + Serial.println("Armed but packet not ok"); + } +#endif + + } + + + + if (controlmode==MODE_DISARMED) { //check if gametrak can be armed + if (gt_length>gt_length_set && gt_length-GT_LENGTH_MINDIFF & _gt_length_diff0) { //needs to drive forward + set_speed = constrain((int16_t)(_gt_length_diff*gt_speed_p),0,GT_SPEED_LIMIT); + }else{ //drive backward + if (_gt_length_diff > GT_LENGTH_MAXIMUMDIFFBACKWARD){ //only drive if not pulled back too much + set_speed = constrain((int16_t)(_gt_length_diff*gt_speedbackward_p),-GT_SPEEDBACKWARD_LIMIT,0); + }else{ + set_speed = 0; //stop + set_steer = 0; + } + } + + + + //calculate speed l and r from speed and steer + #define SPEED_COEFFICIENT_GT 1 // higher value == stronger + #define STEER_COEFFICIENT_GT 0.5 // higher value == stronger + out_speedl = constrain(set_speed * SPEED_COEFFICIENT_GT + set_steer * STEER_COEFFICIENT_GT, -1000, 1000); + out_speedr = constrain(set_speed * SPEED_COEFFICIENT_GT - set_steer * STEER_COEFFICIENT_GT, -1000, 1000); + } + + + if (error > 0) { //disarm if error occured + controlmode = MODE_DISARMED; //force disarmed + } + + if (controlmode == MODE_DISARMED){ //all disarmed + out_speedl = 0; + out_speedr = 0; + } + + + + if (millis() - last_send > SENDPERIOD) { + //calculate checksum + out_checksum = ((uint8_t) ((uint8_t)out_speedl) * ((uint8_t)out_speedr)); //simple checksum + if (out_checksum == 0 || out_checksum == 255) { + out_checksum = 1; //cannot be 0 or 255 (special purpose) + } + + if (!motorenabled) { //disable motors? + out_checksum = 0; //checksum=0 disables motors + } + + /*Serial2.write((uint8_t *) &out_speedl, sizeof(out_speedl)); + Serial2.write((uint8_t *) &out_speedr, sizeof(out_speedr)); + Serial2.write((uint8_t *) &out_checksum, sizeof(out_checksum));*/ + if (motorenabled) { //motors enabled + SendSerial2(out_speedl,out_speedr); + } else { //motors disabled + SendSerial2(0,0); + } + lastsend_out_speedl = out_speedl; //remember last transmittet values (for stat sending) + lastsend_out_speedr = out_speedr; + last_send = millis(); + + + #ifdef DEBUG + Serial.print(" out_speedl="); + Serial.print(out_speedl); + Serial.print(" out_speedr="); + Serial.print(out_speedr); + Serial.print(" checksum="); + Serial.print(out_checksum); + + Serial.print(" controlmode="); + Serial.print(controlmode); + + Serial.println(); + + #endif + } + + + // + #ifdef PARAMETEROUTPUT + if ( millis() - last_parametersend > PARAMETERSENDPERIOD) { + //Serial.write((uint8_t *) &counter, sizeof(counter));//uint8_t, 1 byte + //Serial.write((uint8_t *) &value1, sizeof(value1)); //uint16_t, 2 bytes + //Serial.write((uint8_t *) &value2, sizeof(value2)); //int16_t, 2 bytes + //Serial.write((uint8_t *) &floatvalue, sizeof(floatvalue)); //float, 4 bytes + + uint8_t booleanvalues=0; //reset + booleanvalues |= motorenabled<<0; //bit 0 + booleanvalues |= (controlmode&0b00000011)<<1; //bit 1 and 2 (2bit number for controlmodes (3) + + Serial.write((uint8_t *) &out_speedl, sizeof(out_speedl)); //int16_t, 2 bytes + Serial.write((uint8_t *) &out_speedr, sizeof(out_speedr)); //int16_t, 2 bytes + Serial.write((uint8_t *) &booleanvalues, sizeof(booleanvalues)); //uint8_t, 1 byte //booleanvalues + Serial.write((uint8_t *) &vbat, sizeof(vbat)); //float, 4 bytes + //Serial.write((uint8_t *) &ibat, sizeof(ibat)); //float, 4 bytes + float yaw_float=yaw; + Serial.write((uint8_t *) &yaw_float, sizeof(yaw_float)); //float, 4 bytes + Serial.write((uint8_t *) >_length, sizeof(gt_length)); //uint16_t, 2 bytes + Serial.write((uint8_t *) >_horizontal, sizeof(gt_horizontal)); //int8_t, 1 byte + Serial.write((uint8_t *) >_vertical, sizeof(gt_vertical)); //int8_t, 1 byte + + + + + last_parametersend = millis(); + } + #endif + + +} + +/* +void sendRF(nrfstatdata senddata){ + #ifdef DEBUG + Serial.println("Transmitting..."); + #endif + + radio.stopListening(); //stop listening to be able to transmit + radiosendOk = radio.write( &senddata, sizeof(nrfstatdata) ); + if (!radiosendOk){ + #ifdef DEBUG + Serial.println("send failed"); + #endif + } + radio.startListening(); //start listening again +} +*/ + + + + +// ########################## SEND ########################## +void SendSerial2(int16_t uSpeedLeft, int16_t uSpeedRight) +{ + // Create command + Command.start = (uint16_t)START_FRAME; + Command.speedLeft = (int16_t)uSpeedLeft; + Command.speedRight = (int16_t)uSpeedRight; + Command.checksum = (uint16_t)(Command.start ^ Command.speedLeft ^ Command.speedRight); + + // Write to Serial + Serial2.write((uint8_t *) &Command, sizeof(Command)); +} + +// ########################## RECEIVE ########################## +void ReceiveSerial2() +{ + // Check for new data availability in the Serial buffer + if (Serial2.available()) { + incomingByte = Serial2.read(); // Read the incoming byte + bufStartFrame = ((uint16_t)(incomingBytePrev) << 8) + incomingByte; // Construct the start frame + } + else { + return; + } + + // If DEBUG_RX is defined print all incoming bytes + #ifdef DEBUG_RX + Serial.print(incomingByte); + return; + #endif + + // Copy received data + if (bufStartFrame == START_FRAME) { // Initialize if new data is detected + p = (byte *)&NewFeedback; + *p++ = incomingBytePrev; + *p++ = incomingByte; + idx = 2; + } else if (idx >= 2 && idx < sizeof(SerialFeedback)) { // Save the new received data + *p++ = incomingByte; + idx++; + } + + // Check if we reached the end of the package + if (idx == sizeof(SerialFeedback)) { + uint16_t checksum; + checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2 ^ NewFeedback.speedR ^ NewFeedback.speedL + ^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.curL_DC ^ NewFeedback.curR_DC); + // Check validity of the new data + if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) { + + // Copy the new data + memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback)); + + // Print data to built-in Serial + /*Serial.print("1: "); Serial.print(Feedback.cmd1); + Serial.print(" 2: "); Serial.print(Feedback.cmd2); + Serial.print(" 3: "); Serial.print(Feedback.speedR); + Serial.print(" 4: "); Serial.print(Feedback.speedL); + Serial.print(" 5: "); Serial.print(Feedback.speedR_meas); + Serial.print(" 6: "); Serial.print(Feedback.speedL_meas); + //Serial.print(" 7: "); Serial.print(Feedback.batVoltage); + //Serial.print(" 8: "); Serial.println(Feedback.boardTemp); + Serial.print(" 9: "); Serial.print(Feedback.curL_DC); //in A, in hoverbrett negative sign for forward + Serial.print(" 10: "); Serial.println(Feedback.curR_DC); //in A, in hoverbrett negative sign for forward + */ + Serial.print(" 9: "); Serial.println(Feedback.curL_DC); //in A, in hoverbrett negative sign for forward + } else { + Serial.println("Non-valid data skipped"); + } + idx = 0; // Reset the index (it prevents to enter in this if condition in the next cycle) + } + + // Update previous states + incomingBytePrev = incomingByte; +} \ No newline at end of file diff --git a/controller_pio/test/README b/controller_pio/test/README new file mode 100644 index 0000000..df5066e --- /dev/null +++ b/controller_pio/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PIO Unit Testing and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PIO Unit Testing: +- https://docs.platformio.org/page/plus/unit-testing.html