Update README.md

ADD: define to change gametrak ADC wirering in software
2019-01-04 08:56:05 +01:00 · 2019-01-04 08:55:36 +01:00 · 2018-12-16 09:58:14 +01:00 · 2018-12-16 09:54:53 +01:00 · 2018-12-06 17:10:30 +01:00 · 2018-12-01 23:54:08 +01:00
15 changed files with 3800 additions and 1464 deletions
--- a/Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_i2c.c
+++ b/Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_i2c.c
--- a/Inc/config.h
+++ b/Inc/config.h
@ -6,7 +6,7 @@
 #define PWM_FREQ         16000      // PWM frequency in Hz
 #define DEAD_TIME        32         // PWM deadtime

-#define DELAY_IN_MAIN_LOOP 5        // in ms. default 5. it is independent of all the timing critical stuff. do not touch if you do not know what you are doing.
+#define DELAY_IN_MAIN_LOOP 2        // in ms. default 5. it is independent of all the timing critical stuff. do not touch if you do not know what you are doing.

 #define TIMEOUT          5          // number of wrong / missing input commands before emergency off

@ -15,15 +15,15 @@
 // How to calibrate: connect GND and RX of a 3.3v uart-usb adapter to the right sensor board cable (be careful not to use the red wire of the cable. 15v will destroye verything.). if you are using nunchuck, disable it temporarily. enable DEBUG_SERIAL_USART3 and DEBUG_SERIAL_ASCII use asearial terminal.

 // Battery voltage calibration: connect power source. see <How to calibrate>. write value nr 5 to BAT_CALIB_ADC. make and flash firmware. then you can verify voltage on value 6 (devide it by 100.0 to get calibrated voltage).
-#define BAT_CALIB_REAL_VOLTAGE        43.0       // input voltage measured by multimeter  
+#define BAT_CALIB_REAL_VOLTAGE        43.0       // input voltage measured by multimeter
 #define BAT_CALIB_ADC                 1704       // adc-value measured by mainboard (value nr 5 on UART debug output)

 #define BAT_NUMBER_OF_CELLS     10        // normal Hoverboard battery: 10s
 #define BAT_LOW_LVL1_ENABLE     0         // to beep or not to beep, 1 or 0
-#define BAT_LOW_LVL1            3.6       // gently beeps at this voltage level. [V/cell]
+#define BAT_LOW_LVL1            3.5       // gently beeps at this voltage level. [V/cell]
 #define BAT_LOW_LVL2_ENABLE     1         // to beep or not to beep, 1 or 0
-#define BAT_LOW_LVL2            3.5       // your battery is almost empty. Charge now! [V/cell]
-#define BAT_LOW_DEAD            3.37      // undervoltage poweroff. (while not driving) [V/cell]
+#define BAT_LOW_LVL2            3.3       // your battery is almost empty. Charge now! [V/cell]
+#define BAT_LOW_DEAD            3.2      // undervoltage poweroff. (while not driving) [V/cell]

 #define DC_CUR_LIMIT     15         // DC current limit in amps per motor. so 15 means it will draw 30A out of your battery. it does not disable motors, it is a soft current limit.

@ -39,97 +39,34 @@

 #define INACTIVITY_TIMEOUT 8        // minutes of not driving until poweroff. it is not very precise.

-// ############################### LCD DEBUG ###############################
-
-//#define DEBUG_I2C_LCD             // standard 16x2 or larger text-lcd via i2c-converter on right sensor board cable
-
 // ############################### SERIAL DEBUG ###############################

-#define DEBUG_SERIAL_USART3         // right sensor board cable, disable if I2C (nunchuck or lcd) is used!
-#define DEBUG_BAUD       115200     // UART baud rate
+//#define DEBUG_SERIAL_USART3         // right sensor board cable, disable if I2C (nunchuck or lcd) is used!
+//#define DEBUG_BAUD       115200     // UART baud rate
 //#define DEBUG_SERIAL_SERVOTERM
-#define DEBUG_SERIAL_ASCII          // "1:345 2:1337 3:0 4:0 5:0 6:0 7:0 8:0\r\n"
+//#define DEBUG_SERIAL_ASCII          // "1:345 2:1337 3:0 4:0 5:0 6:0 7:0 8:0\r\n"

-// ############################### INPUT ###############################
+// ###### TRANSPOTTER FIRMWARE #######
+#define CONTROL_GAMETRAK
+#define SUPPORT_LCD
+#define SUPPORT_NUNCHUCK
+//#define SUPPORT_REMOTE

-// ###### CONTROL VIA UART (serial) ######
-//#define CONTROL_SERIAL_USART2       // left sensor board cable, disable if ADC or PPM is used!
-#define CONTROL_BAUD       19200    // control via usart from eg an Arduino or raspberry
-// for Arduino, use void loop(void){ Serial.write((uint8_t *) &steer, sizeof(steer)); Serial.write((uint8_t *) &speed, sizeof(speed));delay(20); }
+#define GAMETRAK_CONNECTION_NORMAL    // for normal wireing accoring to the wiki Instructions
+//#define GAMETRAK_CONNECTION_ALTERNATE // use this define instead if you messed up the gametrak ADC wirering (steering is speed, and length of teh wire is steering)

-// ###### CONTROL VIA RC REMOTE ######
-// left sensor board cable. Channel 1: steering, Channel 2: speed.
-//#define CONTROL_PPM                 // use PPM-Sum as input. disable DEBUG_SERIAL_USART2!
-//#define PPM_NUM_CHANNELS 6          // total number of PPM channels to receive, even if they are not used.
+#define ROT_P         -1.2  // P coefficient for the direction controller. Positive / Negative values to invert gametrak steering direction.
+#define FILTER         0.1  // lower value == softer filter. do not use values <0.01, you will get float precision issues.

-// ###### CONTROL VIA TWO POTENTIOMETERS ######
-// ADC-calibration to cover the full poti-range: connect potis to left sensor board cable (0 to 3.3V) (do NOT use the red 15V wire in the cable!). see <How to calibrate>. turn the potis to minimum position, write value 1 to ADC1_MIN and value 2 to ADC2_MIN. turn to maximum position and repeat it for ADC?_MAX. make, flash and test it.
-#define CONTROL_ADC                 // use ADC as input. disable DEBUG_SERIAL_USART2!
-#define ADC1_MIN 0                // min ADC1-value while poti at minimum-position (0 - 4095)
-#define ADC1_MAX 4095               // max ADC1-value while poti at maximum-position (0 - 4095)
-#define ADC2_MIN 0                // min ADC2-value while poti at minimum-position (0 - 4095)
-#define ADC2_MAX 4095               // max ADC2-value while poti at maximum-position (0 - 4095)
+#define INVERT_R_DIRECTION  // Invert right motor
+#define INVERT_L_DIRECTION  // Invert left motor

-// ###### CONTROL VIA NINTENDO NUNCHUCK ######
-// left sensor board cable. keep cable short, use shielded cable, use ferrits, stabalize voltage in nunchuck, use the right one of the 2 types of nunchucks, add i2c pullups. use original nunchuck. most clones does not work very well.
-//#define CONTROL_NUNCHUCK            // use nunchuck as input. disable DEBUG_SERIAL_USART3!
-
-// ############################### DRIVING BEHAVIOR ###############################
-
-// inputs:
-// - cmd1 and cmd2: analog normalized input values. -1000 to 1000
-// - button1 and button2: digital input values. 0 or 1
-// - adc_buffer.l_tx2 and adc_buffer.l_rx2: unfiltered ADC values (you do not need them). 0 to 4095
-// outputs:
-// - speedR and speedL: normal driving -1000 to 1000
-// - weakr and weakl: field weakening for extra boost at high speed (speedR > 700 and speedL > 700). 0 to ~400
-
-#define FILTER              0.1  // lower value == softer filter. do not use values <0.01, you will get float precision issues.
-#define SPEED_COEFFICIENT   0.5  // higher value == stronger. 0.0 to ~2.0?
+// during nunchuck control (only relevant when activated)
+#define SPEED_COEFFICIENT   0.9  // higher value == stronger. 0.0 to ~2.0?
 #define STEER_COEFFICIENT   0.5  // higher value == stronger. if you do not want any steering, set it to 0.0; 0.0 to 1.0
-#define INVERT_R_DIRECTION
-#define INVERT_L_DIRECTION
+
 #define BEEPS_BACKWARD 1    // 0 or 1

-//Turbo boost at high speeds while button1 is pressed:
-//#define ADDITIONAL_CODE \
-if (button1 && speedR > 700) { /* field weakening at high speeds */ \
-  weakl = cmd1 - 700; /* weak should never exceed 400 or 450 MAX!! */ \
-  weakr = cmd1 - 700; } \
-else { \
-  weakl = 0; \
-  weakr = 0; }
-
-// ###### SIMPLE BOBBYCAR ######
-// for better bobbycar code see: https://github.com/larsmm/hoverboard-firmware-hack-bbcar
-// #define FILTER              0.1
-// #define SPEED_COEFFICIENT   -1
-// #define STEER_COEFFICIENT   0
-
-// #define ADDITIONAL_CODE \
-if (button1 && speedR < 300) { /* drive backwards */ \
-  speedR = speedR * -0.2f;   \
-  speedL = speedL * -0.2f; } \
-else { \
-  direction = 1; } \
-if (button1 && speedR > 700) { /* field weakening at high speeds */ \
-  weakl = speedR - 600; /* weak should never exceed 400 or 450 MAX!! */ \
-  weakr = speedR - 600; } \
-else { \
-  weakl = 0; \
-  weakr = 0; }
-
-// ###### ARMCHAIR ######
-// #define FILTER              0.05
-// #define SPEED_COEFFICIENT   0.5
-// #define STEER_COEFFICIENT   -0.2
-
-// #define ADDITIONAL_CODE if (button1 && scale > 0.8) { /* field weakening at high speeds */ \
-  weakl = speedL - 600; /* weak should never exceed 400 or 450 MAX!! */ \
-  weakr = speedR - 600; } \
-else {\
-  weakl = 0;\
-  weakr = 0;

 // ############################### VALIDATE SETTINGS ###############################

--- a/Inc/eeprom.h
+++ b/Inc/eeprom.h
@ -0,0 +1,223 @@
+/**
+  ******************************************************************************
+  * @file    EEPROM_Emulation/inc/eeprom.h 
+  * @author  MCD Application Team
+  * @version V1.3.0
+  * @date    18-December-2015
+  * @brief   This file contains all the functions prototypes for the EEPROM 
+  *          emulation firmware library.
+  ******************************************************************************
+  * @attention
+  *
+  * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
+  *
+  * Redistribution and use in source and binary forms, with or without modification,
+  * are permitted provided that the following conditions are met:
+  *   1. Redistributions of source code must retain the above copyright notice,
+  *      this list of conditions and the following disclaimer.
+  *   2. Redistributions in binary form must reproduce the above copyright notice,
+  *      this list of conditions and the following disclaimer in the documentation
+  *      and/or other materials provided with the distribution.
+  *   3. Neither the name of STMicroelectronics nor the names of its contributors
+  *      may be used to endorse or promote products derived from this software
+  *      without specific prior written permission.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  ******************************************************************************
+  */ 
+
+/* Define to prevent recursive inclusion -------------------------------------*/
+#ifndef __EEPROM_H
+#define __EEPROM_H
+
+/* Includes ------------------------------------------------------------------*/
+#include "stm32f1xx_hal.h"
+
+/* Exported constants --------------------------------------------------------*/
+/* Base address of the Flash sectors */
+#define ADDR_FLASH_PAGE_0     ((uint32_t)0x08000000) /* Base @ of Page 0, 1 Kbytes */
+#define ADDR_FLASH_PAGE_1     ((uint32_t)0x08000400) /* Base @ of Page 1, 1 Kbytes */
+#define ADDR_FLASH_PAGE_2     ((uint32_t)0x08000800) /* Base @ of Page 2, 1 Kbytes */
+#define ADDR_FLASH_PAGE_3     ((uint32_t)0x08000C00) /* Base @ of Page 3, 1 Kbytes */
+#define ADDR_FLASH_PAGE_4     ((uint32_t)0x08001000) /* Base @ of Page 4, 1 Kbytes */
+#define ADDR_FLASH_PAGE_5     ((uint32_t)0x08001400) /* Base @ of Page 5, 1 Kbytes */
+#define ADDR_FLASH_PAGE_6     ((uint32_t)0x08001800) /* Base @ of Page 6, 1 Kbytes */
+#define ADDR_FLASH_PAGE_7     ((uint32_t)0x08001C00) /* Base @ of Page 7, 1 Kbytes */
+#define ADDR_FLASH_PAGE_8     ((uint32_t)0x08002000) /* Base @ of Page 8, 1 Kbytes */
+#define ADDR_FLASH_PAGE_9     ((uint32_t)0x08002400) /* Base @ of Page 9, 1 Kbytes */
+#define ADDR_FLASH_PAGE_10    ((uint32_t)0x08002800) /* Base @ of Page 10, 1 Kbytes */
+#define ADDR_FLASH_PAGE_11    ((uint32_t)0x08002C00) /* Base @ of Page 11, 1 Kbytes */
+#define ADDR_FLASH_PAGE_12    ((uint32_t)0x08003000) /* Base @ of Page 12, 1 Kbytes */
+#define ADDR_FLASH_PAGE_13    ((uint32_t)0x08003400) /* Base @ of Page 13, 1 Kbytes */
+#define ADDR_FLASH_PAGE_14    ((uint32_t)0x08003800) /* Base @ of Page 14, 1 Kbytes */
+#define ADDR_FLASH_PAGE_15    ((uint32_t)0x08003C00) /* Base @ of Page 15, 1 Kbytes */
+#define ADDR_FLASH_PAGE_16    ((uint32_t)0x08004000) /* Base @ of Page 16, 1 Kbytes */
+#define ADDR_FLASH_PAGE_17    ((uint32_t)0x08004400) /* Base @ of Page 17, 1 Kbytes */
+#define ADDR_FLASH_PAGE_18    ((uint32_t)0x08004800) /* Base @ of Page 18, 1 Kbytes */
+#define ADDR_FLASH_PAGE_19    ((uint32_t)0x08004C00) /* Base @ of Page 19, 1 Kbytes */
+#define ADDR_FLASH_PAGE_20    ((uint32_t)0x08005000) /* Base @ of Page 20, 1 Kbytes */
+#define ADDR_FLASH_PAGE_21    ((uint32_t)0x08005400) /* Base @ of Page 21, 1 Kbytes */
+#define ADDR_FLASH_PAGE_22    ((uint32_t)0x08005800) /* Base @ of Page 22, 1 Kbytes */
+#define ADDR_FLASH_PAGE_23    ((uint32_t)0x08005C00) /* Base @ of Page 23, 1 Kbytes */
+#define ADDR_FLASH_PAGE_24    ((uint32_t)0x08006000) /* Base @ of Page 24, 1 Kbytes */
+#define ADDR_FLASH_PAGE_25    ((uint32_t)0x08006400) /* Base @ of Page 25, 1 Kbytes */
+#define ADDR_FLASH_PAGE_26    ((uint32_t)0x08006800) /* Base @ of Page 26, 1 Kbytes */
+#define ADDR_FLASH_PAGE_27    ((uint32_t)0x08006C00) /* Base @ of Page 27, 1 Kbytes */
+#define ADDR_FLASH_PAGE_28    ((uint32_t)0x08007000) /* Base @ of Page 28, 1 Kbytes */
+#define ADDR_FLASH_PAGE_29    ((uint32_t)0x08007400) /* Base @ of Page 29, 1 Kbytes */
+#define ADDR_FLASH_PAGE_30    ((uint32_t)0x08007800) /* Base @ of Page 30, 1 Kbytes */
+#define ADDR_FLASH_PAGE_31    ((uint32_t)0x08007C00) /* Base @ of Page 31, 1 Kbytes */
+#define ADDR_FLASH_PAGE_32    ((uint32_t)0x08008000) /* Base @ of Page 32, 1 Kbytes */
+#define ADDR_FLASH_PAGE_33    ((uint32_t)0x08008400) /* Base @ of Page 33, 1 Kbytes */
+#define ADDR_FLASH_PAGE_34    ((uint32_t)0x08008800) /* Base @ of Page 34, 1 Kbytes */
+#define ADDR_FLASH_PAGE_35    ((uint32_t)0x08008C00) /* Base @ of Page 35, 1 Kbytes */
+#define ADDR_FLASH_PAGE_36    ((uint32_t)0x08009000) /* Base @ of Page 36, 1 Kbytes */
+#define ADDR_FLASH_PAGE_37    ((uint32_t)0x08009400) /* Base @ of Page 37, 1 Kbytes */
+#define ADDR_FLASH_PAGE_38    ((uint32_t)0x08009800) /* Base @ of Page 38, 1 Kbytes */
+#define ADDR_FLASH_PAGE_39    ((uint32_t)0x08009C00) /* Base @ of Page 39, 1 Kbytes */
+#define ADDR_FLASH_PAGE_40    ((uint32_t)0x0800A000) /* Base @ of Page 40, 1 Kbytes */
+#define ADDR_FLASH_PAGE_41    ((uint32_t)0x0800A400) /* Base @ of Page 41, 1 Kbytes */
+#define ADDR_FLASH_PAGE_42    ((uint32_t)0x0800A800) /* Base @ of Page 42, 1 Kbytes */
+#define ADDR_FLASH_PAGE_43    ((uint32_t)0x0800AC00) /* Base @ of Page 43, 1 Kbytes */
+#define ADDR_FLASH_PAGE_44    ((uint32_t)0x0800B000) /* Base @ of Page 44, 1 Kbytes */
+#define ADDR_FLASH_PAGE_45    ((uint32_t)0x0800B400) /* Base @ of Page 45, 1 Kbytes */
+#define ADDR_FLASH_PAGE_46    ((uint32_t)0x0800B800) /* Base @ of Page 46, 1 Kbytes */
+#define ADDR_FLASH_PAGE_47    ((uint32_t)0x0800BC00) /* Base @ of Page 47, 1 Kbytes */
+#define ADDR_FLASH_PAGE_48    ((uint32_t)0x0800C000) /* Base @ of Page 48, 1 Kbytes */
+#define ADDR_FLASH_PAGE_49    ((uint32_t)0x0800C400) /* Base @ of Page 49, 1 Kbytes */
+#define ADDR_FLASH_PAGE_50    ((uint32_t)0x0800C800) /* Base @ of Page 50, 1 Kbytes */
+#define ADDR_FLASH_PAGE_51    ((uint32_t)0x0800CC00) /* Base @ of Page 51, 1 Kbytes */
+#define ADDR_FLASH_PAGE_52    ((uint32_t)0x0800D000) /* Base @ of Page 52, 1 Kbytes */
+#define ADDR_FLASH_PAGE_53    ((uint32_t)0x0800D400) /* Base @ of Page 53, 1 Kbytes */
+#define ADDR_FLASH_PAGE_54    ((uint32_t)0x0800D800) /* Base @ of Page 54, 1 Kbytes */
+#define ADDR_FLASH_PAGE_55    ((uint32_t)0x0800DC00) /* Base @ of Page 55, 1 Kbytes */
+#define ADDR_FLASH_PAGE_56    ((uint32_t)0x0800E000) /* Base @ of Page 56, 1 Kbytes */
+#define ADDR_FLASH_PAGE_57    ((uint32_t)0x0800E400) /* Base @ of Page 57, 1 Kbytes */
+#define ADDR_FLASH_PAGE_58    ((uint32_t)0x0800E800) /* Base @ of Page 58, 1 Kbytes */
+#define ADDR_FLASH_PAGE_59    ((uint32_t)0x0800EC00) /* Base @ of Page 59, 1 Kbytes */
+#define ADDR_FLASH_PAGE_60    ((uint32_t)0x0800F000) /* Base @ of Page 60, 1 Kbytes */
+#define ADDR_FLASH_PAGE_61    ((uint32_t)0x0800F400) /* Base @ of Page 61, 1 Kbytes */
+#define ADDR_FLASH_PAGE_62    ((uint32_t)0x0800F800) /* Base @ of Page 62, 1 Kbytes */
+#define ADDR_FLASH_PAGE_63    ((uint32_t)0x0800FC00) /* Base @ of Page 63, 1 Kbytes */
+#define ADDR_FLASH_PAGE_64    ((uint32_t)0x08010000) /* Base @ of Page 64, 1 Kbytes */
+#define ADDR_FLASH_PAGE_65    ((uint32_t)0x08010400) /* Base @ of Page 65, 1 Kbytes */
+#define ADDR_FLASH_PAGE_66    ((uint32_t)0x08010800) /* Base @ of Page 66, 1 Kbytes */
+#define ADDR_FLASH_PAGE_67    ((uint32_t)0x08010C00) /* Base @ of Page 67, 1 Kbytes */
+#define ADDR_FLASH_PAGE_68    ((uint32_t)0x08011000) /* Base @ of Page 68, 1 Kbytes */
+#define ADDR_FLASH_PAGE_69    ((uint32_t)0x08011400) /* Base @ of Page 69, 1 Kbytes */
+#define ADDR_FLASH_PAGE_70    ((uint32_t)0x08011800) /* Base @ of Page 70, 1 Kbytes */
+#define ADDR_FLASH_PAGE_71    ((uint32_t)0x08011C00) /* Base @ of Page 71, 1 Kbytes */
+#define ADDR_FLASH_PAGE_72    ((uint32_t)0x08012000) /* Base @ of Page 72, 1 Kbytes */
+#define ADDR_FLASH_PAGE_73    ((uint32_t)0x08012400) /* Base @ of Page 73, 1 Kbytes */
+#define ADDR_FLASH_PAGE_74    ((uint32_t)0x08012800) /* Base @ of Page 74, 1 Kbytes */
+#define ADDR_FLASH_PAGE_75    ((uint32_t)0x08012C00) /* Base @ of Page 75, 1 Kbytes */
+#define ADDR_FLASH_PAGE_76    ((uint32_t)0x08013000) /* Base @ of Page 76, 1 Kbytes */
+#define ADDR_FLASH_PAGE_77    ((uint32_t)0x08013400) /* Base @ of Page 77, 1 Kbytes */
+#define ADDR_FLASH_PAGE_78    ((uint32_t)0x08013800) /* Base @ of Page 78, 1 Kbytes */
+#define ADDR_FLASH_PAGE_79    ((uint32_t)0x08013C00) /* Base @ of Page 79, 1 Kbytes */
+#define ADDR_FLASH_PAGE_80    ((uint32_t)0x08014000) /* Base @ of Page 80, 1 Kbytes */
+#define ADDR_FLASH_PAGE_81    ((uint32_t)0x08014400) /* Base @ of Page 81, 1 Kbytes */
+#define ADDR_FLASH_PAGE_82    ((uint32_t)0x08014800) /* Base @ of Page 82, 1 Kbytes */
+#define ADDR_FLASH_PAGE_83    ((uint32_t)0x08014C00) /* Base @ of Page 83, 1 Kbytes */
+#define ADDR_FLASH_PAGE_84    ((uint32_t)0x08015000) /* Base @ of Page 84, 1 Kbytes */
+#define ADDR_FLASH_PAGE_85    ((uint32_t)0x08015400) /* Base @ of Page 85, 1 Kbytes */
+#define ADDR_FLASH_PAGE_86    ((uint32_t)0x08015800) /* Base @ of Page 86, 1 Kbytes */
+#define ADDR_FLASH_PAGE_87    ((uint32_t)0x08015C00) /* Base @ of Page 87, 1 Kbytes */
+#define ADDR_FLASH_PAGE_88    ((uint32_t)0x08016000) /* Base @ of Page 88, 1 Kbytes */
+#define ADDR_FLASH_PAGE_89    ((uint32_t)0x08016400) /* Base @ of Page 89, 1 Kbytes */
+#define ADDR_FLASH_PAGE_90    ((uint32_t)0x08016800) /* Base @ of Page 90, 1 Kbytes */
+#define ADDR_FLASH_PAGE_91    ((uint32_t)0x08016C00) /* Base @ of Page 91, 1 Kbytes */
+#define ADDR_FLASH_PAGE_92    ((uint32_t)0x08017000) /* Base @ of Page 92, 1 Kbytes */
+#define ADDR_FLASH_PAGE_93    ((uint32_t)0x08017400) /* Base @ of Page 93, 1 Kbytes */
+#define ADDR_FLASH_PAGE_94    ((uint32_t)0x08017800) /* Base @ of Page 94, 1 Kbytes */
+#define ADDR_FLASH_PAGE_95    ((uint32_t)0x08017C00) /* Base @ of Page 95, 1 Kbytes */
+#define ADDR_FLASH_PAGE_96    ((uint32_t)0x08018000) /* Base @ of Page 96, 1 Kbytes */
+#define ADDR_FLASH_PAGE_97    ((uint32_t)0x08018400) /* Base @ of Page 97, 1 Kbytes */
+#define ADDR_FLASH_PAGE_98    ((uint32_t)0x08018800) /* Base @ of Page 98, 1 Kbytes */
+#define ADDR_FLASH_PAGE_99    ((uint32_t)0x08018C00) /* Base @ of Page 99, 1 Kbytes */
+#define ADDR_FLASH_PAGE_100   ((uint32_t)0x08019000) /* Base @ of Page 100, 1 Kbytes */
+#define ADDR_FLASH_PAGE_101   ((uint32_t)0x08019400) /* Base @ of Page 101, 1 Kbytes */
+#define ADDR_FLASH_PAGE_102   ((uint32_t)0x08019800) /* Base @ of Page 102, 1 Kbytes */
+#define ADDR_FLASH_PAGE_103   ((uint32_t)0x08019C00) /* Base @ of Page 103, 1 Kbytes */
+#define ADDR_FLASH_PAGE_104   ((uint32_t)0x0801A000) /* Base @ of Page 104, 1 Kbytes */
+#define ADDR_FLASH_PAGE_105   ((uint32_t)0x0801A400) /* Base @ of Page 105, 1 Kbytes */
+#define ADDR_FLASH_PAGE_106   ((uint32_t)0x0801A800) /* Base @ of Page 106, 1 Kbytes */
+#define ADDR_FLASH_PAGE_107   ((uint32_t)0x0801AC00) /* Base @ of Page 107, 1 Kbytes */
+#define ADDR_FLASH_PAGE_108   ((uint32_t)0x0801B000) /* Base @ of Page 108, 1 Kbytes */
+#define ADDR_FLASH_PAGE_109   ((uint32_t)0x0801B400) /* Base @ of Page 109, 1 Kbytes */
+#define ADDR_FLASH_PAGE_110   ((uint32_t)0x0801B800) /* Base @ of Page 110, 1 Kbytes */
+#define ADDR_FLASH_PAGE_111   ((uint32_t)0x0801BC00) /* Base @ of Page 111, 1 Kbytes */
+#define ADDR_FLASH_PAGE_112   ((uint32_t)0x0801C000) /* Base @ of Page 112, 1 Kbytes */
+#define ADDR_FLASH_PAGE_113   ((uint32_t)0x0801C400) /* Base @ of Page 113, 1 Kbytes */
+#define ADDR_FLASH_PAGE_114   ((uint32_t)0x0801C800) /* Base @ of Page 114, 1 Kbytes */
+#define ADDR_FLASH_PAGE_115   ((uint32_t)0x0801CC00) /* Base @ of Page 115, 1 Kbytes */
+#define ADDR_FLASH_PAGE_116   ((uint32_t)0x0801D000) /* Base @ of Page 116, 1 Kbytes */
+#define ADDR_FLASH_PAGE_117   ((uint32_t)0x0801D400) /* Base @ of Page 117, 1 Kbytes */
+#define ADDR_FLASH_PAGE_118   ((uint32_t)0x0801D800) /* Base @ of Page 118, 1 Kbytes */
+#define ADDR_FLASH_PAGE_119   ((uint32_t)0x0801DC00) /* Base @ of Page 119, 1 Kbytes */
+#define ADDR_FLASH_PAGE_120   ((uint32_t)0x0801E000) /* Base @ of Page 120, 1 Kbytes */
+#define ADDR_FLASH_PAGE_121   ((uint32_t)0x0801E400) /* Base @ of Page 121, 1 Kbytes */
+#define ADDR_FLASH_PAGE_122   ((uint32_t)0x0801E800) /* Base @ of Page 122, 1 Kbytes */
+#define ADDR_FLASH_PAGE_123   ((uint32_t)0x0801EC00) /* Base @ of Page 123, 1 Kbytes */
+#define ADDR_FLASH_PAGE_124   ((uint32_t)0x0801F000) /* Base @ of Page 124, 1 Kbytes */
+#define ADDR_FLASH_PAGE_125   ((uint32_t)0x0801F400) /* Base @ of Page 125, 1 Kbytes */
+#define ADDR_FLASH_PAGE_126   ((uint32_t)0x0801F800) /* Base @ of Page 126, 1 Kbytes */
+#define ADDR_FLASH_PAGE_127   ((uint32_t)0x0801FC00) /* Base @ of Page 127, 1 Kbytes */
+
+/* Define the size of the sectors to be used */
+#define PAGE_SIZE               (uint32_t)FLASH_PAGE_SIZE  /* Page size */
+
+/* EEPROM start address in Flash */
+#define EEPROM_START_ADDRESS  ((uint32_t)ADDR_FLASH_PAGE_32) /* EEPROM emulation start address */
+
+/* Pages 0 and 1 base and end addresses */
+#define PAGE0_BASE_ADDRESS    ((uint32_t)(EEPROM_START_ADDRESS + 0x0000))
+#define PAGE0_END_ADDRESS     ((uint32_t)(EEPROM_START_ADDRESS + (PAGE_SIZE - 1)))
+#define PAGE0_ID               ADDR_FLASH_PAGE_32
+
+#define PAGE1_BASE_ADDRESS    ((uint32_t)(EEPROM_START_ADDRESS + 0x10000))
+#define PAGE1_END_ADDRESS     ((uint32_t)(EEPROM_START_ADDRESS + 0x10000 + PAGE_SIZE - 1))
+#define PAGE1_ID               ADDR_FLASH_PAGE_96
+
+/* Used Flash pages for EEPROM emulation */
+#define PAGE0                 ((uint16_t)0x0000)
+#define PAGE1                 ((uint16_t)0x0040)
+
+/* No valid page define */
+#define NO_VALID_PAGE         ((uint16_t)0x00AB)
+
+/* Page status definitions */
+#define ERASED                ((uint16_t)0xFFFF)     /* Page is empty */
+#define RECEIVE_DATA          ((uint16_t)0xEEEE)     /* Page is marked to receive data */
+#define VALID_PAGE            ((uint16_t)0x0000)     /* Page containing valid data */
+
+/* Valid pages in read and write defines */
+#define READ_FROM_VALID_PAGE  ((uint8_t)0x00)
+#define WRITE_IN_VALID_PAGE   ((uint8_t)0x01)
+
+/* Page full define */
+#define PAGE_FULL             ((uint8_t)0x80)
+
+/* Variables' number */
+#define NB_OF_VAR             ((uint8_t)0x03)
+
+/* Exported types ------------------------------------------------------------*/
+/* Exported macro ------------------------------------------------------------*/
+/* Exported functions ------------------------------------------------------- */
+uint16_t EE_Init(void);
+uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data);
+uint16_t EE_WriteVariable(uint16_t VirtAddress, uint16_t Data);
+
+#endif /* __EEPROM_H */
+
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
--- a/Inc/hd44780.h
+++ b/Inc/hd44780.h
@ -0,0 +1,294 @@
+/*
+ * hd44780.h
+ *
+ *  Created on: Feb 20, 2014
+ *      Author: Peter
+ */
+
+#ifndef HD44780_H_
+#define HD44780_H_
+
+#include <stdio.h>
+#include <stdint.h>
+#include "stm32f1xx_hal.h"
+#include "pcf8574.h"
+
+/**
+ * @file	hd44780.h
+ * @brief	Header file for communication with the HD44780 LCD driver.
+ * To use it you will have to create a variable of type LCD_PCF8574_HandleTypeDef (e.g. "lcd") and then
+ * set the I2C address based on the address pins on your PCF8574 (0-7) (lcd.pcf8574.PCF_I2C_ADDRESS),
+ * set the I2C timeout (in milliseconds) (lcd.pcf8574.PCF_I2C_TIMEOUT),
+ * set the I2C instance (e.g. I2C1 or I2C2) (lcd.pcf8574.i2c.Instance),
+ * set the I2C clock speed (in Hertz) (lcd.pcf8574.i2c.Init.ClockSpeed),
+ * set the number of lines (has to be type of LCD_NUMBER_OF_LINES) (lcd.NUMBER_OF_LINES),
+ * set the interface type (has to be type of LCD_TYPE) (lcd.type).
+ *
+ * Example:
+ * example.c
+ * example_msp.c
+ */
+
+/** LCD Interface possibilities
+ */
+typedef enum{
+	PCF8574,	/*!< Use PCF8574 I2C IO expander as the interface */
+	GPIO		/*!< Use GPIO pins directly */
+} LCD_INTERFACE;
+
+/** Possible return values for the functions
+ */
+typedef enum{
+	LCD_OK,		/*!< Everything went OK */
+	LCD_ERROR	/*!< An error occured */
+} LCD_RESULT;
+
+/** Type of hardware to use
+ */
+typedef enum{
+	TYPE0,
+	TYPE1,
+	TYPE2
+} LCD_TYPE;
+
+
+/** Number of lines on your LCD
+ */
+typedef enum{
+	NUMBER_OF_LINES_1=0,
+	NUMBER_OF_LINES_4=3,
+	NUMBER_OF_LINES_2=1
+} LCD_NUMBER_OF_LINES;
+
+/**
+ * Structure that hold all the required variables in
+ * order to simplify the communication process
+ */
+typedef struct{
+	LCD_NUMBER_OF_LINES		NUMBER_OF_LINES;	/**< Number of lines on your LCD */
+	uint8_t 				D;
+	uint8_t 				C;
+	uint8_t 				B;
+	char 					lcdbuf[2][16];		/**< Buffer for the LCD */
+	int 					x, oldx, y, oldy;
+	uint8_t 				state;				/**< Holds current state of the PCF8574 expander */
+	uint32_t*				pins;				/**< Array of pins based on your hardware (wiring) */
+	LCD_TYPE				type;				/**< Type of hardware you want to use */
+	PCF8574_HandleTypeDef 	pcf8574;			/**< PCF8574_HandleTypeDef for communication with PCF8574 */
+	void					(*errorCallback)(LCD_RESULT);
+} LCD_PCF8574_HandleTypeDef;
+
+/** @def INTERFACE - Selector for the type of interface you want to use (has to be a type of LCD_INTERFACE) */
+#define LCD_INTERFACE_SELECTOR			PCF8574
+
+/** Enumeration of the LCD pins */
+typedef enum{
+	LCD_PIN_D4=0,
+	LCD_PIN_D5=1,
+	LCD_PIN_D6=2,
+	LCD_PIN_D7=3,
+	LCD_PIN_RS=4,
+	LCD_PIN_RW=5,
+	LCD_PIN_E=6,
+	LCD_PIN_LED=7
+} LCD_PIN;
+
+/** Used to specify the direction in certain LCD operations */
+typedef enum{
+	DIRECTION_LEFT=0,
+	DIRECTION_RIGHT=1
+} LCD_DIRECTION;
+
+/**  */
+typedef enum{
+	DIRECTION_INCREMENT=1,
+	DIRECTION_DECREMENT=2
+} LCD_DIRECTION_INC_DEC;
+
+/**  */
+typedef enum{
+	SHIFT_YES=1,
+	SHIFT_NO=0
+} LCD_SHIFT;
+
+#if LCD_INTERFACE_SELECTOR==PCF8574
+
+/**
+ *	LCD initialization function
+ *	@param	handle - a pointer to the LCD handle
+ *	@return	whether the function was successful or not
+ */
+LCD_RESULT LCD_Init(LCD_PCF8574_HandleTypeDef* handle);
+
+/**
+ * LCD deinitialization function
+ * @param	handle - a pointer to the LCD handle
+ * @return	whether the function was successful or not
+ */
+LCD_RESULT LCD_DeInit(LCD_PCF8574_HandleTypeDef* handle);
+
+/**
+ * Sends a command to the HD44780 controller
+ * @param	handle - a pointer to the LCD handle
+ * @param	cmd - a command you want to send
+ * @return	whether the function was successful or not
+ */
+LCD_RESULT LCD_WriteCMD(LCD_PCF8574_HandleTypeDef* handle, uint8_t cmd);
+
+/**
+ * Sends data to the HD44780 controller
+ * @param	handle - a pointer to the LCD handle
+ * @param	data - data you want to send
+ * @return	whether the function was successful or not
+ */
+LCD_RESULT LCD_WriteDATA(LCD_PCF8574_HandleTypeDef* handle, uint8_t data);
+
+/**
+ * Gets the state of the busy flag
+ * @param	handle - a pointer to the LCD handle
+ * @param	flag - a pointer to a variable that will contain the state of the flag
+ * @return	whether the function was successful or not
+ */
+LCD_RESULT LCD_GetBusyFlag(LCD_PCF8574_HandleTypeDef* handle,uint8_t* flag);
+
+/**
+ * Writes lower 4bits of data to the data bus of the controller
+ * @param	handle - a pointer to the LCD handle
+ * @param	data - data you want to put on the data bus (lower 4bits)
+ * @return	whether the function was successful or not
+ */
+LCD_RESULT LCD_WriteToDataBus(LCD_PCF8574_HandleTypeDef* handle, uint8_t data);
+
+/**
+ * Clears the LCD
+ * @param	handle - a pointer to the LCD handle
+ * @return	whether the function was successful or not
+ */
+LCD_RESULT LCD_ClearDisplay(LCD_PCF8574_HandleTypeDef* handle);
+
+/**
+ * Writes a string to the LCD
+ * @param	handle - a pointer to the LCD handle
+ * @param	s - string you want to write to the LCD
+ * @return	whether the function was successful or not
+ */
+LCD_RESULT LCD_WriteString(LCD_PCF8574_HandleTypeDef* handle, char *s);
+
+/**
+ * Sets the location of the memory pointer in the controller (used to control other operations (for example where to write a string))
+ * @param	handle - a pointer to the LCD handle
+ * @param	x - x-coordinate of the location
+ * @param	y - y-coordinate of the location
+ * @return	whether the function was successful or not
+ */
+LCD_RESULT LCD_SetLocation(LCD_PCF8574_HandleTypeDef* handle, uint8_t x, uint8_t y);
+
+/**
+ * Turns ON the display
+ * @param	handle - a pointer to the LCD handle
+ * @return	whether the function was successful or not
+ */
+LCD_RESULT LCD_DisplayON(LCD_PCF8574_HandleTypeDef* handle);
+
+/**
+ * Turns OFF the display
+ * @param	handle - a pointer to the LCD handle
+ * @return	whether the function was successful or not
+ */
+LCD_RESULT LCD_DisplayOFF(LCD_PCF8574_HandleTypeDef* handle);
+
+/**
+ * Turns ON the cursor
+ * @param	handle - a pointer to the LCD handle
+ * @param	blink - if you want the cursor to blink set this to 1, else 0
+ * @return	whether the function was successful or not
+ */
+LCD_RESULT LCD_CursorON(LCD_PCF8574_HandleTypeDef* handle, uint8_t blink);
+
+/**
+ * Turns OFF the cursor
+ * @param	handle - a pointer to the LCD handle
+ * @return	whether the function was successful or not
+ */
+LCD_RESULT LCD_CursorOFF(LCD_PCF8574_HandleTypeDef* handle);
+
+/**
+ * Shifts the cursor in the specified direction certain number of steps
+ * @param	handle - a pointer to the LCD handle
+ * @param	direction - specifies the direction
+ * @param	steps - specifies how many positions to shift the cursor by
+ * @return	whether the function was successful or not
+ */
+LCD_RESULT LCD_ShiftCursor(LCD_PCF8574_HandleTypeDef* handle, LCD_DIRECTION direction,uint8_t steps);
+
+/**
+ * Shifts the contents of the LCD
+ * @param	handle - a pointer to the LCD handle
+ * @param	direction - directions of the shift
+ * @param	steps - how many positions to shift the contents by
+ * @return	whether the function was successful or not
+ */
+LCD_RESULT LCD_ShiftDisplay(LCD_PCF8574_HandleTypeDef* handle, uint8_t direction, uint8_t steps);
+
+/**
+ * Writes a number to the LCD
+ * @param	handle - a pointer to the LCD handle
+ * @param	n - a number you want to write to the LCD
+ * @return	whether the function was successful or not
+ */
+LCD_RESULT LCD_WriteNumber(LCD_PCF8574_HandleTypeDef* handle, unsigned long n, uint8_t base);
+
+
+LCD_RESULT LCD_WriteFloat(LCD_PCF8574_HandleTypeDef* handle, double number, uint8_t digits);
+
+/**
+ * Sets the mode by which data is written to the LCD
+ * @param	handle - a pointer to the LCD handle
+ * @param	direction
+ * @param	shift
+ * @return	whether the function was successful or not
+ */
+LCD_RESULT LCD_EntryModeSet(LCD_PCF8574_HandleTypeDef* handle, LCD_DIRECTION_INC_DEC direction,LCD_SHIFT shift);
+
+/**
+ * Creates a custom character at the given address
+ * @param	handle - a pointer to the LCD handle
+ * @param	pattern - pointer to the bit pattern of the character
+ * @param	address - an address to which the character will be written
+ * @return	whether the function was successful or not
+ */
+LCD_RESULT LCD_CustomChar(LCD_PCF8574_HandleTypeDef* handle, uint8_t *pattern,uint8_t address);
+
+/**
+ * Writes the current state to the PCF8574 expander
+ * @param	handle - a pointer to the LCD handle
+ * @return	whether the function was successful or not
+ */
+LCD_RESULT LCD_I2C_WriteOut(LCD_PCF8574_HandleTypeDef* handle);
+
+/**
+ * Controls the state of the LCD backlight
+ * @param	handle - a pointer to the LCD handle
+ * @param	on - set it to 1 if you want to turn the backlight on, else 0
+ * @return	whether the function was successful or not
+ */
+LCD_RESULT LCD_StateLEDControl(LCD_PCF8574_HandleTypeDef* handle, uint8_t on);
+
+/**
+ * Rewrites a bit in the state variable with the value specified
+ * @param	handle - a pointer to the LCD handle
+ * @param	value - value of the bit (0 or 1)
+ * @param	pin - pin which you want to write to
+ * @return	whether the function was successful or not
+ */
+LCD_RESULT LCD_StateWriteBit(LCD_PCF8574_HandleTypeDef* handle, uint8_t value, LCD_PIN pin);
+
+/**
+ * Waits until the busy flag is reset
+ * @param	handle - a pointer to the LCD handle
+ */
+void LCD_WaitForBusyFlag(LCD_PCF8574_HandleTypeDef* handle);
+
+#endif
+
+#endif /* HD44780_H_ */
--- a/Inc/pcf8574.h
+++ b/Inc/pcf8574.h
@ -0,0 +1,82 @@
+/*
+ * pcf8574.h
+ *
+ *  Created on: Dec 30, 2014
+ *      Author: peter
+ */
+
+#ifndef INC_PCF8574_H_
+#define INC_PCF8574_H_
+
+#include "stm32f1xx_hal.h"
+
+/** @file	pcf8574.h
+ * @brief	In order to use this you have to create a PCF8574_HandleTypeDef variable (e.g. "pcf").
+ * Then you will set the the address based on the configuration of your chip (pins A0, A1, A2) ( pcf.PCF_I2C_ADDRESS ) (0 to 7),
+ * timeout ( pcf.PCF_I2C_TIMEOUT ) (e.g. 1000 (=1 sec)),
+ * I2C instance to use ( pcf.i2c.Instance ) (e.g. I2C1 or I2C2 ...),
+ * speed of the communication ( pcf.i2c.Init.ClockSpeed ) (e.g. 100 000 (=100kHz)).
+ *
+ * Example:
+ * example.c
+ * example_msp.c
+ */
+
+/**
+ * Provides possible return values for the functions
+ */
+typedef enum{
+	PCF8574_OK,		/**< Everything went OK */
+	PCF8574_ERROR	/**< An error occured */
+} PCF8574_RESULT;
+
+/** @def PCF8574_I2C_ADDRESS_MASK - Pulled from the datasheet
+ */
+#define PCF8574_I2C_ADDRESS_MASK	0x40
+
+/**
+ * PCF8574 handle structure which wraps all the necessary variables together in
+ * order to simplify the communication with the chip
+ */
+typedef struct{
+	uint8_t				PCF_I2C_ADDRESS;	/**< address of the chip you want to communicate with */
+	uint32_t			PCF_I2C_TIMEOUT;	/**< timeout value for the communication in milliseconds */
+	I2C_HandleTypeDef 	i2c;				/**< I2C_HandleTypeDef structure */
+	void				(*errorCallback)(PCF8574_RESULT);
+} PCF8574_HandleTypeDef;
+
+/** @var PCF8574_Type0Pins[8] - characterization of pins for hardware of type 0
+ */
+extern uint32_t PCF8574_Type0Pins[];
+
+/**
+ * Initializes the I2C for communication
+ * @param	handle - a pointer to the PCF8574 handle
+ * @return	whether the function was successful or not
+ */
+PCF8574_RESULT PCF8574_Init(PCF8574_HandleTypeDef* handle);
+
+/**
+ * Deinitializes the I2C
+ * @param	handle - a pointer to the PCF8574 handle
+ * @return	whether the function was successful or not
+ */
+PCF8574_RESULT PCF8574_DeInit(PCF8574_HandleTypeDef* handle);
+
+/**
+ * Writes a given value to the port of PCF8574
+ * @param	handle - a pointer to the PCF8574 handle
+ * @param	val - a value to be written to the port
+ * @return	whether the function was successful or not
+ */
+PCF8574_RESULT PCF8574_Write(PCF8574_HandleTypeDef* handle, uint8_t val);
+
+/**
+ * Reads the current state of the port of PCF8574
+ * @param	handle - a pointer to the PCF8574 handle
+ * @param	val - a pointer to the variable that will be assigned a value from the chip
+ * @return	whether the function was successful or not
+ */
+PCF8574_RESULT PCF8574_Read(PCF8574_HandleTypeDef* handle, uint8_t* val);
+
+#endif /* INC_PCF8574_H_ */
--- a/3
+++ b/3
@ -40,6 +40,9 @@ Src/setup.c \
 Src/control.c \
 Src/main.c \
 Src/bldc.c \
+Src/eeprom.c \
+Src/hd44780.c \
+Src/pcf8574.c \
 Src/comms.c \
 Src/stm32f1xx_it.c \

--- a/README.md
+++ b/README.md
@ -1,4 +1,4 @@
-This firmware is much better than the old one. tested up to 40A / 60V, no dead board so far :)
+Build Instructions for this very branch: https://github.com/NiklasFauth/hoverboard-firmware-hack/wiki/Build-Instruction:-TranspOtter

 # hoverboard-firmware-hack

@ -72,3 +72,4 @@ If you need additional features like a boost button, have a look at the while(1)
 ### Projects based on it
 * [bobbycar-optimized firmware](https://github.com/larsmm/hoverboard-firmware-hack-bbcar)  based on this one with driving modes, acceleration ramps and some other features
 * [wheel chair](https://github.com/Lahorde/steer_speed_ctrl) controlled with a joystick or using a CC2650 sensortag to control it over  bluetooth with pitch/roll.
+
--- a/Src/bldc.c
+++ b/Src/bldc.c
@ -18,6 +18,9 @@ extern volatile adc_buf_t adc_buffer;

 extern volatile uint32_t timeout;

+float currentR = 0;
+float currentL = 0;
+
 uint32_t buzzerFreq = 0;
 uint32_t buzzerPattern = 0;

@ -166,7 +169,8 @@ void DMA1_Channel1_IRQHandler() {
  }

  //disable PWM when current limit is reached (current chopping)
-  if(ABS((adc_buffer.dcl - offsetdcl) * MOTOR_AMP_CONV_DC_AMP) > DC_CUR_LIMIT || timeout > TIMEOUT || enable == 0) {
+  currentL = (adc_buffer.dcl - offsetdcl) * MOTOR_AMP_CONV_DC_AMP;
+  if(ABS(currentL) > DC_CUR_LIMIT || timeout > TIMEOUT || enable == 0) {
    LEFT_TIM->BDTR &= ~TIM_BDTR_MOE;
    //HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1);
  } else {
@ -174,7 +178,8 @@ void DMA1_Channel1_IRQHandler() {
    //HAL_GPIO_WritePin(LED_PORT, LED_PIN, 0);
  }

-  if(ABS((adc_buffer.dcr - offsetdcr) * MOTOR_AMP_CONV_DC_AMP)  > DC_CUR_LIMIT || timeout > TIMEOUT || enable == 0) {
+  currentR = (adc_buffer.dcr - offsetdcr) * MOTOR_AMP_CONV_DC_AMP;
+  if(ABS(currentR)  > DC_CUR_LIMIT || timeout > TIMEOUT || enable == 0) {
    RIGHT_TIM->BDTR &= ~TIM_BDTR_MOE;
  } else {
    RIGHT_TIM->BDTR |= TIM_BDTR_MOE;
@ -214,18 +219,18 @@ void DMA1_Channel1_IRQHandler() {
    // if(len == 0){
        // return(0);
    // }
-    
+
    // struct {
        // uint16_t freq : 4;
        // uint16_t volume : 4;
        // uint16_t time : 8;
    // } note = notes[counter];
-    
+
    // if(timer / 500 == note.time){
        // timer = 0;
        // counter++;
    // }
-    
+
    // if(counter == len){
        // counter = 0;
    // }
--- a/Src/control.c
+++ b/Src/control.c
@ -84,6 +84,13 @@ void PPM_Init() {
 }
 #endif

+uint8_t Nunchuck_Ping() {
+  if (HAL_I2C_Master_Receive(&hi2c2,0xA4,(uint8_t*)nunchuck_data, 1, 10) == HAL_OK) {
+    return 1;
+  }
+  return 0;
+}
+
 void Nunchuck_Init() {
    //-- START -- init WiiNunchuck
  i2cBuffer[0] = 0xF0;
@ -101,18 +108,18 @@ void Nunchuck_Init() {

 void Nunchuck_Read() {
  i2cBuffer[0] = 0x00;
-  HAL_I2C_Master_Transmit(&hi2c2,0xA4,(uint8_t*)i2cBuffer, 1, 100);
-  HAL_Delay(5);
-  if (HAL_I2C_Master_Receive(&hi2c2,0xA4,(uint8_t*)nunchuck_data, 6, 100) == HAL_OK) {
+  HAL_I2C_Master_Transmit(&hi2c2,0xA4,(uint8_t*)i2cBuffer, 1, 10);
+  HAL_Delay(3);
+  if (HAL_I2C_Master_Receive(&hi2c2,0xA4,(uint8_t*)nunchuck_data, 6, 10) == HAL_OK) {
    timeout = 0;
  } else {
    timeout++;
  }

-  if (timeout > 3) {
+  /*if (timeout > 3) {
    HAL_Delay(50);
    Nunchuck_Init();
-  }
+  }*/

  //setScopeChannel(0, (int)nunchuck_data[0]);
  //setScopeChannel(1, (int)nunchuck_data[1]);
--- a/Src/eeprom.c
+++ b/Src/eeprom.c
@ -0,0 +1,730 @@
+/**
+  ******************************************************************************
+  * @file    EEPROM_Emulation/src/eeprom.c
+  * @author  MCD Application Team
+  * @version V1.3.0
+  * @date    18-December-2015
+  * @brief   This file provides all the EEPROM emulation firmware functions.
+  ******************************************************************************
+  * @attention
+  *
+  * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
+  *
+  * Redistribution and use in source and binary forms, with or without modification,
+  * are permitted provided that the following conditions are met:
+  *   1. Redistributions of source code must retain the above copyright notice,
+  *      this list of conditions and the following disclaimer.
+  *   2. Redistributions in binary form must reproduce the above copyright notice,
+  *      this list of conditions and the following disclaimer in the documentation
+  *      and/or other materials provided with the distribution.
+  *   3. Neither the name of STMicroelectronics nor the names of its contributors
+  *      may be used to endorse or promote products derived from this software
+  *      without specific prior written permission.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  ******************************************************************************
+  */
+
+/** @addtogroup EEPROM_Emulation
+  * @{
+  */
+
+/* Includes ------------------------------------------------------------------*/
+#include "eeprom.h"
+
+/* Private typedef -----------------------------------------------------------*/
+/* Private define ------------------------------------------------------------*/
+/* Private macro -------------------------------------------------------------*/
+/* Private variables ---------------------------------------------------------*/
+
+/* Global variable used to store variable value in read sequence */
+uint16_t DataVar = 0;
+
+/* Virtual address defined by the user: 0xFFFF value is prohibited */
+extern uint16_t VirtAddVarTab[NB_OF_VAR];
+
+/* Private function prototypes -----------------------------------------------*/
+/* Private functions ---------------------------------------------------------*/
+static HAL_StatusTypeDef EE_Format(void);
+static uint16_t EE_FindValidPage(uint8_t Operation);
+static uint16_t EE_VerifyPageFullWriteVariable(uint16_t VirtAddress, uint16_t Data);
+static uint16_t EE_PageTransfer(uint16_t VirtAddress, uint16_t Data);
+static uint16_t EE_VerifyPageFullyErased(uint32_t Address);
+
+/**
+  * @brief  Restore the pages to a known good state in case of page's status
+  *   corruption after a power loss.
+  * @param  None.
+  * @retval - Flash error code: on write Flash error
+  *         - FLASH_COMPLETE: on success
+  */
+uint16_t EE_Init(void)
+{
+  uint16_t pagestatus0 = 6, pagestatus1 = 6;
+  uint16_t varidx = 0;
+  uint16_t eepromstatus = 0, readstatus = 0;
+  int16_t x = -1;
+  HAL_StatusTypeDef  flashstatus;
+  uint32_t page_error = 0;
+  FLASH_EraseInitTypeDef s_eraseinit;
+
+
+  /* Get Page0 status */
+  pagestatus0 = (*(__IO uint16_t*)PAGE0_BASE_ADDRESS);
+  /* Get Page1 status */
+  pagestatus1 = (*(__IO uint16_t*)PAGE1_BASE_ADDRESS);
+
+  /* Fill EraseInit structure*/
+  s_eraseinit.TypeErase   = FLASH_TYPEERASE_PAGES;
+  s_eraseinit.PageAddress = PAGE0_ID;
+  s_eraseinit.NbPages     = 1;
+
+  /* Check for invalid header states and repair if necessary */
+  switch (pagestatus0)
+  {
+    case ERASED:
+      if (pagestatus1 == VALID_PAGE) /* Page0 erased, Page1 valid */
+      {
+          /* Erase Page0 */
+        if(!EE_VerifyPageFullyErased(PAGE0_BASE_ADDRESS))
+        {
+          flashstatus = HAL_FLASHEx_Erase(&s_eraseinit, &page_error);
+          /* If erase operation was failed, a Flash error code is returned */
+          if (flashstatus != HAL_OK)
+          {
+            return flashstatus;
+          }
+        }
+      }
+      else if (pagestatus1 == RECEIVE_DATA) /* Page0 erased, Page1 receive */
+      {
+        /* Erase Page0 */
+        if(!EE_VerifyPageFullyErased(PAGE0_BASE_ADDRESS))
+        {
+          flashstatus = HAL_FLASHEx_Erase(&s_eraseinit, &page_error);
+          /* If erase operation was failed, a Flash error code is returned */
+          if (flashstatus != HAL_OK)
+          {
+            return flashstatus;
+          }
+        }
+        /* Mark Page1 as valid */
+        flashstatus = HAL_FLASH_Program(FLASH_TYPEPROGRAM_HALFWORD, PAGE1_BASE_ADDRESS, VALID_PAGE);
+        /* If program operation was failed, a Flash error code is returned */
+        if (flashstatus != HAL_OK)
+        {
+          return flashstatus;
+        }
+      }
+      else /* First EEPROM access (Page0&1 are erased) or invalid state -> format EEPROM */
+      {
+        /* Erase both Page0 and Page1 and set Page0 as valid page */
+        flashstatus = EE_Format();
+        /* If erase/program operation was failed, a Flash error code is returned */
+        if (flashstatus != HAL_OK)
+        {
+          return flashstatus;
+        }
+      }
+      break;
+
+    case RECEIVE_DATA:
+      if (pagestatus1 == VALID_PAGE) /* Page0 receive, Page1 valid */
+      {
+        /* Transfer data from Page1 to Page0 */
+        for (varidx = 0; varidx < NB_OF_VAR; varidx++)
+        {
+          if (( *(__IO uint16_t*)(PAGE0_BASE_ADDRESS + 6)) == VirtAddVarTab[varidx])
+          {
+            x = varidx;
+          }
+          if (varidx != x)
+          {
+            /* Read the last variables' updates */
+            readstatus = EE_ReadVariable(VirtAddVarTab[varidx], &DataVar);
+            /* In case variable corresponding to the virtual address was found */
+            if (readstatus != 0x1)
+            {
+              /* Transfer the variable to the Page0 */
+              eepromstatus = EE_VerifyPageFullWriteVariable(VirtAddVarTab[varidx], DataVar);
+              /* If program operation was failed, a Flash error code is returned */
+              if (eepromstatus != HAL_OK)
+              {
+                return eepromstatus;
+              }
+            }
+          }
+        }
+        /* Mark Page0 as valid */
+        flashstatus = HAL_FLASH_Program(FLASH_TYPEPROGRAM_HALFWORD, PAGE0_BASE_ADDRESS, VALID_PAGE);
+        /* If program operation was failed, a Flash error code is returned */
+        if (flashstatus != HAL_OK)
+        {
+          return flashstatus;
+        }
+        s_eraseinit.TypeErase   = FLASH_TYPEERASE_PAGES;
+        s_eraseinit.PageAddress = PAGE1_ID;
+        s_eraseinit.NbPages     = 1;
+        /* Erase Page1 */
+        if(!EE_VerifyPageFullyErased(PAGE1_BASE_ADDRESS))
+        {
+          flashstatus = HAL_FLASHEx_Erase(&s_eraseinit, &page_error);
+          /* If erase operation was failed, a Flash error code is returned */
+          if (flashstatus != HAL_OK)
+          {
+            return flashstatus;
+          }
+        }
+      }
+      else if (pagestatus1 == ERASED) /* Page0 receive, Page1 erased */
+      {
+        s_eraseinit.TypeErase   = FLASH_TYPEERASE_PAGES;
+        s_eraseinit.PageAddress = PAGE1_ID;
+        s_eraseinit.NbPages     = 1;
+        /* Erase Page1 */
+        if(!EE_VerifyPageFullyErased(PAGE1_BASE_ADDRESS))
+        {
+          flashstatus = HAL_FLASHEx_Erase(&s_eraseinit, &page_error);
+          /* If erase operation was failed, a Flash error code is returned */
+          if (flashstatus != HAL_OK)
+          {
+            return flashstatus;
+          }
+        }
+        /* Mark Page0 as valid */
+        flashstatus = HAL_FLASH_Program(FLASH_TYPEPROGRAM_HALFWORD, PAGE0_BASE_ADDRESS, VALID_PAGE);
+        /* If program operation was failed, a Flash error code is returned */
+        if (flashstatus != HAL_OK)
+        {
+          return flashstatus;
+        }
+      }
+      else /* Invalid state -> format eeprom */
+      {
+        /* Erase both Page0 and Page1 and set Page0 as valid page */
+        flashstatus = EE_Format();
+        /* If erase/program operation was failed, a Flash error code is returned */
+        if (flashstatus != HAL_OK)
+        {
+          return flashstatus;
+        }
+      }
+      break;
+
+    case VALID_PAGE:
+      if (pagestatus1 == VALID_PAGE) /* Invalid state -> format eeprom */
+      {
+        /* Erase both Page0 and Page1 and set Page0 as valid page */
+        flashstatus = EE_Format();
+        /* If erase/program operation was failed, a Flash error code is returned */
+        if (flashstatus != HAL_OK)
+        {
+          return flashstatus;
+        }
+      }
+      else if (pagestatus1 == ERASED) /* Page0 valid, Page1 erased */
+      {
+        s_eraseinit.TypeErase   = FLASH_TYPEERASE_PAGES;
+        s_eraseinit.PageAddress = PAGE1_ID;
+        s_eraseinit.NbPages     = 1;
+        /* Erase Page1 */
+        if(!EE_VerifyPageFullyErased(PAGE1_BASE_ADDRESS))
+        {
+          flashstatus = HAL_FLASHEx_Erase(&s_eraseinit, &page_error);
+          /* If erase operation was failed, a Flash error code is returned */
+          if (flashstatus != HAL_OK)
+          {
+            return flashstatus;
+          }
+        }
+      }
+      else /* Page0 valid, Page1 receive */
+      {
+        /* Transfer data from Page0 to Page1 */
+        for (varidx = 0; varidx < NB_OF_VAR; varidx++)
+        {
+          if ((*(__IO uint16_t*)(PAGE1_BASE_ADDRESS + 6)) == VirtAddVarTab[varidx])
+          {
+            x = varidx;
+          }
+          if (varidx != x)
+          {
+            /* Read the last variables' updates */
+            readstatus = EE_ReadVariable(VirtAddVarTab[varidx], &DataVar);
+            /* In case variable corresponding to the virtual address was found */
+            if (readstatus != 0x1)
+            {
+              /* Transfer the variable to the Page1 */
+              eepromstatus = EE_VerifyPageFullWriteVariable(VirtAddVarTab[varidx], DataVar);
+              /* If program operation was failed, a Flash error code is returned */
+              if (eepromstatus != HAL_OK)
+              {
+                return eepromstatus;
+              }
+            }
+          }
+        }
+        /* Mark Page1 as valid */
+        flashstatus = HAL_FLASH_Program(FLASH_TYPEPROGRAM_HALFWORD, PAGE1_BASE_ADDRESS, VALID_PAGE);
+        /* If program operation was failed, a Flash error code is returned */
+        if (flashstatus != HAL_OK)
+        {
+          return flashstatus;
+        }
+        s_eraseinit.TypeErase   = FLASH_TYPEERASE_PAGES;
+        s_eraseinit.PageAddress = PAGE0_ID;
+        s_eraseinit.NbPages     = 1;
+        /* Erase Page0 */
+        if(!EE_VerifyPageFullyErased(PAGE0_BASE_ADDRESS))
+        {
+          flashstatus = HAL_FLASHEx_Erase(&s_eraseinit, &page_error);
+          /* If erase operation was failed, a Flash error code is returned */
+          if (flashstatus != HAL_OK)
+          {
+            return flashstatus;
+          }
+        }
+      }
+      break;
+
+    default:  /* Any other state -> format eeprom */
+      /* Erase both Page0 and Page1 and set Page0 as valid page */
+      flashstatus = EE_Format();
+      /* If erase/program operation was failed, a Flash error code is returned */
+      if (flashstatus != HAL_OK)
+      {
+        return flashstatus;
+      }
+      break;
+  }
+
+  return HAL_OK;
+}
+
+/**
+  * @brief  Verify if specified page is fully erased.
+  * @param  Address: page address
+  *   This parameter can be one of the following values:
+  *     @arg PAGE0_BASE_ADDRESS: Page0 base address
+  *     @arg PAGE1_BASE_ADDRESS: Page1 base address
+  * @retval page fully erased status:
+  *           - 0: if Page not erased
+  *           - 1: if Page erased
+  */
+uint16_t EE_VerifyPageFullyErased(uint32_t Address)
+{
+  uint32_t readstatus = 1;
+  uint16_t addressvalue = 0x5555;
+
+  /* Check each active page address starting from end */
+  while (Address <= PAGE0_END_ADDRESS)
+  {
+    /* Get the current location content to be compared with virtual address */
+    addressvalue = (*(__IO uint16_t*)Address);
+
+    /* Compare the read address with the virtual address */
+    if (addressvalue != ERASED)
+    {
+
+      /* In case variable value is read, reset readstatus flag */
+      readstatus = 0;
+
+      break;
+    }
+    /* Next address location */
+    Address = Address + 4;
+  }
+
+  /* Return readstatus value: (0: Page not erased, 1: Page erased) */
+  return readstatus;
+}
+
+/**
+  * @brief  Returns the last stored variable data, if found, which correspond to
+  *   the passed virtual address
+  * @param  VirtAddress: Variable virtual address
+  * @param  Data: Global variable contains the read variable value
+  * @retval Success or error status:
+  *           - 0: if variable was found
+  *           - 1: if the variable was not found
+  *           - NO_VALID_PAGE: if no valid page was found.
+  */
+uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data)
+{
+  uint16_t validpage = PAGE0;
+  uint16_t addressvalue = 0x5555, readstatus = 1;
+  uint32_t address = EEPROM_START_ADDRESS, PageStartAddress = EEPROM_START_ADDRESS;
+
+  /* Get active Page for read operation */
+  validpage = EE_FindValidPage(READ_FROM_VALID_PAGE);
+
+  /* Check if there is no valid page */
+  if (validpage == NO_VALID_PAGE)
+  {
+    return  NO_VALID_PAGE;
+  }
+
+  /* Get the valid Page start Address */
+  PageStartAddress = (uint32_t)(EEPROM_START_ADDRESS + (uint32_t)(validpage * PAGE_SIZE));
+
+  /* Get the valid Page end Address */
+  address = (uint32_t)((EEPROM_START_ADDRESS - 2) + (uint32_t)((1 + validpage) * PAGE_SIZE));
+
+  /* Check each active page address starting from end */
+  while (address > (PageStartAddress + 2))
+  {
+    /* Get the current location content to be compared with virtual address */
+    addressvalue = (*(__IO uint16_t*)address);
+
+    /* Compare the read address with the virtual address */
+    if (addressvalue == VirtAddress)
+    {
+      /* Get content of Address-2 which is variable value */
+      *Data = (*(__IO uint16_t*)(address - 2));
+
+      /* In case variable value is read, reset readstatus flag */
+      readstatus = 0;
+
+      break;
+    }
+    else
+    {
+      /* Next address location */
+      address = address - 4;
+    }
+  }
+
+  /* Return readstatus value: (0: variable exist, 1: variable doesn't exist) */
+  return readstatus;
+}
+
+/**
+  * @brief  Writes/upadtes variable data in EEPROM.
+  * @param  VirtAddress: Variable virtual address
+  * @param  Data: 16 bit data to be written
+  * @retval Success or error status:
+  *           - FLASH_COMPLETE: on success
+  *           - PAGE_FULL: if valid page is full
+  *           - NO_VALID_PAGE: if no valid page was found
+  *           - Flash error code: on write Flash error
+  */
+uint16_t EE_WriteVariable(uint16_t VirtAddress, uint16_t Data)
+{
+  uint16_t Status = 0;
+
+  /* Write the variable virtual address and value in the EEPROM */
+  Status = EE_VerifyPageFullWriteVariable(VirtAddress, Data);
+
+  /* In case the EEPROM active page is full */
+  if (Status == PAGE_FULL)
+  {
+    /* Perform Page transfer */
+    Status = EE_PageTransfer(VirtAddress, Data);
+  }
+
+  /* Return last operation status */
+  return Status;
+}
+
+/**
+  * @brief  Erases PAGE and PAGE1 and writes VALID_PAGE header to PAGE
+  * @param  None
+  * @retval Status of the last operation (Flash write or erase) done during
+  *         EEPROM formating
+  */
+static HAL_StatusTypeDef EE_Format(void)
+{
+  HAL_StatusTypeDef flashstatus = HAL_OK;
+  uint32_t page_error = 0;
+  FLASH_EraseInitTypeDef s_eraseinit;
+
+  s_eraseinit.TypeErase   = FLASH_TYPEERASE_PAGES;
+  s_eraseinit.PageAddress = PAGE0_ID;
+  s_eraseinit.NbPages     = 1;
+  /* Erase Page0 */
+  if(!EE_VerifyPageFullyErased(PAGE0_BASE_ADDRESS))
+  {
+    flashstatus = HAL_FLASHEx_Erase(&s_eraseinit, &page_error);
+    /* If erase operation was failed, a Flash error code is returned */
+    if (flashstatus != HAL_OK)
+    {
+      return flashstatus;
+    }
+  }
+  /* Set Page0 as valid page: Write VALID_PAGE at Page0 base address */
+  flashstatus = HAL_FLASH_Program(FLASH_TYPEPROGRAM_HALFWORD, PAGE0_BASE_ADDRESS, VALID_PAGE);
+  /* If program operation was failed, a Flash error code is returned */
+  if (flashstatus != HAL_OK)
+  {
+    return flashstatus;
+  }
+
+  s_eraseinit.PageAddress = PAGE1_ID;
+  /* Erase Page1 */
+  if(!EE_VerifyPageFullyErased(PAGE1_BASE_ADDRESS))
+  {
+    flashstatus = HAL_FLASHEx_Erase(&s_eraseinit, &page_error);
+    /* If erase operation was failed, a Flash error code is returned */
+    if (flashstatus != HAL_OK)
+    {
+      return flashstatus;
+    }
+  }
+
+  return HAL_OK;
+}
+
+/**
+  * @brief  Find valid Page for write or read operation
+  * @param  Operation: operation to achieve on the valid page.
+  *   This parameter can be one of the following values:
+  *     @arg READ_FROM_VALID_PAGE: read operation from valid page
+  *     @arg WRITE_IN_VALID_PAGE: write operation from valid page
+  * @retval Valid page number (PAGE or PAGE1) or NO_VALID_PAGE in case
+  *   of no valid page was found
+  */
+static uint16_t EE_FindValidPage(uint8_t Operation)
+{
+  uint16_t pagestatus0 = 6, pagestatus1 = 6;
+
+  /* Get Page0 actual status */
+  pagestatus0 = (*(__IO uint16_t*)PAGE0_BASE_ADDRESS);
+
+  /* Get Page1 actual status */
+  pagestatus1 = (*(__IO uint16_t*)PAGE1_BASE_ADDRESS);
+
+  /* Write or read operation */
+  switch (Operation)
+  {
+    case WRITE_IN_VALID_PAGE:   /* ---- Write operation ---- */
+      if (pagestatus1 == VALID_PAGE)
+      {
+        /* Page0 receiving data */
+        if (pagestatus0 == RECEIVE_DATA)
+        {
+          return PAGE0;         /* Page0 valid */
+        }
+        else
+        {
+          return PAGE1;         /* Page1 valid */
+        }
+      }
+      else if (pagestatus0 == VALID_PAGE)
+      {
+        /* Page1 receiving data */
+        if (pagestatus1 == RECEIVE_DATA)
+        {
+          return PAGE1;         /* Page1 valid */
+        }
+        else
+        {
+          return PAGE0;         /* Page0 valid */
+        }
+      }
+      else
+      {
+        return NO_VALID_PAGE;   /* No valid Page */
+      }
+
+    case READ_FROM_VALID_PAGE:  /* ---- Read operation ---- */
+      if (pagestatus0 == VALID_PAGE)
+      {
+        return PAGE0;           /* Page0 valid */
+      }
+      else if (pagestatus1 == VALID_PAGE)
+      {
+        return PAGE1;           /* Page1 valid */
+      }
+      else
+      {
+        return NO_VALID_PAGE ;  /* No valid Page */
+      }
+
+    default:
+      return PAGE0;             /* Page0 valid */
+  }
+}
+
+/**
+  * @brief  Verify if active page is full and Writes variable in EEPROM.
+  * @param  VirtAddress: 16 bit virtual address of the variable
+  * @param  Data: 16 bit data to be written as variable value
+  * @retval Success or error status:
+  *           - FLASH_COMPLETE: on success
+  *           - PAGE_FULL: if valid page is full
+  *           - NO_VALID_PAGE: if no valid page was found
+  *           - Flash error code: on write Flash error
+  */
+static uint16_t EE_VerifyPageFullWriteVariable(uint16_t VirtAddress, uint16_t Data)
+{
+  HAL_StatusTypeDef flashstatus = HAL_OK;
+  uint16_t validpage = PAGE0;
+  uint32_t address = EEPROM_START_ADDRESS, pageendaddress = EEPROM_START_ADDRESS+PAGE_SIZE;
+
+  /* Get valid Page for write operation */
+  validpage = EE_FindValidPage(WRITE_IN_VALID_PAGE);
+
+  /* Check if there is no valid page */
+  if (validpage == NO_VALID_PAGE)
+  {
+    return  NO_VALID_PAGE;
+  }
+
+  /* Get the valid Page start address */
+  address = (uint32_t)(EEPROM_START_ADDRESS + (uint32_t)(validpage * PAGE_SIZE));
+
+  /* Get the valid Page end address */
+  pageendaddress = (uint32_t)((EEPROM_START_ADDRESS - 1) + (uint32_t)((validpage + 1) * PAGE_SIZE));
+
+  /* Check each active page address starting from begining */
+  while (address < pageendaddress)
+  {
+    /* Verify if address and address+2 contents are 0xFFFFFFFF */
+    if ((*(__IO uint32_t*)address) == 0xFFFFFFFF)
+    {
+      /* Set variable data */
+      flashstatus = HAL_FLASH_Program(FLASH_TYPEPROGRAM_HALFWORD, address, Data);
+      /* If program operation was failed, a Flash error code is returned */
+      if (flashstatus != HAL_OK)
+      {
+        return flashstatus;
+      }
+      /* Set variable virtual address */
+      flashstatus = HAL_FLASH_Program(FLASH_TYPEPROGRAM_HALFWORD, address + 2, VirtAddress);       
+      /* Return program operation status */
+      return flashstatus;
+    }
+    else
+    {
+      /* Next address location */
+      address = address + 4;
+    }
+  }
+
+  /* Return PAGE_FULL in case the valid page is full */
+  return PAGE_FULL;
+}
+
+/**
+  * @brief  Transfers last updated variables data from the full Page to
+  *   an empty one.
+  * @param  VirtAddress: 16 bit virtual address of the variable
+  * @param  Data: 16 bit data to be written as variable value
+  * @retval Success or error status:
+  *           - FLASH_COMPLETE: on success
+  *           - PAGE_FULL: if valid page is full
+  *           - NO_VALID_PAGE: if no valid page was found
+  *           - Flash error code: on write Flash error
+  */
+static uint16_t EE_PageTransfer(uint16_t VirtAddress, uint16_t Data)
+{
+  HAL_StatusTypeDef flashstatus = HAL_OK;
+  uint32_t newpageaddress = EEPROM_START_ADDRESS;
+  uint32_t oldpageid = 0;
+  uint16_t validpage = PAGE0, varidx = 0;
+  uint16_t eepromstatus = 0, readstatus = 0;
+  uint32_t page_error = 0;
+  FLASH_EraseInitTypeDef s_eraseinit;
+
+  /* Get active Page for read operation */
+  validpage = EE_FindValidPage(READ_FROM_VALID_PAGE);
+
+  if (validpage == PAGE1)       /* Page1 valid */
+  {
+    /* New page address where variable will be moved to */
+    newpageaddress = PAGE0_BASE_ADDRESS;
+
+    /* Old page ID where variable will be taken from */
+    oldpageid = PAGE1_ID;
+  }
+  else if (validpage == PAGE0)  /* Page0 valid */
+  {
+    /* New page address  where variable will be moved to */
+    newpageaddress = PAGE1_BASE_ADDRESS;
+
+    /* Old page ID where variable will be taken from */
+    oldpageid = PAGE0_ID;
+  }
+  else
+  {
+    return NO_VALID_PAGE;       /* No valid Page */
+  }
+
+  /* Set the new Page status to RECEIVE_DATA status */
+  flashstatus = HAL_FLASH_Program(FLASH_TYPEPROGRAM_HALFWORD, newpageaddress, RECEIVE_DATA);
+  /* If program operation was failed, a Flash error code is returned */
+  if (flashstatus != HAL_OK)
+  {
+    return flashstatus;
+  }
+
+  /* Write the variable passed as parameter in the new active page */
+  eepromstatus = EE_VerifyPageFullWriteVariable(VirtAddress, Data);
+  /* If program operation was failed, a Flash error code is returned */
+  if (eepromstatus != HAL_OK)
+  {
+    return eepromstatus;
+  }
+
+  /* Transfer process: transfer variables from old to the new active page */
+  for (varidx = 0; varidx < NB_OF_VAR; varidx++)
+  {
+    if (VirtAddVarTab[varidx] != VirtAddress)  /* Check each variable except the one passed as parameter */
+    {
+      /* Read the other last variable updates */
+      readstatus = EE_ReadVariable(VirtAddVarTab[varidx], &DataVar);
+      /* In case variable corresponding to the virtual address was found */
+      if (readstatus != 0x1)
+      {
+        /* Transfer the variable to the new active page */
+        eepromstatus = EE_VerifyPageFullWriteVariable(VirtAddVarTab[varidx], DataVar);
+        /* If program operation was failed, a Flash error code is returned */
+        if (eepromstatus != HAL_OK)
+        {
+          return eepromstatus;
+        }
+      }
+    }
+  }
+
+  s_eraseinit.TypeErase   = FLASH_TYPEERASE_PAGES;
+  s_eraseinit.PageAddress = oldpageid;
+  s_eraseinit.NbPages     = 1;
+
+  /* Erase the old Page: Set old Page status to ERASED status */
+  flashstatus = HAL_FLASHEx_Erase(&s_eraseinit, &page_error);
+  /* If erase operation was failed, a Flash error code is returned */
+  if (flashstatus != HAL_OK)
+  {
+    return flashstatus;
+  }
+
+  /* Set new Page status to VALID_PAGE status */
+  flashstatus = HAL_FLASH_Program(FLASH_TYPEPROGRAM_HALFWORD, newpageaddress, VALID_PAGE);
+  /* If program operation was failed, a Flash error code is returned */
+  if (flashstatus != HAL_OK)
+  {
+    return flashstatus;
+  }
+
+
+
+  /* Return last operation flash status */
+  return flashstatus;
+}
+
+/**
+  * @}
+  */
+
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
--- a/Src/hd44780.c
+++ b/Src/hd44780.c
@ -0,0 +1,402 @@
+/*
+ *  * hd44780.c
+ *
+ *  Created on: Feb 20, 2014
+ *      Author: Peter
+ */
+
+#include "hd44780.h"
+
+uint32_t PCF8574_Type0Pins[8] = { 4, 5, 6, 7, 0, 1, 2, 3 };
+uint8_t LCDerrorFlag = 0;
+
+void LCD_WaitForBusyFlag(LCD_PCF8574_HandleTypeDef* handle) {
+	uint8_t flag;
+	LCD_GetBusyFlag(handle, &flag);
+	//uint32_t startTick=HAL_GetTick();
+	//while (flag == 1 && HAL_GetTick()-startTick<handle->pcf8574.PCF_I2C_TIMEOUT) {
+	//	LCD_GetBusyFlag(handle, &flag);
+	//}
+	return;
+}
+
+LCD_RESULT LCD_I2C_WriteOut(LCD_PCF8574_HandleTypeDef* handle) {
+	if (!LCDerrorFlag) {
+		if (PCF8574_Write(&handle->pcf8574, handle->state) != PCF8574_OK) {
+			//handle->errorCallback(LCD_ERROR);
+			LCDerrorFlag = 1;
+			return LCD_ERROR;
+		}
+		return LCD_OK;
+	}
+	return LCD_ERROR;
+}
+
+LCD_RESULT LCD_StateLEDControl(LCD_PCF8574_HandleTypeDef* handle, uint8_t on) {
+	return LCD_StateWriteBit(handle, on & 1, LCD_PIN_LED);
+}
+
+LCD_RESULT LCD_StateWriteBit(LCD_PCF8574_HandleTypeDef* handle, uint8_t value,
+		LCD_PIN pin) {
+
+	if (value) {
+		handle->state |= 1 << handle->pins[pin];
+	} else {
+		handle->state &= ~(1 << handle->pins[pin]);
+	}
+	return LCD_I2C_WriteOut(handle);
+}
+
+LCD_RESULT LCD_Init(LCD_PCF8574_HandleTypeDef* handle) {
+	handle->D = 1;
+	handle->B = 0;
+	handle->C = 0;
+	if (handle->type == TYPE0) {
+		handle->pins = PCF8574_Type0Pins;
+	} else {
+		//handle->errorCallback(LCD_ERROR);
+		return LCD_ERROR;	// no type of subinterface was specified
+	}
+	if (PCF8574_Init(&handle->pcf8574) != PCF8574_OK) {
+		//handle->errorCallback(LCD_ERROR);
+		return LCD_ERROR;
+	}
+
+	//HAL_Delay(50);
+	LCD_StateWriteBit(handle, 0, LCD_PIN_RS);
+	LCD_StateWriteBit(handle, 0, LCD_PIN_RW);
+	LCD_StateWriteBit(handle, 0, LCD_PIN_E);
+
+	LCD_WriteToDataBus(handle, 3);
+
+	LCD_StateWriteBit(handle, 1, LCD_PIN_E);
+	HAL_Delay(1);
+	LCD_StateWriteBit(handle, 0, LCD_PIN_E);
+	HAL_Delay(5);
+
+	LCD_WriteToDataBus(handle, 3);
+
+	LCD_StateWriteBit(handle, 1, LCD_PIN_E);
+	HAL_Delay(1);
+	LCD_StateWriteBit(handle, 0, LCD_PIN_E);
+	HAL_Delay(1);
+
+	LCD_WriteToDataBus(handle, 3);
+
+	LCD_StateWriteBit(handle, 1, LCD_PIN_E);
+	HAL_Delay(1);
+	LCD_StateWriteBit(handle, 0, LCD_PIN_E);
+	HAL_Delay(1);
+
+	LCD_WriteToDataBus(handle, 2);
+
+	LCD_StateWriteBit(handle, 1, LCD_PIN_E);
+	HAL_Delay(1);
+	LCD_StateWriteBit(handle, 0, LCD_PIN_E);
+	HAL_Delay(1);
+
+	uint8_t cmd = 0;
+	cmd = cmd | (handle->NUMBER_OF_LINES << 3);
+	cmd = cmd | (1 << 5);
+
+	LCD_WriteCMD(handle, cmd);	// setting interface
+
+	cmd = 0;
+	cmd = cmd | (1 << 3);
+	cmd = cmd | (handle->C << 1);
+	cmd = cmd | handle->B;
+
+	LCD_WriteCMD(handle, cmd);	// setting display/cursor
+
+	LCD_ClearDisplay(handle);
+
+	LCD_EntryModeSet(handle, DIRECTION_INCREMENT, SHIFT_NO);
+
+	LCD_DisplayON(handle);
+
+	LCD_StateLEDControl(handle, 1);	// LED power on
+
+	return LCD_OK;
+
+}
+
+LCD_RESULT LCD_WriteToDataBus(LCD_PCF8574_HandleTypeDef* handle, uint8_t data) {
+	if ((data & 1) == 1) {
+		handle->state |= 1 << handle->pins[LCD_PIN_D4];
+	} else {
+		handle->state &= ~(1 << handle->pins[LCD_PIN_D4]);
+	}
+
+	if ((data & 2) == 2) {
+		handle->state |= 1 << handle->pins[LCD_PIN_D5];
+	} else {
+		handle->state &= ~(1 << handle->pins[LCD_PIN_D5]);
+	}
+
+	if ((data & 4) == 4) {
+		handle->state |= 1 << handle->pins[LCD_PIN_D6];
+	} else {
+		handle->state &= ~(1 << handle->pins[LCD_PIN_D6]);
+	}
+
+	if ((data & 8) == 8) {
+		handle->state |= 1 << handle->pins[LCD_PIN_D7];
+	} else {
+		handle->state &= ~(1 << handle->pins[LCD_PIN_D7]);
+	}
+
+	return LCD_I2C_WriteOut(handle);
+}
+
+LCD_RESULT LCD_GetBusyFlag(LCD_PCF8574_HandleTypeDef* handle, uint8_t* flag) {
+
+	LCD_StateWriteBit(handle, 0, LCD_PIN_E);
+	LCD_StateWriteBit(handle, 0, LCD_PIN_RS);
+	LCD_StateWriteBit(handle, 1, LCD_PIN_RW);
+
+	LCD_StateWriteBit(handle, 1, LCD_PIN_E);
+
+	//PCF8574_Read(&handle->pcf8574, flag);
+
+	//*flag &= 1 << handle->pins[LCD_PIN_D7];
+	//*flag >>= handle->pins[LCD_PIN_D7];
+
+	LCD_StateWriteBit(handle, 0, LCD_PIN_E);
+
+	LCD_StateWriteBit(handle, 1, LCD_PIN_E);
+
+	//uint8_t flag2;
+	//PCF8574_Read(&handle->pcf8574, &flag2);
+
+	LCD_StateWriteBit(handle, 0, LCD_PIN_E);
+	LCD_StateWriteBit(handle, 0, LCD_PIN_RW);
+
+	return LCD_OK;
+
+}
+
+LCD_RESULT LCD_WriteCMD(LCD_PCF8574_HandleTypeDef* handle, uint8_t cmd) {
+	if (!LCDerrorFlag) {
+		LCD_StateWriteBit(handle, 0, LCD_PIN_E);
+		LCD_StateWriteBit(handle, 0, LCD_PIN_RS);
+
+		LCD_WriteToDataBus(handle, cmd >> 4);
+		LCD_StateWriteBit(handle, 1, LCD_PIN_E);
+		LCD_StateWriteBit(handle, 0, LCD_PIN_E);
+
+		LCD_WriteToDataBus(handle, cmd);
+		LCD_StateWriteBit(handle, 1, LCD_PIN_E);
+		LCD_StateWriteBit(handle, 0, LCD_PIN_E);
+
+		LCD_WaitForBusyFlag(handle);
+
+		return LCD_OK;
+	} return LCD_ERROR;
+
+}
+
+LCD_RESULT LCD_WriteDATA(LCD_PCF8574_HandleTypeDef* handle, uint8_t data) {
+
+	LCD_StateWriteBit(handle, 0, LCD_PIN_E);
+	LCD_StateWriteBit(handle, 1, LCD_PIN_RS);
+
+	LCD_WriteToDataBus(handle, data >> 4);
+	LCD_StateWriteBit(handle, 1, LCD_PIN_E);
+	LCD_StateWriteBit(handle, 0, LCD_PIN_E);
+
+	LCD_WriteToDataBus(handle, data);
+	LCD_StateWriteBit(handle, 1, LCD_PIN_E);
+	LCD_StateWriteBit(handle, 0, LCD_PIN_E);
+
+	LCD_WaitForBusyFlag(handle);
+
+	return LCD_OK;
+
+}
+
+LCD_RESULT LCD_SetLocation(LCD_PCF8574_HandleTypeDef* handle, uint8_t x,
+		uint8_t y) {
+
+	uint8_t add = 0x40 * y + x;
+	uint8_t cmd = 1 << 7;
+	cmd = cmd | add;
+	return LCD_WriteCMD(handle, cmd);
+
+}
+
+LCD_RESULT LCD_WriteString(LCD_PCF8574_HandleTypeDef* handle, char *s) {
+	int i = 0;
+
+	if (s != 0) {
+
+		while (i < 80 && s[i] != 0) {
+			LCD_WaitForBusyFlag(handle);
+			LCD_WriteDATA(handle, s[i]);
+			i++;
+		}
+	}
+	return LCD_OK;
+}
+
+LCD_RESULT LCD_ClearDisplay(LCD_PCF8574_HandleTypeDef* handle) {
+	return LCD_WriteCMD(handle, 1);
+}
+
+LCD_RESULT LCD_DisplayON(LCD_PCF8574_HandleTypeDef* handle) {
+	handle->D = 1;
+	uint8_t cmd = 0;
+	cmd = cmd | (1 << 3);
+	cmd = cmd | (handle->D << 2);
+	cmd = cmd | (handle->C << 1);
+	cmd = cmd | handle->B;
+	return LCD_WriteCMD(handle, cmd);
+}
+
+LCD_RESULT LCD_DisplayOFF(LCD_PCF8574_HandleTypeDef* handle) {
+	handle->D = 0;
+	uint8_t cmd = 0;
+	cmd = cmd | (1 << 3);
+	cmd = cmd | (handle->D << 2);
+	cmd = cmd | (handle->C << 1);
+	cmd = cmd | handle->B;
+	return LCD_WriteCMD(handle, cmd);
+}
+
+LCD_RESULT LCD_CursorON(LCD_PCF8574_HandleTypeDef* handle, uint8_t blink) {
+	handle->C = 1;
+	blink &= 1;
+	handle->B = blink;
+	uint8_t cmd = 0;
+	cmd = cmd | (1 << 3);
+	cmd = cmd | (handle->D << 2);
+	cmd = cmd | (handle->C << 1);
+	cmd = cmd | handle->B;
+	return LCD_WriteCMD(handle, cmd);
+}
+
+LCD_RESULT LCD_CursorOFF(LCD_PCF8574_HandleTypeDef* handle) {
+	handle->C = 0;
+	uint8_t cmd = 0;
+	cmd = cmd | (1 << 3);
+	cmd = cmd | (handle->D << 2);
+	cmd = cmd | (handle->C << 1);
+	cmd = cmd | handle->B;
+	return LCD_WriteCMD(handle, cmd);
+}
+
+LCD_RESULT LCD_ShiftCursor(LCD_PCF8574_HandleTypeDef* handle, uint8_t direction,
+		uint8_t steps) {
+	direction &= 1;
+
+	uint8_t cmd = 0;
+	cmd |= 1 << 4;
+	cmd |= direction << 2;
+
+	int i = 0;
+	for (i = 0; i < steps; i++) {
+		if (LCD_WriteCMD(handle, cmd) != LCD_OK) {
+			//handle->errorCallback(LCD_ERROR);
+			return LCD_ERROR;
+		}
+	}
+	return LCD_OK;
+}
+
+LCD_RESULT LCD_ShiftDisplay(LCD_PCF8574_HandleTypeDef* handle,
+		uint8_t direction, uint8_t steps) {
+	direction &= 1;
+
+	uint8_t cmd = 0;
+	cmd |= 1 << 4;
+	cmd |= 1 << 3;
+	cmd |= direction << 2;
+
+	int i = 0;
+	for (i = 0; i < steps; i++) {
+		if (LCD_WriteCMD(handle, cmd) != LCD_OK) {
+			//handle->errorCallback(LCD_ERROR);
+			return LCD_ERROR;
+		}
+	}
+	return LCD_OK;
+}
+
+LCD_RESULT LCD_WriteNumber(LCD_PCF8574_HandleTypeDef* handle, unsigned long n,
+		uint8_t base) {
+
+	char buf[8 * sizeof(long) + 1]; // Assumes 8-bit chars plus zero byte.
+	char *str = &buf[sizeof(buf) - 1];
+
+	*str = '\0';
+
+	// prevent crash if called with base == 1
+	if (base < 2)
+		base = 10;
+
+	do {
+		unsigned long m = n;
+		n /= base;
+		char c = m - base * n;
+		*--str = c < 10 ? c + '0' : c + 'A' - 10;
+	} while (n);
+	return LCD_WriteString(handle, str);
+}
+
+LCD_RESULT LCD_WriteFloat(LCD_PCF8574_HandleTypeDef* handle, double number,
+		uint8_t digits) {
+	// Handle negative numbers
+	if (number < 0.0) {
+		LCD_WriteString(handle,"-");
+		number = -number;
+	}
+
+	// Round correctly so that print(1.999, 2) prints as "2.00"
+	double rounding = 0.5;
+	for (uint8_t i = 0; i < digits; ++i)
+		rounding /= 10.0;
+
+	number += rounding;
+
+	// Extract the integer part of the number and print it
+	unsigned long int_part = (unsigned long) number;
+	double remainder = number - (double) int_part;
+	LCD_WriteNumber(handle,int_part,10);
+
+	// Print the decimal point, but only if there are digits beyond
+	if (digits > 0) {
+		LCD_WriteString(handle,".");
+	}
+
+	// Extract digits from the remainder one at a time
+	while (digits-- > 0) {
+		remainder *= 10.0;
+		int toPrint = (int)(remainder);
+		LCD_WriteNumber(handle,toPrint,10);
+		remainder -= toPrint;
+	}
+	return LCD_OK;
+}
+
+LCD_RESULT LCD_EntryModeSet(LCD_PCF8574_HandleTypeDef* handle,
+		LCD_DIRECTION_INC_DEC direction, LCD_SHIFT shift) {
+
+	uint8_t cmd = 0;
+	cmd |= 1 << 2;
+	cmd |= direction << 1;
+	cmd |= shift;
+
+	return LCD_WriteCMD(handle, cmd);
+
+}
+
+LCD_RESULT LCD_CustomChar(LCD_PCF8574_HandleTypeDef* handle, uint8_t *pattern,
+		uint8_t address) {
+	uint8_t a = 0;
+	int i = 0;
+	a = 8 * address;
+	LCD_WriteCMD(handle, a | 0x40);
+	for (i = 0; i < 8; i++) {
+		LCD_WriteDATA(handle, pattern[i]);
+	}
+	return LCD_OK;
+}
--- a/Src/main.c
+++ b/Src/main.c
@ -23,7 +23,37 @@
 #include "defines.h"
 #include "setup.h"
 #include "config.h"
-//#include "hd44780.h"
+
+#ifdef CONTROL_GAMETRAK
+#include "hd44780.h"
+#include "eeprom.h"
+LCD_PCF8574_HandleTypeDef lcd;
+extern I2C_HandleTypeDef hi2c2;
+
+extern uint8_t LCDerrorFlag;
+
+uint8_t nunchuck_connected = 0;
+
+float steering;
+int feedforward;
+
+
+void longBeep(void);
+void shortBeep(void);
+
+/* Virtual address defined by the user: 0xFFFF value is prohibited */
+uint16_t VirtAddVarTab[NB_OF_VAR] = {0x1337};
+uint16_t VarDataTab[NB_OF_VAR] = {0};
+uint16_t VarValue = 0;
+uint16_t saveValue = 0;
+
+extern volatile float currentR;
+extern volatile float currentL;
+
+uint16_t counter = 0;
+
+#endif
+

 void SystemClock_Config(void);

@ -127,11 +157,104 @@ int main(void) {
  HAL_ADC_Start(&hadc1);
  HAL_ADC_Start(&hadc2);

+  #ifndef CONTROL_GAMETRAK
  for (int i = 8; i >= 0; i--) {
    buzzerFreq = i;
    HAL_Delay(100);
  }
  buzzerFreq = 0;
+  #endif
+
+  #ifdef CONTROL_GAMETRAK
+  for (int i = 4; i >= 0; i--) {
+    buzzerFreq = i*2;
+    HAL_Delay(100);
+  }
+  for (int i = 4; i >= 0; i--) {
+    buzzerFreq = i*2-1;
+    HAL_Delay(100);
+  }
+  buzzerFreq = 0;
+
+  int  lastDistance = 0;
+  enable = 1;
+  uint8_t checkRemote = 0;
+
+  HAL_FLASH_Unlock();
+
+  /* EEPROM Init */
+  EE_Init();
+
+  EE_ReadVariable(VirtAddVarTab[0], &saveValue);
+
+  HAL_FLASH_Lock();
+  float setDistance = saveValue / 1000.0;
+  if (setDistance < 0.2) {
+    setDistance = 1.0;
+  }
+  #endif
+
+  #ifdef SUPPORT_LCD
+  I2C_Init();
+  //Led_init();
+
+  lcd.pcf8574.PCF_I2C_ADDRESS = 0x27;
+	lcd.pcf8574.PCF_I2C_TIMEOUT = 5;
+	lcd.pcf8574.i2c = hi2c2;
+	lcd.NUMBER_OF_LINES = NUMBER_OF_LINES_2;
+	lcd.type = TYPE0;
+
+	if(LCD_Init(&lcd)!=LCD_OK){
+		// error occured
+		//TODO while(1);
+	}
+
+  HAL_Delay(50);
+  LCD_ClearDisplay(&lcd);
+  HAL_Delay(50);
+  LCD_SetLocation(&lcd, 0, 0);
+  LCD_WriteString(&lcd, "TranspOtter V2.1");
+
+  LCD_SetLocation(&lcd, 0, 1);
+  LCD_WriteString(&lcd, "Initializing...");
+
+  int buttonTimeout = 0;
+
+  #ifdef SUPPORT_REMOTE
+  checkRemote = 1;
+  while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
+    buttonTimeout++;
+    HAL_Delay(100);
+    if(buttonTimeout > 20) {
+      LCD_ClearDisplay(&lcd);
+      HAL_Delay(5);
+      LCD_SetLocation(&lcd, 0, 0);
+      LCD_WriteString(&lcd, "Starting without");
+      LCD_SetLocation(&lcd, 0, 1);
+      LCD_WriteString(&lcd, "remote E-off!");
+      checkRemote = 0;
+      HAL_Delay(2000);
+    }
+  }
+  #endif
+
+  LCD_ClearDisplay(&lcd);
+  HAL_Delay(5);
+  LCD_SetLocation(&lcd, 0, 1);
+	LCD_WriteString(&lcd, "Bat:");
+  LCD_SetLocation(&lcd, 8, 1);
+  LCD_WriteString(&lcd, "V");
+
+  LCD_SetLocation(&lcd, 15, 1);
+  LCD_WriteString(&lcd, "A");
+
+  LCD_SetLocation(&lcd, 0, 0);
+	LCD_WriteString(&lcd, "Len:");
+  LCD_SetLocation(&lcd, 8, 0);
+  LCD_WriteString(&lcd, "m(");
+  LCD_SetLocation(&lcd, 14, 0);
+  LCD_WriteString(&lcd, "m)");
+  #endif

  HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1);

@ -157,15 +280,15 @@ int main(void) {
    I2C_Init();
    HAL_Delay(50);
    lcd.pcf8574.PCF_I2C_ADDRESS = 0x27;
-      lcd.pcf8574.PCF_I2C_TIMEOUT = 5;
-      lcd.pcf8574.i2c = hi2c2;
-      lcd.NUMBER_OF_LINES = NUMBER_OF_LINES_2;
-      lcd.type = TYPE0;
+    lcd.pcf8574.PCF_I2C_TIMEOUT = 1;
+    lcd.pcf8574.i2c = hi2c2;
+    lcd.NUMBER_OF_LINES = NUMBER_OF_LINES_2;
+    lcd.type = TYPE0;

-      if(LCD_Init(&lcd)!=LCD_OK){
-          // error occured
-          //TODO while(1);
-      }
+    if(LCD_Init(&lcd)!=LCD_OK){
+        // error occured
+        //TODO while(1);
+    }

    LCD_ClearDisplay(&lcd);
    HAL_Delay(5);
@ -183,102 +306,189 @@ int main(void) {
  while(1) {
    HAL_Delay(DELAY_IN_MAIN_LOOP); //delay in ms

-    #ifdef CONTROL_NUNCHUCK
+    #ifdef CONTROL_GAMETRAK
+    if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
+      enable = 0;
+      while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
+        HAL_Delay(10);
+      }
+      shortBeep();
+      HAL_Delay(300);
+      if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
+        while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
+          HAL_Delay(10);
+        }
+        longBeep();
+        HAL_Delay(350);
+        //Power_Set(0);
+        poweroff();
+      } else {
+        setDistance += 0.25;
+        if (setDistance > 2.6) {
+          setDistance = 0.5;
+        }
+        saveValue = setDistance * 1000;
+        saveConfig();
+      }
+    }
+
+
+
+
+    #ifdef GAMETRAK_CONNECTION_NORMAL
+    uint16_t distance = CLAMP((adc_buffer.l_rx2) - 180, 0, 4095);
+    steering = (adc_buffer.l_tx2 - 2048) / 2048.0;
+    #endif
+    #ifdef GAMETRAK_CONNECTION_ALTERNATE
+    uint16_t distance = CLAMP((adc_buffer.l_tx2) - 180, 0, 4095);
+    steering = (adc_buffer.l_rx2 - 2048) / 2048.0;
+    #endif
+
+    feedforward = ((distance - (int)(setDistance * 1345)));
+    if (nunchuck_connected == 0) {
+      speedL = speedL * 0.8f + (CLAMP(feedforward +  ((steering)*((float)MAX(ABS(feedforward), 50)) * ROT_P), -850, 850) * -0.2f);
+      speedR = speedR * 0.8f + (CLAMP(feedforward -  ((steering)*((float)MAX(ABS(feedforward), 50)) * ROT_P), -850, 850) * -0.2f);
+      if ((speedL < lastSpeedL + 50 && speedL > lastSpeedL - 50) && (speedR < lastSpeedR + 50 && speedR > lastSpeedR - 50)) {
+        if (distance - (int)(setDistance * 1345) > 0) {
+          enable = 1;
+        }
+        if (distance - (int)(setDistance * 1345) > -300) {
+          #ifdef INVERT_R_DIRECTION
+          pwmr = -speedR;
+          #endif
+          #ifndef INVERT_R_DIRECTION
+          pwmr = speedR;
+          #endif
+
+          #ifdef INVERT_L_DIRECTION
+          pwml = -speedL;
+          #endif
+          #ifndef INVERT_L_DIRECTION
+          pwml = speedL;
+          #endif
+
+          if (checkRemote) {
+            if (!HAL_GPIO_ReadPin(LED_PORT, LED_PIN)) {
+              //enable = 1;
+            } else {
+              enable = 0;
+            }
+          }
+        } else {
+          enable = 0;
+        }
+      }
+      lastSpeedL = speedL;
+      lastSpeedR = speedR;
+
+      timeout = 0;
+    } else {
      Nunchuck_Read();
+      enable = 1;
      cmd1 = CLAMP((nunchuck_data[0] - 127) * 8, -1000, 1000); // x - axis. Nunchuck joystick readings range 30 - 230
      cmd2 = CLAMP((nunchuck_data[1] - 128) * 8, -1000, 1000); // y - axis

-      button1 = (uint8_t)nunchuck_data[5] & 1;
-      button2 = (uint8_t)(nunchuck_data[5] >> 1) & 1;
-    #endif
-
-    #ifdef CONTROL_PPM
-      cmd1 = CLAMP((ppm_captured_value[0] - 500) * 2, -1000, 1000);
-      cmd2 = CLAMP((ppm_captured_value[1] - 500) * 2, -1000, 1000);
-      button1 = ppm_captured_value[5] > 500;
-      float scale = ppm_captured_value[2] / 1000.0f;
-    #endif
-
-    #ifdef CONTROL_ADC
-      // ADC values range: 0-4095, see ADC-calibration in config.h
-      cmd1 = CLAMP(adc_buffer.l_tx2 - ADC1_MIN, 0, ADC1_MAX) / (ADC1_MAX / 1000.0f);  // ADC1
-      cmd2 = CLAMP(adc_buffer.l_rx2 - ADC2_MIN, 0, ADC2_MAX) / (ADC2_MAX / 1000.0f);  // ADC2
-
-      // use ADCs as button inputs:
-      button1 = (uint8_t)(adc_buffer.l_tx2 > 2000);  // ADC1
-      button2 = (uint8_t)(adc_buffer.l_rx2 > 2000);  // ADC2
-
-      timeout = 0;
-    #endif
-
-    #ifdef CONTROL_SERIAL_USART2
-      cmd1 = CLAMP((int16_t)command.steer, -1000, 1000);
-      cmd2 = CLAMP((int16_t)command.speed, -1000, 1000);
-
-      timeout = 0;
-    #endif
+        // ####### LOW-PASS FILTER #######
+      steer = steer * (1.0 - FILTER) + cmd1 * FILTER;
+      speed = speed * (1.0 - FILTER) + cmd2 * FILTER;


-    // ####### LOW-PASS FILTER #######
-    steer = steer * (1.0 - FILTER) + cmd1 * FILTER;
-    speed = speed * (1.0 - FILTER) + cmd2 * FILTER;
+      // ####### MIXER #######
+      speedR = CLAMP(speed * SPEED_COEFFICIENT -  steer * STEER_COEFFICIENT, -900, 900);
+      speedL = CLAMP(speed * SPEED_COEFFICIENT +  steer * STEER_COEFFICIENT, -900, 900);

+      // ####### SET OUTPUTS #######
+      if ((speedL < lastSpeedL + 100 && speedL > lastSpeedL - 100) && (speedR < lastSpeedR + 100 && speedR > lastSpeedR - 100) && timeout < TIMEOUT) {
+        #ifdef INVERT_R_DIRECTION
+          pwmr = speedR;
+        #else
+          pwmr = -speedR;
+        #endif
+        #ifdef INVERT_L_DIRECTION
+          pwml = speedL;
+        #else
+          pwml = -speedL;
+        #endif
+      }
+      else if (timeout > TIMEOUT) {
+        pwml = 0;
+        pwmr = 0;
+        enable = 0;

-    // ####### MIXER #######
-    speedR = CLAMP(speed * SPEED_COEFFICIENT -  steer * STEER_COEFFICIENT, -1000, 1000);
-    speedL = CLAMP(speed * SPEED_COEFFICIENT +  steer * STEER_COEFFICIENT, -1000, 1000);
+        LCD_SetLocation(&lcd, 0, 0);
+      	LCD_WriteString(&lcd, "Len:");
+        LCD_SetLocation(&lcd, 8, 0);
+        LCD_WriteString(&lcd, "m(");
+        LCD_SetLocation(&lcd, 14, 0);
+        LCD_WriteString(&lcd, "m)");

+        HAL_Delay(1000);

-    #ifdef ADDITIONAL_CODE
-      ADDITIONAL_CODE;
-    #endif
-
-
-    // ####### SET OUTPUTS #######
-    if ((speedL < lastSpeedL + 50 && speedL > lastSpeedL - 50) && (speedR < lastSpeedR + 50 && speedR > lastSpeedR - 50) && timeout < TIMEOUT) {
-    #ifdef INVERT_R_DIRECTION
-      pwmr = speedR;
-    #else
-      pwmr = -speedR;
-    #endif
-    #ifdef INVERT_L_DIRECTION
-      pwml = -speedL;
-    #else
-      pwml = speedL;
-    #endif
-    }
-
-    lastSpeedL = speedL;
-    lastSpeedR = speedR;
-
-
-    if (inactivity_timeout_counter % 25 == 0) {
-      // ####### CALC BOARD TEMPERATURE #######
-      board_temp_adc_filtered = board_temp_adc_filtered * 0.99 + (float)adc_buffer.temp * 0.01;
-      board_temp_deg_c = ((float)TEMP_CAL_HIGH_DEG_C - (float)TEMP_CAL_LOW_DEG_C) / ((float)TEMP_CAL_HIGH_ADC - (float)TEMP_CAL_LOW_ADC) * (board_temp_adc_filtered - (float)TEMP_CAL_LOW_ADC) + (float)TEMP_CAL_LOW_DEG_C;
-      
-      // ####### DEBUG SERIAL OUT #######
-      #ifdef CONTROL_ADC
-        setScopeChannel(0, (int)adc_buffer.l_tx2);  // 1: ADC1
-        setScopeChannel(1, (int)adc_buffer.l_rx2);  // 2: ADC2
-      #endif
-      setScopeChannel(2, (int)speedR);  // 3: output speed: 0-1000
-      setScopeChannel(3, (int)speedL);  // 4: output speed: 0-1000
-      setScopeChannel(4, (int)adc_buffer.batt1);  // 5: for battery voltage calibration
-      setScopeChannel(5, (int)(batteryVoltage * 100.0f));  // 6: for verifying battery voltage calibration
-      setScopeChannel(6, (int)board_temp_adc_filtered);  // 7: for board temperature calibration
-      setScopeChannel(7, (int)board_temp_deg_c);  // 8: for verifying board temperature calibration
-      consoleScope();
+        nunchuck_connected = 0;
+      }
+      lastSpeedL = speedL;
+      lastSpeedR = speedR;
    }


-    // ####### POWEROFF BY POWER-BUTTON #######
-    if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && weakr == 0 && weakl == 0) {
+
+
+    if ((distance / 1345.0) - setDistance > 0.5 && (lastDistance / 1345.0) - setDistance > 0.5) { // Error, robot too far away!
      enable = 0;
-      while (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {}
+      longBeep();
+      #ifdef SUPPORT_LCD
+      LCD_ClearDisplay(&lcd);
+      HAL_Delay(5);
+      LCD_SetLocation(&lcd, 0, 0);
+    	LCD_WriteString(&lcd, "Emergency Off!");
+      LCD_SetLocation(&lcd, 0, 1);
+    	LCD_WriteString(&lcd, "Keeper to fast.");
+      #endif
      poweroff();
    }

+    #ifdef SUPPORT_NUNCHUCK
+    if (counter % 500 == 0) {
+      if (nunchuck_connected == 0 && enable == 0) {
+        if (Nunchuck_Ping()) {
+          HAL_Delay(500);
+          Nunchuck_Init();
+          #ifdef SUPPORT_LCD
+          LCD_SetLocation(&lcd, 0, 0);
+          LCD_WriteString(&lcd, "Nunchuck Control");
+          #endif
+          timeout = 0;
+          HAL_Delay(1000);
+          nunchuck_connected = 1;
+        }
+      }
+    }
+    #endif
+
+    #ifdef SUPPORT_LCD
+    if (counter % 100 == 0) {
+      if (LCDerrorFlag == 1 && enable == 0) {
+
+      } else {
+        if (nunchuck_connected == 0) {
+          LCD_SetLocation(&lcd, 4, 0);
+          LCD_WriteFloat(&lcd,distance/1345.0,2);
+          LCD_SetLocation(&lcd, 10, 0);
+          LCD_WriteFloat(&lcd,setDistance,2);
+        }
+        LCD_SetLocation(&lcd, 4, 1);
+        LCD_WriteFloat(&lcd,batteryVoltage, 1);
+        LCD_SetLocation(&lcd, 11, 1);
+        LCD_WriteFloat(&lcd,MAX(ABS(currentR), ABS(currentL)),2);
+      }
+    }
+
+
+    #endif
+    counter++;
+    #endif
+
+

    // ####### BEEP AND EMERGENCY POWEROFF #######
    if ((TEMP_POWEROFF_ENABLE && board_temp_deg_c >= TEMP_POWEROFF && abs(speed) < 20) || (batteryVoltage < ((float)BAT_LOW_DEAD * (float)BAT_NUMBER_OF_CELLS) && abs(speed) < 20)) {  // poweroff before mainboard burns OR low bat 3
@ -313,6 +523,25 @@ int main(void) {
  }
 }

+
+void longBeep(){
+    buzzerFreq = 5;
+    HAL_Delay(500);
+    buzzerFreq = 0;
+}
+
+void shortBeep(){
+    buzzerFreq = 5;
+    HAL_Delay(100);
+    buzzerFreq = 0;
+}
+
+void saveConfig() {
+  HAL_FLASH_Unlock();
+  EE_WriteVariable(VirtAddVarTab[0], saveValue);
+  HAL_FLASH_Lock();
+}
+
 /** System Clock Configuration
 */
 void SystemClock_Config(void) {
--- a/Src/pcf8574.c
+++ b/Src/pcf8574.c
@ -0,0 +1,58 @@
+/*
+ * pcf8574.c
+ *
+ *  Created on: Dec 30, 2014
+ *      Author: peter
+ */
+
+#include "pcf8574.h"
+
+PCF8574_RESULT PCF8574_Init(PCF8574_HandleTypeDef* handle) {
+
+	handle->PCF_I2C_ADDRESS &= 0x07;
+
+	/*if (handle->i2c.State == HAL_I2C_STATE_RESET) {
+		handle->i2c.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
+		handle->i2c.Init.DualAddressMode = I2C_DUALADDRESS_DISABLED;
+		handle->i2c.Init.DutyCycle = I2C_DUTYCYCLE_2;
+		handle->i2c.Init.NoStretchMode = I2C_NOSTRETCH_DISABLED;
+		handle->i2c.Init.OwnAddress1 = 0xFE;
+		if (HAL_I2C_Init(&handle->i2c) != HAL_OK) {
+			handle->errorCallback(PCF8574_ERROR);
+			return PCF8574_ERROR;
+		}
+	}*/
+	return PCF8574_OK;
+}
+
+PCF8574_RESULT PCF8574_DeInit(PCF8574_HandleTypeDef* handle) {
+	HAL_I2C_DeInit(&handle->i2c);
+	return PCF8574_OK;
+}
+
+PCF8574_RESULT PCF8574_Write(PCF8574_HandleTypeDef* handle, uint8_t val) {
+	if (HAL_I2C_Master_Transmit(&handle->i2c,
+			(handle->PCF_I2C_ADDRESS << 1) | PCF8574_I2C_ADDRESS_MASK, &val, 1,
+			handle->PCF_I2C_TIMEOUT) != HAL_OK) {
+		//handle->errorCallback(PCF8574_ERROR);
+		return PCF8574_ERROR;
+	}
+
+	/*if (HAL_I2C_Master_Transmit_DMA(&handle->i2c,
+			(handle->PCF_I2C_ADDRESS << 1) | PCF8574_I2C_ADDRESS_MASK, &val, 1) != HAL_OK) {
+		handle->errorCallback(PCF8574_ERROR);
+		//<return PCF8574_ERROR;
+	}*/
+
+	//HAL_I2C_Master_Transmit_DMA(&hi2c2, 0xA4, (uint8_t*)ai2cBuffer, 2);
+	return PCF8574_OK;
+}
+
+PCF8574_RESULT PCF8574_Read(PCF8574_HandleTypeDef* handle, uint8_t* val) {
+	if (HAL_I2C_Master_Receive(&handle->i2c,
+			(handle->PCF_I2C_ADDRESS << 1) | PCF8574_I2C_ADDRESS_MASK, val, 1,
+			handle->PCF_I2C_TIMEOUT) != HAL_OK) {
+		return PCF8574_ERROR;
+	}
+	return PCF8574_OK;
+}
--- a/Src/setup.c
+++ b/Src/setup.c
@ -238,7 +238,7 @@ void I2C_Init()
  HAL_NVIC_EnableIRQ(DMA1_Channel5_IRQn);

  hi2c2.Instance = I2C2;
-  hi2c2.Init.ClockSpeed = 100000;
+  hi2c2.Init.ClockSpeed = 200000;
  hi2c2.Init.DutyCycle = I2C_DUTYCYCLE_2;
  hi2c2.Init.OwnAddress1 = 0;
  hi2c2.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
--- a/build/hover.hex
+++ b/build/hover.hex
Author	SHA1	Message	Date
Jan Henrik	415ba0e7ab	Update README.md	2019-01-04 08:56:05 +01:00
Jan Henrik	afffb6209c	Update README.md	2019-01-04 08:55:36 +01:00
Niklas Fauth	41a0f4b72d	ADD: define to change gametrak ADC wirering in software	2018-12-16 09:58:14 +01:00
Niklas Fauth	95dc4938cf	Firmware release for TranspOtterNG. Compatible for older TranspOtters	2018-12-16 09:54:53 +01:00
Niklas Fauth	c6e5cf5baf	TranspOtterNG firmware ready	2018-12-06 17:10:30 +01:00
Niklas Fauth	a6659bbbea	add build instrcuctions to readme	2018-12-01 23:54:08 +01:00
Niklas Fauth	b582fda321	FIX: Button poweroff. Firmware since merge still untested. Use firmware from before nov 2018 if you need it asap	2018-12-01 23:52:40 +01:00
Niklas Fauth	20ebed520f	fixed compile issues	2018-11-30 09:12:48 +01:00
Niklas Fauth	9076ae7e88	UPD: Merged current master with transpOtter Firmware modifications	2018-11-29 17:38:22 +01:00