interfisch
/
hoverbrett
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

port controller for bluepill to platformio because flashing didnt work anymore

This commit is contained in:
interfisch 2020-06-07 00:21:35 +02:00
parent b9a7f6e873
commit 454c49946d
11 changed files with 1031 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
controller/controller.ino
View File

@ -23,7 +23,7 @@
#define START_FRAME 0xAAAA // [-] Start frme definition for reliable serial communication #define START_FRAME 0xAAAA // [-] Start frme definition for reliable serial communication
//#define DEBUG //#define DEBUG
#define PARAMETEROUTPUT //#define PARAMETEROUTPUT
uint8_t error = 0; uint8_t error = 0;
#define IMU_NO_CHANGE 2 //IMU values did not change for too long #define IMU_NO_CHANGE 2 //IMU values did not change for too long
uint8_t imu_no_change_counter = 0; 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 ); set_steer = (int16_t)( ((int16_t)(lastnrfdata.steer) - NRFDATA_CENTER) * 1000 / 127 );
//align to compass //align to compass
/*
double yawdiff = (setYaw - 180) - (yaw - 180); //following angle difference works only for angles [-180,180]. yaw here is [0,360] 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 += (yawdiff > 180) ? -360 : (yawdiff < -180) ? 360 : 0;
//yawdiff/=2; //yawdiff/=2;
@ -406,6 +407,7 @@ void loop() {
magalign_multiplier = constrain(magalign_multiplier, 0.0, 1.0); //safety constrain again magalign_multiplier = constrain(magalign_multiplier, 0.0, 1.0); //safety constrain again
set_steer = set_steer * (1 - magalign_multiplier) + set_steer_mag * magalign_multiplier; 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 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(" 4: "); Serial.print(Feedback.speedL);
Serial.print(" 5: "); Serial.print(Feedback.speedR_meas); Serial.print(" 5: "); Serial.print(Feedback.speedR_meas);
Serial.print(" 6: "); Serial.print(Feedback.speedL_meas); Serial.print(" 6: "); Serial.print(Feedback.speedL_meas);
Serial.print(" 7: "); Serial.print(Feedback.batVoltage); //Serial.print(" 7: "); Serial.print(Feedback.batVoltage);
Serial.print(" 8: "); Serial.println(Feedback.boardTemp); //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(" 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(" 10: "); Serial.println(Feedback.curR_DC); //in A, in hoverbrett negative sign for forward
} else { } else {

1
controller_pio/.gitignore vendored Normal file
View File

@ -0,0 +1 @@
.pio

67
controller_pio/.travis.yml Normal file
View File

@ -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

142
controller_pio/.vscode/c_cpp_properties.json vendored Normal file
View File

@ -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
}

7
controller_pio/.vscode/extensions.json vendored Normal file
View File

@ -0,0 +1,7 @@
{
// See http://go.microsoft.com/fwlink/?LinkId=827846
// for the documentation about the extensions.json format
"recommendations": [
"platformio.platformio-ide"
]
}

34
controller_pio/.vscode/launch.json vendored Normal file
View File

@ -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"
}
]
}

39
controller_pio/include/README Normal file
View File

@ -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

46
controller_pio/lib/README Normal file
View File

@ -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 <Foo.h>
#include <Bar.h>
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

21
controller_pio/platformio.ini Normal file
View File

@ -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

658
controller_pio/src/main.cpp Normal file
View File

@ -0,0 +1,658 @@
#include <Arduino.h>
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 <IMUGY85.h>
//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 <SPI.h>
#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_set+10) { //is in trackable length
controlmode=MODE_GAMETRAK; //enable gametrak mode
}
}else if (controlmode==MODE_GAMETRAK){ //gametrak control active and not remote active
//Gametrak Control Code
motorenabled=true;
if (gt_length<=GT_LENGTH_MIN){ //let go
controlmode=MODE_DISARMED;
motorenabled=false;
}
int16_t _gt_length_diff = gt_length-gt_length_set; //positive if needs to drive forward
if (_gt_length_diff>-GT_LENGTH_MINDIFF & _gt_length_diff<GT_LENGTH_MINDIFF){ //minimum difference to drive
_gt_length_diff=0; //threshold
}
set_steer=constrain((int16_t)(-gt_horizontal*gt_steer_p),-GT_STEER_LIMIT,GT_STEER_LIMIT); //steer positive is left //gt_horizontal left is negative
if (_gt_length_diff>0) { //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 *) &gt_length, sizeof(gt_length)); //uint16_t, 2 bytes
Serial.write((uint8_t *) &gt_horizontal, sizeof(gt_horizontal)); //int8_t, 1 byte
Serial.write((uint8_t *) &gt_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;
}

11
controller_pio/test/README Normal file
View File

@ -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