diff --git a/controller/controller.ino b/controller/controller.ino index 4922a63..e857de4 100644 --- a/controller/controller.ino +++ b/controller/controller.ino @@ -11,7 +11,7 @@ // RX(green) is A10 , TX (blue) 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 +//set boot0 back to 0 to run proPIN_GAMETRAK_VERTICALgram on powerup // ########################## DEFINES ########################## #define SERIAL_CONTROL_BAUD 38400 // [-] Baud rate for HoverSerial (used to communicate with the hoverboard) @@ -233,7 +233,19 @@ void loop() { SendSerial1(out_speedRL,out_speedRR); //Rear } if (currentmode==on) { - Serial.print("lastData1="); Serial.print(loopmillis-lastValidDataSerial1_time); Serial.print(", lastData2=");Serial.print(loopmillis-lastValidDataSerial2_time); Serial.print(", speedFL="); Serial.println(out_speedFL); + /*Serial.print("lastData1="); Serial.print(loopmillis-lastValidDataSerial1_time); Serial.print(", lastData2=");Serial.print(loopmillis-lastValidDataSerial2_time); + Serial.print(", speedFL="); Serial.print(out_speedFL); + float _current = (Feedback1.curL_DC+Feedback1.curR_DC+Feedback2.curL_DC+Feedback2.curR_DC)/4.0 / 50; + Serial.print(", current="); Serial.print(_current);*/ + + float _current = (Feedback1.curL_DC+Feedback1.curR_DC+Feedback2.curL_DC+Feedback2.curR_DC)/4.0 / 50; + + Serial.print(Feedback1.curL_DC); Serial.print(", "); //1 is rear + Serial.print(Feedback1.curR_DC); Serial.print(", "); + Serial.print(Feedback2.curL_DC); Serial.print(", "); //2 is front + Serial.print(Feedback2.curR_DC); + Serial.print(", mean="); Serial.print(_current); + Serial.println(); } } @@ -483,16 +495,21 @@ void loop_on() { int16_t speedvalue=throttlevalue*(1- (((float)brakevalue)/_maxbrake)) - (brakevalue*(1- (((float)throttlevalue)/_maxspeed)) ); //brake reduces throttle and adds negative torque */ + + + + int16_t speedvalue = (out_speedFL+out_speedFR+out_speedRL+out_speedRR)/4; //generate last speedvalue from individual motor speeds + uint16_t throttlevalue=constrain( map(adc_throttle, ADC_CALIB_THROTTLE_MIN, ADC_CALIB_THROTTLE_MAX, 0, _maxspeed ) ,0, _maxspeed); - int16_t brakevalue=constrain( map(adc_brake, ADC_CALIB_BRAKE_MIN, ADC_CALIB_BRAKE_MAX, 0, _maxbrake ) ,0, _maxbrake); //positive value for braking - + int16_t brakevalue=constrain( map(adc_brake, ADC_CALIB_BRAKE_MIN, ADC_CALIB_BRAKE_MAX, 0, _maxbrake ) ,0, _maxbrake); //positive value for braking + int16_t combthrottlevalue=throttlevalue*(1- (((float)brakevalue)/_maxbrake)) - (brakevalue*(1- (((float)throttlevalue)/_maxspeed)) ); //brake reduces throttle and adds negative torque - int16_t combthrottlevalue_positive = max(0,combthrottlevalue); //only positive + int16_t combthrottlevalue_positive = max((int16_t)0,combthrottlevalue); //only positive - #define CURRENTBRAKE_P 2.0 //proportional brake when throttle is lower than current speed. Depends on LOOPTIME - #define BRAKE_P 0.1 //speed-=brakevalue*brake_p . depends on LOOPTIME + #define CURRENTBRAKE_P 5.0 //proportional brake when throttle is lower than current speed. Depends on LOOPTIME + #define BRAKE_P 0.02 //speed-=brakevalue*brake_p . depends on LOOPTIME //serial2 is Front. serial1 is Rear float _current = (Feedback1.curL_DC+Feedback1.curR_DC+Feedback2.curL_DC+Feedback2.curR_DC)/4.0 / A2BIT_CONV; @@ -501,14 +518,16 @@ void loop_on() { speedvalue = combthrottlevalue_positive; }else{ //throttle lever is lower than current set speedvalue if (_current > 0) { //is consuming current when it shouldnt - speedvalue = max( speedvalue-_current*CURRENTBRAKE_P ,combthrottlevalue_positive); //not lower than throttlevalue + speedvalue = max( (int16_t)(speedvalue-_current*CURRENTBRAKE_P) ,combthrottlevalue_positive); //not lower than throttlevalue } } if (combthrottlevalue<0){ //throttle off and brake pressed - speedvalue= max(speedvalue + combthrottlevalue*BRAKE_P,0); //not negative = not backwards + speedvalue= max((int16_t)(speedvalue + combthrottlevalue*BRAKE_P),(int16_t)0); //not negative = not backwards } + speedvalue = throttlevalue; //TEST OVERRIDE + out_speedFL=speedvalue; out_speedFR=speedvalue; out_speedRL=speedvalue; 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..c418e4d --- /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/bobbycar/bobbycar_repo/controller_pio/include", + "/media/fisch/HDD/Projects/bobbycar/bobbycar_repo/controller_pio/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", + "/media/fisch/HDD/Projects/bobbycar/bobbycar_repo/controller_pio/.pio/libdeps/genericSTM32F103C8/RF24_ID433", + "/media/fisch/HDD/Projects/bobbycar/bobbycar_repo/controller_pio/.pio/libdeps/genericSTM32F103C8/RF24_ID433/utility", + "/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/SPI/src", + "/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/bobbycar/bobbycar_repo/controller_pio/include", + "/media/fisch/HDD/Projects/bobbycar/bobbycar_repo/controller_pio/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", + "/media/fisch/HDD/Projects/bobbycar/bobbycar_repo/controller_pio/.pio/libdeps/genericSTM32F103C8/RF24_ID433", + "/media/fisch/HDD/Projects/bobbycar/bobbycar_repo/controller_pio/.pio/libdeps/genericSTM32F103C8/RF24_ID433/utility", + "/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/SPI/src", + "/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..b24e30e --- /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/bobbycar/bobbycar_repo/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/bobbycar/bobbycar_repo/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..b3f9365 --- /dev/null +++ b/controller_pio/platformio.ini @@ -0,0 +1,18 @@ +; 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 diff --git a/controller_pio/src/main.cpp b/controller_pio/src/main.cpp new file mode 100644 index 0000000..a5f26f1 --- /dev/null +++ b/controller_pio/src/main.cpp @@ -0,0 +1,1022 @@ +#include + +void handleInputs(); +void handleModeChange(); +void modeloops(); +void loop_idle(); +void loop_on(); +void loop_error(); +void loop_test(); +void loop_off(); +boolean boardsPowered(); +void failChecks(); +String modeToString(uint8_t m); +void ledUpdate(); +void selfTest(); + +void SendSerial1(int16_t uSpeedLeft, int16_t uSpeedRight); +void SendSerial2(int16_t uSpeedLeft, int16_t uSpeedRight); +void ReceiveSerial1(); +void ReceiveSerial2(); + + +//Board: Generic STM32F103C series +//Upload method: serial +//20k RAM 64k Flash +//may need 3v3 from usb ttl converter (hold down flash button while connecting). Holding down the power button is not needed in this case. +//Sometimes reconnecting the usb ttl converter to the pc helps just before pressing the upload button + +/* TODO: + * Do not immediately drive backwards. + */ + +// RX(green) is A10 , TX (blue) 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 + +// ########################## DEFINES ########################## +#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_RX // [-] Debug received data. Prints all bytes to serial (comment-out to disable) + +//#define MAXADCVALUE 4095 +#define ADC_CALIB_THROTTLE_LOWEST 1900 //a bit above adc value if throttle it not touched +#define ADC_CALIB_THROTTLE_MIN 2000 //minimum adc value that should correspond to 0 speed +#define ADC_CALIB_THROTTLE_MAX 3110 //maximum adc value that should correspond to full speed + +#define ADC_CALIB_BRAKE_MIN 800 //minimum adc value that should correspond to 0 speed +#define ADC_CALIB_BRAKE_MAX 2400 //maximum adc value that should correspond to full speed + +#define PIN_STARTLED PA0 //Red LED inside Engine Start Button. Powered with 5V via transistor +uint8_t startled=0; +#define PIN_STARTBUTTON PB8 //"Enginge Start" Button. connected To NC (=LOW). HIGH when pressed +#define STARTBUTTON_DOWN digitalRead(PIN_STARTBUTTON) + +#define SENDPERIOD 50 //ms. delay for sending speed and steer data to motor controller via serial +#define PIN_THROTTLE PA4 +#define PIN_BRAKE PA5 + +#define PIN_ENABLE PB9 + +#define PIN_MODESWITCH PB5 // LOW if pressed in ("down") +#define MODESWITCH_DOWN !digitalRead(PIN_MODESWITCH) +#define PIN_MODELED_GREEN PA12 +#define PIN_MODELED_RED PA11 +uint8_t modeled_green=0; +uint8_t modeled_red=0; +long last_ledupdate=0; + +#define PIN_RELAISFRONT PB15 //connected to relais which presses the powerbutton of the hoverboard for the front wheels +#define PIN_RELAISREAR PB14 //connected to relais which presses the powerbutton of the hoverboard for the rear wheels + +#define DEBOUNCETIME 20 //time to not check for inputs after key press +#define BUTTONTIMEHOLD 750 //time for button hold +long millis_lastinput=0; //for button debounce +long timebuttonpressed_start; +boolean button_start=false; +boolean button_hold_start=false; + + +#define TIME_AUTOPOWEROFF 600000 //600000 = 10 minutes + +long loopmillis=0; //only use one millis reading each loop +long last_looptime=0; //for looptiming +#define LOOPTIME 10 //how often the loop(s) should run +long millis_lastchange=0; //for poweroff after some time with no movement +#define MAXBRAKERATE 7 //maximum rate for braking (loop timing) + +String errormessage=""; //store some error message to print + +//Mode change variables +uint8_t state_modechange=0; +long state_modechange_time=0; + +long millis_lastadc=0; +#define ADC_READTIME 10 //time interval to read adc (for filtering) +#define ADC_THROTTLE_FILTER 0.4 //low value = slower change +#define ADC_BRAKE_FILTER 0.4 //low value = slower change + +int adc_throttle_raw=0; //raw throttle value from adc +float adc_throttle=0; //filtered value + +int adc_brake_raw=0; //raw throttle value from adc +float adc_brake=0; //filtered value + +int16_t out_speedFL=0; +int16_t out_speedFR=0; +int16_t out_speedRL=0; +int16_t out_speedRR=0; + + +long last_send = 0; + +boolean board1Enabled=false; +boolean board2Enabled=false; + + +// Global variables for serial communication +//Serial1 (Rear) +uint8_t idx1 = 0; // Index for new data pointer +uint16_t bufStartFrame1; // Buffer Start Frame +byte *p1; // Pointer declaration for the new received data +byte incomingByte1; +byte incomingBytePrev1; +long lastValidDataSerial1_time; +long board1lastPoweron=0; //mainly for failcheck +long board1lastPoweroff=0; + + +//Same for Serial2 (Front) +uint8_t idx2 = 0; // Index for new data pointer +uint16_t bufStartFrame2; // Buffer Start Frame +byte *p2; // Pointer declaration for the new received data +byte incomingByte2; +byte incomingBytePrev2; +long lastValidDataSerial2_time; +long board2lastPoweron=0; //mainly for failcheck +long board2lastPoweroff=0; + +typedef struct{ + uint16_t start; + int16_t speedLeft; + int16_t speedRight; + uint16_t checksum; +} SerialCommand; +SerialCommand Command1; +SerialCommand Command2; + +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 Feedback1; +SerialFeedback NewFeedback1; +SerialFeedback Feedback2; +SerialFeedback NewFeedback2; + + +#define A2BIT_CONV 50 //divide curL_DC value by A2BIT_CONV to get current in amperes. Take this value from hoverboard firmware config.h + + +enum mode{booting, idle, on, error, off}; +/* + * idle: controller is on, hoverboards are off + * on: hoverbaords are on and happy + * error: some error occured, stop everything and show errors + * off: shutdown triggered. will power down latch soon + */ +mode currentmode; //current active mode +mode requestmode; //change this variable to initiate a mode change + +mode last_requestmode=off; //for printout +mode last_currentmode=off; //for printout + +//#include "serialControl.cpp" + + +// ########################## SEND ########################## +void SendSerial1(int16_t uSpeedLeft, int16_t uSpeedRight) +{ + // Create command + Command1.start = (uint16_t)START_FRAME; + Command1.speedLeft = (int16_t)uSpeedLeft; + Command1.speedRight = (int16_t)uSpeedRight; + Command1.checksum = (uint16_t)(Command1.start ^ Command1.speedLeft ^ Command1.speedRight); + Serial1.write((uint8_t *) &Command1, sizeof(Command1)); +} +void SendSerial2(int16_t uSpeedLeft, int16_t uSpeedRight) +{ + // Create command + Command2.start = (uint16_t)START_FRAME; + Command2.speedLeft = (int16_t)uSpeedLeft; + Command2.speedRight = (int16_t)uSpeedRight; + Command2.checksum = (uint16_t)(Command2.start ^ Command2.speedLeft ^ Command2.speedRight); + Serial2.write((uint8_t *) &Command2, sizeof(Command2)); +} + +// ########################## RECEIVE ########################## +void ReceiveSerial1() +{ + // Check for new data availability in the Serial buffer + if ( Serial1.available() ) { + incomingByte1 = Serial1.read(); // Read the incoming byte + bufStartFrame1 = ((uint16_t)(incomingBytePrev1) << 8) + incomingByte1; // Construct the start frame + } + else { + return; + } + + // If DEBUG_RX is defined print all incoming bytes + #ifdef DEBUG_RX + Serial.print(incomingByte1); + return; + #endif + + // Copy received data + if (bufStartFrame1 == START_FRAME) { // Initialize if new data is detected + p1 = (byte *)&NewFeedback1; + *p1++ = incomingBytePrev1; + *p1++ = incomingByte1; + idx1 = 2; + } else if (idx1 >= 2 && idx1 < sizeof(SerialFeedback)) { // Save the new received data + *p1++ = incomingByte1; + idx1++; + } + + // Check if we reached the end of the package + if (idx1 == sizeof(SerialFeedback)) { + uint16_t checksum; + + checksum = (uint16_t)(NewFeedback1.start ^ NewFeedback1.cmd1 ^ NewFeedback1.cmd2 ^ NewFeedback1.speedR ^ NewFeedback1.speedL + ^ NewFeedback1.speedR_meas ^ NewFeedback1.speedL_meas ^ NewFeedback1.batVoltage ^ NewFeedback1.boardTemp ^ NewFeedback1.curL_DC ^ NewFeedback1.curR_DC); + + // Check validity of the new data + if (NewFeedback1.start == START_FRAME && checksum == NewFeedback1.checksum) { + // Copy the new data + memcpy(&Feedback1, &NewFeedback1, sizeof(SerialFeedback)); + lastValidDataSerial1_time = millis(); + } + idx1 = 0; // Reset the index (it prevents to enter in this if condition in the next cycle) + /* + // 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); + } else { + Serial.println("Non-valid data skipped"); + }*/ + + } + + // Update previous states + incomingBytePrev1 = incomingByte1; +} +void ReceiveSerial2() +{ + // Check for new data availability in the Serial buffer + if ( Serial2.available() ) { + incomingByte2 = Serial2.read(); // Read the incoming byte + bufStartFrame2 = ((uint16_t)(incomingBytePrev2) << 8) + incomingByte2; // Construct the start frame + } + else { + return; + } + + // If DEBUG_RX is defined print all incoming bytes + #ifdef DEBUG_RX + Serial.print(incomingByte2); + return; + #endif + + // Copy received data + if (bufStartFrame2 == START_FRAME) { // Initialize if new data is detected + p2 = (byte *)&NewFeedback2; + *p2++ = incomingBytePrev2; + *p2++ = incomingByte2; + idx2 = 2; + } else if (idx2 >= 2 && idx2 < sizeof(SerialFeedback)) { // Save the new received data + *p2++ = incomingByte2; + idx2++; + } + + // Check if we reached the end of the package + if (idx2 == sizeof(SerialFeedback)) { + uint16_t checksum; + checksum = (uint16_t)(NewFeedback2.start ^ NewFeedback2.cmd1 ^ NewFeedback2.cmd2 ^ NewFeedback2.speedR ^ NewFeedback2.speedL + ^ NewFeedback2.speedR_meas ^ NewFeedback2.speedL_meas ^ NewFeedback2.batVoltage ^ NewFeedback2.boardTemp ^ NewFeedback2.curL_DC ^ NewFeedback2.curR_DC); + + // Check validity of the new data + if (NewFeedback2.start == START_FRAME && checksum == NewFeedback2.checksum) { + // Copy the new data + memcpy(&Feedback2, &NewFeedback2, sizeof(SerialFeedback)); + lastValidDataSerial2_time = millis(); + } + idx2 = 0; // Reset the index (it prevents to enter in this if condition in the next cycle) + + } + + // Update previous states + incomingBytePrev2 = incomingByte2; +} + + + + + + + +// ########################## SETUP ########################## +void setup() +{ + + Serial.begin(SERIAL_BAUD); //Debug and Program. A9=TX1, A10=RX1 (3v3 level) + + Serial1.begin(SERIAL_CONTROL_BAUD); //control. A2=TX2, A3=RX2 (Serial1 is Usart 2). Marked with "1" on connector (Rear) + Serial2.begin(SERIAL_CONTROL_BAUD); //control. B10=TX3, B11=RX3 (Serial2 is Usart 3). Marked with "II" on connector (Front) + + // Pin Setup + pinMode(PIN_STARTLED, OUTPUT); //MODE=PWM (needs testing, mcu locks up when using writePWM() + pinMode(PIN_ENABLE, OUTPUT); + digitalWrite(PIN_ENABLE, HIGH); //keep power on + pinMode(PIN_STARTBUTTON, INPUT_PULLUP); + pinMode(PIN_MODESWITCH, INPUT_PULLUP); + pinMode(PIN_MODELED_GREEN, OUTPUT); + pinMode(PIN_MODELED_RED, OUTPUT); + pinMode(PIN_RELAISFRONT, OUTPUT); + pinMode(PIN_RELAISREAR, OUTPUT); + pinMode(PIN_THROTTLE, INPUT); + pinMode(PIN_BRAKE, INPUT); + + Serial.println("Initialized Serial"); + Serial1.println("Initialized Serial1"); + Serial2.println("Initialized Serial2"); + + currentmode = booting; //start in idle mode + requestmode = currentmode; + + millis_lastchange=millis(); +} + + +// ########################## LOOP ########################## +void loop() { + loopmillis=millis(); //read millis for this cycle + + ReceiveSerial1(); // Check for new received data + ReceiveSerial2(); // Check for new received data + + handleInputs(); + if (button_start) { + Serial.println("button_start"); + } + if (button_hold_start) { + Serial.println("button_hold_start"); + } + + handleModeChange(); //mode changes + if (last_requestmode!=requestmode) { + Serial.print("requestmode="); Serial.println(modeToString(requestmode)); + last_requestmode=requestmode; + } + if (last_currentmode!=currentmode) { + Serial.print("currentmode="); Serial.println(modeToString(currentmode)); + last_currentmode=currentmode; + } + + ledUpdate(); + + modeloops(); + + + if (loopmillis - last_send > SENDPERIOD) { + last_send=loopmillis; + + if (currentmode!=off || currentmode!=idle) { //if boards should be powered on + SendSerial2(out_speedFL,out_speedFR); //Front + SendSerial1(out_speedRL,out_speedRR); //Rear + } + if (currentmode==on) { + /*Serial.print("lastData1="); Serial.print(loopmillis-lastValidDataSerial1_time); Serial.print(", lastData2=");Serial.print(loopmillis-lastValidDataSerial2_time); + Serial.print(", speedFL="); Serial.print(out_speedFL); + float _current = (Feedback1.curL_DC+Feedback1.curR_DC+Feedback2.curL_DC+Feedback2.curR_DC)/4.0 / 50; + Serial.print(", current="); Serial.print(_current);*/ + + float _current = (Feedback1.curL_DC+Feedback1.curR_DC+Feedback2.curL_DC+Feedback2.curR_DC)/4.0 / 50; + + Serial.print(Feedback1.curL_DC); Serial.print(", "); //1 is rear + Serial.print(Feedback1.curR_DC); Serial.print(", "); + Serial.print(Feedback2.curL_DC); Serial.print(", "); //2 is front + Serial.print(Feedback2.curR_DC); + Serial.print(", mean="); Serial.print(_current); + Serial.println(); + } + } + + + + if (currentmode!=error) { //keep last errormessage + failChecks(); + } +} + + +void handleInputs() +{ + //Short press (true when button short pressed, on release) + button_start=false; + + //long press (true when button is held down for BUTTONTIMEHOLD, on time elapsed) + button_hold_start=false; + + if (loopmillis-millis_lastinput>DEBOUNCETIME) //Button debouncing + { + //Trigger + if (timebuttonpressed_start == 0 && STARTBUTTON_DOWN){ //first time pressed down. (low when pressed) + timebuttonpressed_start=loopmillis; //set time of button press + millis_lastinput=loopmillis; + }else if(timebuttonpressed_start != 0 && !STARTBUTTON_DOWN){ //button released (was pressed) + if (loopmillis-timebuttonpressed_start < BUTTONTIMEHOLD){ //short press + button_start=true; + } + timebuttonpressed_start=0; //re-enable after short press and release from hold + millis_lastinput=loopmillis; + }else if(loopmillis-timebuttonpressed_start >= BUTTONTIMEHOLD && timebuttonpressed_start>0){ //held down long enough and not already hold triggered + button_hold_start=true; + timebuttonpressed_start=-1; //-1 as flag for hold triggered + } + } + + if ( button_start || button_hold_start) { + millis_lastchange=loopmillis; //for auto poweroff + millis_lastinput=loopmillis; //for debouncing + } + + if (loopmillis-millis_lastadc>ADC_READTIME) { + adc_throttle_raw = analogRead(PIN_THROTTLE); + adc_throttle = adc_throttle*(1-ADC_THROTTLE_FILTER) + adc_throttle_raw*ADC_THROTTLE_FILTER; + + adc_brake_raw = analogRead(PIN_BRAKE); + adc_brake = adc_brake*(1-ADC_BRAKE_FILTER) + adc_brake_raw*ADC_BRAKE_FILTER; + + if (adc_throttle_raw >= ADC_CALIB_THROTTLE_MIN || adc_brake_raw >= ADC_CALIB_BRAKE_MIN) { //throttle or brake pressed + millis_lastchange=loopmillis; + } + millis_lastadc=loopmillis; + } + + if (loopmillis-millis_lastchange>TIME_AUTOPOWEROFF){ + requestmode = off; + } +} + + + +void handleModeChange() { + if (currentmode==requestmode) { //## Not currently changing modes ## + switch (currentmode) { //mode dependant + case booting: //on startup. active while start button is still pressed + if (button_start) { //button first release + requestmode=idle; //start in idle state + state_modechange=0; //reset state for safety + }//TODO else if (button_hold_start) { requestmode=on; } + break; + case idle: + if (button_hold_start){ //long press + requestmode=on; //long press switches betweeen idle and on + state_modechange=0; //start at state 0 + } + if (button_start) { //short press + requestmode=off; + state_modechange=0; + } + break; + case on: + if (button_hold_start){ //long press + requestmode=idle; //long press switches betweeen idle and on + state_modechange=0; //start at state 0 + } + if (button_start) { //short press + requestmode=off; + state_modechange=0; + } + break; + case error: + if (button_start) { //short press + requestmode=off; + state_modechange=0; + } + break; + case off: + break; + default: + currentmode=error; //something else? -> error + } + }else{ // ## Change requested ## + switch (requestmode) { //mode changes + case booting: + requestmode=error; + currentmode=requestmode; + errormessage="Change to booting mode cannot be requested"; + break; + case idle: case on: case off: //similar for on, idle and off + if (currentmode == booting) { //coming from booting mode + currentmode=idle; //switch directly without powering boards + requestmode=currentmode; //make shure it stay in this mode + state_modechange=0; + break; + } + if ( (state_modechange>0 || (requestmode==idle && boardsPowered()) || (requestmode==off && boardsPowered()) || (requestmode==on && !boardsPowered()) )) { //power cylce in progress OR need to power on/off boards + //Hoverboard powering + switch(state_modechange) { + case 0: + if (requestmode==on && (adc_throttle > ADC_CALIB_THROTTLE_LOWEST || adc_brake > ADC_CALIB_BRAKE_MIN) ) { //requested to turn on but throttle or brake is pressed + state_modechange=0; + requestmode=currentmode; //abort modechange + //TODO: led show aborted modechange + }else{ //everythings fine, turn on/off + digitalWrite(PIN_RELAISFRONT,HIGH); //simulate hoverboard power button press + //Front board is Serial2 + if (requestmode==on) { + board2Enabled=true; //assume board is online + board2lastPoweron=loopmillis; //save time at which board was powered on + // ### Request Idle or Off ### + }else if(requestmode==idle || requestmode==off) { + board2Enabled=false; //assume board is offline + board2lastPoweroff=loopmillis; //save time at which board was powered off + } + state_modechange++; + state_modechange_time=loopmillis; //set to current time + Serial.println("PIN_RELAISFRONT,HIGH"); + } + break; + case 1: + if (loopmillis - state_modechange_time > 200) { //wait some time + digitalWrite(PIN_RELAISFRONT,LOW); //release simulated button + state_modechange++; + state_modechange_time=loopmillis; //set to current time + Serial.println("PIN_RELAISFRONT,LOW"); + } + break; + case 2: + if (loopmillis - state_modechange_time > 200) { //wait some time + digitalWrite(PIN_RELAISREAR,HIGH); //simulate hoverboard power button press + //Rear board is Serial1 + if (requestmode==on) { + board1Enabled=true; //assume board is online + board1lastPoweron=loopmillis; //save time at which board was powered on + // ### Request Idle or Off ### + }else if(requestmode==idle || requestmode==off) { + board1Enabled=false; //assume board is offline + board1lastPoweroff=loopmillis; //save time at which board was powered off + } + state_modechange++; + state_modechange_time=loopmillis; //set to current time + Serial.println("PIN_RELAISREAR,HIGH"); + } + break; + case 3: + if (loopmillis - state_modechange_time > 200) { //wait some time + digitalWrite(PIN_RELAISREAR,LOW); //release simulated button + state_modechange++; + state_modechange_time=loopmillis; //set to current time + Serial.println("PIN_RELAISREAR,LOW"); + } + break; + case 4: + if (loopmillis - state_modechange_time > 1000) { //wait some time after turning on/off + state_modechange++; + state_modechange_time=loopmillis; //set to current time + Serial.println("Waiting finished"); + } + break; + default: //finished modechange + currentmode=requestmode; + state_modechange=0; + break; + } + }else{ + currentmode=requestmode; + state_modechange=0; //for safety + //Should not happen + Serial.print("Warning: power cycle not needed. board1Enabled="); Serial.print(board1Enabled); Serial.print("board2Enabled="); Serial.println(board2Enabled); + } + + break; + + + case error: + currentmode=error; //stay in this mode + break; + default: + currentmode=error; + } + } +} + + + +void modeloops() { + if (loopmillis - last_looptime >= LOOPTIME) { + last_looptime=loopmillis; + //loop_test(); //for testing (adc calibration prints). comment out following switch case + + switch (currentmode) { //mode changes + case booting: + break; + case idle: + loop_idle(); + break; + case on: + loop_on(); + break; + case error: + loop_error(); + break; + case off: + loop_off(); + break; + } + } +} + + +void loop_idle() { + out_speedFL=out_speedFR=out_speedRR=out_speedRL=0; //stop motors +} + +void loop_on() { + + int _maxspeed=1000; + int _maxbrake=400; + if (MODESWITCH_DOWN) { + _maxspeed=200; + _maxbrake=200; + } + + /* + uint16_t throttlevalue=constrain( map(adc_throttle, ADC_CALIB_THROTTLE_MIN, ADC_CALIB_THROTTLE_MAX, 0, _maxspeed ) ,0, _maxspeed); + + int16_t brakevalue=constrain( map(adc_brake, ADC_CALIB_BRAKE_MIN, ADC_CALIB_BRAKE_MAX, 0, _maxbrake ) ,0, _maxbrake); //positive value for braking + + int16_t speedvalue=throttlevalue*(1- (((float)brakevalue)/_maxbrake)) - (brakevalue*(1- (((float)throttlevalue)/_maxspeed)) ); //brake reduces throttle and adds negative torque + */ + + + + + + int16_t speedvalue = (out_speedFL+out_speedFR+out_speedRL+out_speedRR)/4; //generate last speedvalue from individual motor speeds + + + uint16_t throttlevalue=constrain( map(adc_throttle, ADC_CALIB_THROTTLE_MIN, ADC_CALIB_THROTTLE_MAX, 0, _maxspeed ) ,0, _maxspeed); + int16_t brakevalue=constrain( map(adc_brake, ADC_CALIB_BRAKE_MIN, ADC_CALIB_BRAKE_MAX, 0, _maxbrake ) ,0, _maxbrake); //positive value for braking + + int16_t combthrottlevalue=throttlevalue*(1- (((float)brakevalue)/_maxbrake)) - (brakevalue*(1- (((float)throttlevalue)/_maxspeed)) ); //brake reduces throttle and adds negative torque + int16_t combthrottlevalue_positive = max((int16_t)0,combthrottlevalue); //only positive + + #define CURRENTBRAKE_P 5.0 //proportional brake when throttle is lower than current speed. Depends on LOOPTIME + #define BRAKE_P 0.02 //speed-=brakevalue*brake_p . depends on LOOPTIME + + //serial2 is Front. serial1 is Rear + float _current = (Feedback1.curL_DC+Feedback1.curR_DC+Feedback2.curL_DC+Feedback2.curR_DC)/4.0 / A2BIT_CONV; + + if (combthrottlevalue_positive>=speedvalue) { //if throttle higher then apply immediately + speedvalue = combthrottlevalue_positive; + }else{ //throttle lever is lower than current set speedvalue + if (_current > 0) { //is consuming current when it shouldnt + speedvalue = max( (int16_t)(speedvalue-_current*CURRENTBRAKE_P) ,combthrottlevalue_positive); //not lower than throttlevalue + } + } + + if (combthrottlevalue<0){ //throttle off and brake pressed + speedvalue= max((int16_t)(speedvalue + combthrottlevalue*BRAKE_P),(int16_t)0); //not negative = not backwards + } + + speedvalue = throttlevalue; //TEST OVERRIDE + + out_speedFL=speedvalue; + out_speedFR=speedvalue; + out_speedRL=speedvalue; + out_speedRR=speedvalue; + +} + + + + +void loop_error() { + out_speedFL=out_speedFR=out_speedRR=out_speedRL=0; //stop motors + Serial.print("Error:"); Serial.println(errormessage); +} + +void loop_test() { + Serial.print("adc_throttle_raw="); Serial.print(adc_throttle_raw); + Serial.print(", adc_brake_raw="); Serial.print(adc_brake_raw); + + int _maxspeed=1000; + int _maxbrake=400; + if (MODESWITCH_DOWN) { + _maxspeed=200; + _maxbrake=200; + } + int16_t throttlevalue=constrain( map(adc_throttle, ADC_CALIB_THROTTLE_MIN, ADC_CALIB_THROTTLE_MAX, 0, _maxspeed ) ,0, _maxspeed); + + + int16_t brakevalue=constrain( map(adc_brake, ADC_CALIB_BRAKE_MIN, ADC_CALIB_BRAKE_MAX, 0, _maxbrake ) ,0, _maxbrake); //positive value for braking + + int16_t speedvalue=throttlevalue*(1- (((float)brakevalue)/_maxbrake)) - (brakevalue*(1- (((float)throttlevalue)/_maxspeed)) ); //brake reduces throttle and adds negative torque + Serial.print(", throttle="); Serial.print(throttlevalue); Serial.print(", brake="); Serial.print(brakevalue); Serial.print(", speed="); Serial.println(speedvalue); +} + + + +void loop_off() { + //loop enters when boards are sucessfully turned off + digitalWrite(PIN_ENABLE, LOW); //cut own power + +} + +boolean boardsPowered() +{ + return (board1Enabled && board2Enabled); //true if both boards enabled +} + +void failChecks() +{ + #define FAILCHECK_WAITCHECK_AFTER_POWEROFF_TIME 1000 //time to start failchecking boardpower after board poweroff + #define FAILCHECK_RECEIVERECENT_TIME 100 //timeout .should be less than FAILCHECK_WAITCHECK_AFTER_POWEROFF_TIME and greater than send delay from mainboard + // ## Check if board is really offline ## + if (!board1Enabled) { //board should be offline + if (loopmillis-board1lastPoweroff > FAILCHECK_WAITCHECK_AFTER_POWEROFF_TIME){ //wait some time before checking if board did power off + if (loopmillis-lastValidDataSerial1_time < FAILCHECK_RECEIVERECENT_TIME) { //new message received recently? + errormessage="Board 1 should be offline but feedback received"; + Serial.println(errormessage); + requestmode=error; + } + } + } + if (!board2Enabled) { //board should be offline + if (loopmillis-board2lastPoweroff > FAILCHECK_WAITCHECK_AFTER_POWEROFF_TIME){ //wait some time before checking if board did power off + if (loopmillis-lastValidDataSerial2_time < FAILCHECK_RECEIVERECENT_TIME) { //new message received recently? + errormessage="Board 2 should be offline but feedback received"; + Serial.println(errormessage); + requestmode=error; + } + } + } + + + #define FAILCHECK_WAITCHECK_AFTER_POWERON_TIME 2000 //time to start failchecking boardpower after startup + // ## Check if board is online (when it should send feedback) ## + if (board1Enabled) { //board should be online + if (loopmillis-board1lastPoweron > FAILCHECK_WAITCHECK_AFTER_POWERON_TIME) { //wait some time before checking + if (loopmillis-lastValidDataSerial1_time > FAILCHECK_RECEIVERECENT_TIME) { //speed still high enough but no new messages recently received? + errormessage="Board 1 should be online and give feedback but didnt"; + Serial.println(errormessage); + requestmode=error; + } + } + } + if (board2Enabled) { //board should be online + if (loopmillis-board2lastPoweron > FAILCHECK_WAITCHECK_AFTER_POWERON_TIME) { //wait some time before checking + if (loopmillis-lastValidDataSerial2_time > FAILCHECK_RECEIVERECENT_TIME) { //no new messages recently received? + errormessage="Board 2 should be online and give feedback but didnt"; + Serial.println(errormessage); + requestmode=error; + } + } + } + +} + + +String modeToString(uint8_t m) { + if (m==idle) return "idle"; + if (m==off) return "off"; + if (m==error) return "error"; + if (m==on) return "on"; + if (m==booting) return "booting"; +} + + +void ledUpdate() { + #define LEDUPDATETIME 20 + #define FASTERRORBLINKDELAY 100 //period for startled to blink on error + if (loopmillis - last_ledupdate >= LEDUPDATETIME) { + last_ledupdate=loopmillis; + // ## StartLed ## + uint8_t _ledbrightness; + switch (currentmode) { //modeLed for different currentmodes + case booting: + startled=255; + break; + case idle: //Breathing Startled + _ledbrightness=uint8_t( (loopmillis/10)%(512) ); + startled=_ledbrightness<=255 ? _ledbrightness : (512)-_ledbrightness; //reverse if >255 to go down again + break; + case on: //Startled on + startled=255; + break; + case error: //Startled blink + startled=(loopmillis/FASTERRORBLINKDELAY)%2==0 ? 0 : 255; // Blink led + break; + case off: //Startled off + startled=0; + break; + } + + // ## ModeLed ## + if (currentmode!=requestmode) { //ongoing modechange + modeled_green=0; modeled_red=0; //ModeLed=Off + }else{ + switch (currentmode) { //modeLed for different currentmodes + case booting: + modeled_green=255; modeled_red=0; //ModeLed=Green + break; + case idle: + modeled_green=255; modeled_red=255; //ModeLed=Yellow + break; + case on: + modeled_green=255; modeled_red=0; //ModeLed=Green + break; + case error: + modeled_green=0; modeled_red=(loopmillis/FASTERRORBLINKDELAY)%2==0 ? 0 : 255; // Blink led , ModeLed=Red + break; + case off: + modeled_green=255; modeled_red=255; //ModeLed=Yellow + break; + } + } + /* + pwmWrite(PIN_MODELED_GREEN, map(modeled_green, 0, 255, 0, 65535)); + pwmWrite(PIN_MODELED_RED, map(modeled_red, 0, 255, 0, 65535)); + pwmWrite(PIN_STARTLED, map(startled, 0, 255, 0, 65535)); + */ + digitalWrite(PIN_MODELED_GREEN, modeled_green<127? true:false); //red and green inverted (common anode) + digitalWrite(PIN_MODELED_RED, modeled_red<127? true:false); //red and green inverted (common anode) + digitalWrite(PIN_STARTLED, startled>127? true:false); + } +} + + + + +/* +// Old loop +void loopold() { + //selfTest(); //start selftest, does not return + + ReceiveSerial1(); // Check for new received data + + if (millis()>2000 && STARTBUTTON_DOWN) { + poweronBoards(); + } + + if (millis() - last_send > SENDPERIOD) { + //Serial.print("powerbutton="); Serial.print(STARTBUTTON_DOWN); Serial.print(" modeswitch down="); Serial.println(MODESWITCH_DOWN); + + int _read=analogRead(PIN_THROTTLE); + + int16_t speedvalue=constrain( map(_read, ADC_CALIB_THROTTLE_MIN, ADC_CALIB_THROTTLE_MAX, 0, 1000 ) ,0, 1000); + + if (MODESWITCH_DOWN) { + SendSerial1(speedvalue,0); + SendSerial2(speedvalue,0); + Serial.print("L_"); + }else{ + SendSerial1(0,speedvalue); + SendSerial2(0,speedvalue); + Serial.print("R_"); + } + Serial.print("millis="); Serial.print(millis()); Serial.print(", adcthrottle="); Serial.print(_read); + Serial.print(", 1.L="); Serial.print(Command1.speedLeft); Serial.print(", 1.R="); Serial.print(Command1.speedRight); + Serial.print(", 2.L="); Serial.print(Command2.speedLeft); Serial.print(", 2.R="); Serial.println(Command2.speedRight); + + last_send = millis(); + + digitalWrite(PIN_STARTLED, !digitalRead(PIN_STARTLED)); + if (testcounter%3==0) { + digitalWrite(PIN_MODELED_GREEN, !digitalRead(PIN_MODELED_GREEN)); + } + if (testcounter%5==0) { + digitalWrite(PIN_MODELED_RED, !digitalRead(PIN_MODELED_RED)); + } + + testcounter++; + + + //Print Motor values + Serial.print("cmd1"); + Serial.print(", "); Serial.print("cmd2"); + Serial.print(","); Serial.print("speedR"); + Serial.print(","); Serial.print("speedL"); + Serial.print(", "); Serial.print("speedR_meas"); + Serial.print(","); Serial.print("speedL_meas"); + Serial.print(", "); Serial.print("batVoltage"); + Serial.print(", "); Serial.println("boardTemp"); + Serial.println(); + Serial.print("1: "); Serial.print(Feedback1.cmd1); + Serial.print(", "); Serial.print(Feedback1.cmd2); + Serial.print(","); Serial.print(Feedback1.speedR); + Serial.print(","); Serial.print(Feedback1.speedL); + Serial.print(", "); Serial.print(Feedback1.speedR_meas); + Serial.print(","); Serial.print(Feedback1.speedL_meas); + Serial.print(", "); Serial.print(Feedback1.batVoltage); + Serial.print(", "); Serial.println(Feedback1.boardTemp); + Serial.println(); + Serial.print("2: "); Serial.print(Feedback2.cmd1); + Serial.print(", "); Serial.print(Feedback2.cmd2); + Serial.print(","); Serial.print(Feedback2.speedR); + Serial.print(","); Serial.print(Feedback2.speedL); + Serial.print(", "); Serial.print(Feedback2.speedR_meas); + Serial.print(","); Serial.print(Feedback2.speedL_meas); + Serial.print(", "); Serial.print(Feedback2.batVoltage); + Serial.print(", "); Serial.println(Feedback2.boardTemp); + } + + if (millis()>30000 && STARTBUTTON_DOWN) { + poweroff(); + } + +} + +// ########################## END ########################## + + +void poweroff() { + + //TODO: trigger Relais for Board 1 + // Wait for board to shut down + //TODO: trigger Relais for Board 2 + // Wait for board to shut down + + //Timeout error handling + + digitalWrite(PIN_ENABLE, LOW); //poweroff own latch + + delay(1000); + Serial.println("Still powered"); + //still powered on: set error status "power latch error" +} + +void poweronBoards() { + digitalWrite(PIN_RELAISFRONT,HIGH); + delay(200);digitalWrite(PIN_RELAISFRONT,LOW); + delay(50); + digitalWrite(PIN_RELAISREAR,HIGH); + delay(200);digitalWrite(PIN_RELAISREAR,LOW); +} + + +*/ + + + +void selfTest() { + digitalWrite(PIN_ENABLE,HIGH); //make shure latch is on + Serial.println("Entering selftest"); + #define TESTDELAY 1000 //delay between test + #define TESTTIME 500 //time to keep tested pin on + + delay(TESTDELAY); Serial.println("PIN_STARTLED"); + digitalWrite(PIN_STARTLED,HIGH); delay(TESTTIME); digitalWrite(PIN_STARTLED,LOW); + + delay(TESTDELAY); Serial.println("PIN_MODELED_GREEN"); + digitalWrite(PIN_MODELED_GREEN,LOW); delay(TESTTIME); digitalWrite(PIN_MODELED_GREEN,HIGH); + + delay(TESTDELAY); Serial.println("PIN_MODELED_RED"); + digitalWrite(PIN_MODELED_RED,LOW); delay(TESTTIME); digitalWrite(PIN_MODELED_RED,HIGH); + + delay(TESTDELAY); Serial.println("PIN_RELAISFRONT"); + digitalWrite(PIN_RELAISFRONT,HIGH); delay(TESTTIME); digitalWrite(PIN_RELAISFRONT,LOW); + + delay(TESTDELAY); Serial.println("PIN_RELAISREAR"); + digitalWrite(PIN_RELAISREAR,HIGH); delay(TESTTIME); digitalWrite(PIN_RELAISREAR,LOW); + + delay(TESTDELAY); Serial.println("ALL ON"); + digitalWrite(PIN_STARTLED,HIGH); + digitalWrite(PIN_MODELED_GREEN,LOW); + digitalWrite(PIN_MODELED_RED,LOW); + digitalWrite(PIN_RELAISFRONT,HIGH); + digitalWrite(PIN_RELAISREAR,HIGH); + delay(TESTTIME*5); + digitalWrite(PIN_STARTLED,LOW); + digitalWrite(PIN_MODELED_GREEN,HIGH); + digitalWrite(PIN_MODELED_RED,HIGH); + digitalWrite(PIN_RELAISFRONT,LOW); + digitalWrite(PIN_RELAISREAR,LOW); + delay(TESTDELAY); + + Serial.println("Powers off latch at millis>=60000"); + Serial.println("Inputs:"); + while(true) { //Keep printing input values forever + delay(100); + Serial.print("millis="); Serial.print(millis()); Serial.print(", throttle ADC="); Serial.println(analogRead(PIN_THROTTLE)); + Serial.print("powerbutton down="); Serial.print(STARTBUTTON_DOWN); Serial.print(" modeswitch down="); Serial.println(MODESWITCH_DOWN); + + while (millis()>=60000) { + digitalWrite(PIN_ENABLE, LOW); //poweroff own latch + Serial.println(millis()); + } + } + + + +} 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