1151 lines
38 KiB
C++
1151 lines
38 KiB
C++
#include <Arduino.h>
|
|
#define SERIAL_BAUD 115200 // [-] Baud rate for built-in Serial (used for the Serial Monitor)
|
|
|
|
unsigned long loopmillis;
|
|
unsigned long last_loopmillis;
|
|
|
|
//for testing debug
|
|
uint8_t debug_count_disarmedbecauseofnrfdelay=0;
|
|
|
|
#include <TimeLib.h> //for teensy rtc
|
|
time_t getTeensy3Time();
|
|
|
|
#include <OneButton.h>
|
|
|
|
#include <SPI.h>
|
|
#include <Wire.h>
|
|
#include <Adafruit_GFX.h>
|
|
#include <Adafruit_SSD1306.h>
|
|
//128 x 64 px
|
|
#define SCREEN_WIDTH 128
|
|
#define SCREEN_HEIGHT 64
|
|
#define SCREEN_ADDRESS 0x3C
|
|
#define OLED_RESET -1 // Reset pin # (or -1 if sharing Arduino reset pin)
|
|
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
|
|
#define DISPLAYUPDATE_INTERVAL 200
|
|
bool flag_updatedisplay=false;
|
|
|
|
uint8_t displaymode=0;
|
|
#define DISPLAY_STATS 0
|
|
#define DISPLAY_STATS2 1
|
|
#define DISPLAY_STATS3 2
|
|
#define DISPLAY_MENU 3
|
|
uint8_t menu_entrypos=0;
|
|
#define MENU_ENTRIES 8 // max id is MENU_ENTRIES-1
|
|
uint8_t menu_pagepos=0;
|
|
#define MENU_PAGES 3
|
|
|
|
uint8_t error = 0;
|
|
#define IMU_NO_CHANGE 2 //IMU values did not change for too long
|
|
|
|
#include "hoverboard-esc-serial-comm.h"
|
|
|
|
ESCSerialComm esc(Serial2);
|
|
//Serial1 = TX1=1, RX1=0
|
|
//Serial2 = TX2=10, RX2=9
|
|
//Serial3 = TX3=8, RX3=7
|
|
|
|
#define PIN_BUTTON_UP 3
|
|
#define PIN_BUTTON_INC 4
|
|
#define PIN_BUTTON_DEC 5
|
|
#define PIN_BUTTON_DOWN 6
|
|
|
|
OneButton btn_up = OneButton(PIN_BUTTON_UP, true, true); // Input pin for the button, Button is active LOW, Enable internal pull-up resistor
|
|
OneButton btn_inc = OneButton(PIN_BUTTON_INC, true, true);
|
|
OneButton btn_dec = OneButton(PIN_BUTTON_DEC, true, true);
|
|
OneButton btn_down = OneButton(PIN_BUTTON_DOWN, true, true);
|
|
|
|
//flags
|
|
bool button_up_click=false;
|
|
bool button_inc_click=false;
|
|
bool button_dec_click=false;
|
|
bool button_down_click=false;
|
|
|
|
#define PIN_GAMETRAK_LENGTH_A A6 //A6=20
|
|
#define PIN_GAMETRAK_LENGTH_B A7 //A7=21
|
|
#define PIN_GAMETRAK_VERTICAL A8 //A8=22
|
|
#define PIN_GAMETRAK_HORIZONTAL A9 //A9=23
|
|
|
|
|
|
float speed_coefficient_nrf=0.4; // higher value == stronger
|
|
float steer_coefficient_nrf=0.5; // higher value == stronger
|
|
|
|
float speed_coefficient_gt=1.0; // higher value == stronger
|
|
float steer_coefficient_gt=0.5; // higher value == stronger
|
|
|
|
unsigned long watchdogtimer=0; //set to current millis everytime new good control input is calculated
|
|
|
|
long last_adcupdated=0;
|
|
#define ADC_UPDATEPERIOD 10 //in ms
|
|
|
|
#define CONTROLUPDATEPERIOD 10
|
|
float filter_nrf_set_speed=0.06; //higher value, faster response. depends on CONTROLUPDATEPERIOD
|
|
float filter_nrf_set_steer=0.06;
|
|
|
|
float filter_stop_set_speed=0.015; //safety stop
|
|
float filter_stop_set_steer=0.06; //safety stop
|
|
|
|
#define GT_LENGTH_MIN 200 //minimum length for stuff to start happen
|
|
|
|
#define GT_LENGTH_1_OFFSET -22.5
|
|
#define GT_LENGTH_1_SCALE 2.5
|
|
#define GT_LENGTH_2_OFFSET 563.6
|
|
#define GT_LENGTH_2_SCALE 0.45
|
|
|
|
#define GT_LENGTH_CROSSOVERADC ((GT_LENGTH_2_OFFSET-GT_LENGTH_1_OFFSET)/(GT_LENGTH_1_SCALE-GT_LENGTH_2_SCALE)) //crossover point from adc, where first and second lines cross
|
|
#define GT_LENGTH_CROSSOVER_FEATHER 76.0 //how much adc change in both directions should be smoothed when switching between first and second line
|
|
|
|
#define GT_LENGTH_MAXLENGTH 2000 //maximum length in [mm]. maximum string length is around 2m80
|
|
#define GT_LENGTH_ADC_MAXDIFF 127 //maximum adc value difference between A and B poti. Used to detect scratching poti. during length calibration was 57
|
|
int raw_length_maxdiff=0;
|
|
//TODO: implement error for poti maxdiff
|
|
uint16_t gt_length=0; //0=rolled up, 1unit = 1mm
|
|
/* calibration 20220410
|
|
lenght[mm], adc
|
|
0,9
|
|
100,52
|
|
200,86
|
|
300,124
|
|
400,165
|
|
500,212
|
|
600,286
|
|
700,376
|
|
800,520
|
|
900,746
|
|
1000,984
|
|
1100,1198
|
|
1200,1404
|
|
1300,1628
|
|
1400,1853
|
|
1500,2107
|
|
1600,2316
|
|
1700,2538
|
|
1800,2730
|
|
1900,2942
|
|
2000,3150
|
|
*/
|
|
|
|
#define GT_VERTICAL_CENTER 2048 //adc value for center position
|
|
#define GT_VERTICAL_RANGE 2047 //adc value difference from center to maximum (30 deg)
|
|
int8_t gt_vertical=0; //0=center. joystick can rotate +-30 degrees. -127 = -30 deg
|
|
//left = -30 deg, right= 30deg
|
|
|
|
#define GT_HORIZONTAL_CENTER 2048 //adc value for center position
|
|
#define GT_HORIZONTAL_RANGE 2047 //adc value difference from center to maximum (30 deg)
|
|
int8_t gt_horizontal=0; //0=center
|
|
|
|
uint16_t gt_length_set=1000; //set length to keep [mm]
|
|
#define GT_LENGTH_MINDIFF 10 //[mm] threshold, do not move within gt_length_set-GT_LENGTH_MINDIFF and gt_length_set+GT_LENGTH_MINDIFF
|
|
float gt_speed_p=0.7; //value to multipy difference [mm] with -> out_speed
|
|
float gt_speedbackward_p=0.7;
|
|
float gt_steer_p=2.0;
|
|
int16_t gt_speed_limit=500; //maximum out_speed value +
|
|
int16_t gt_backward_speed_limit=100; //maximum out_speed value (for backward driving) -
|
|
int16_t gt_steer_limit=300; //maximum out_steer value +-
|
|
#define GT_LENGTH_MAXIMUMDIFFBACKWARD -250 //[mm]. if gt_length_set=1000 and GT_LENGTH_MAXIMUMDIFFBACKWARD=-200 then only drives backward if lenght is greater 800
|
|
|
|
|
|
#include <SPI.h>
|
|
#include "nRF24L01.h"
|
|
#include "RF24.h"
|
|
|
|
RF24 radio(14, 15); //ce, cs
|
|
//SCK D13 (Pro mini), A5 (bluepill),13 (teensy32)
|
|
//Miso D12 (Pro mini), A6 (bluepill),12 (teensy32)
|
|
//Mosi D11 (Pro mini), A7 (bluepill),11 (teensy32)
|
|
|
|
// Radio pipe addresses for the 2 nodes to communicate.
|
|
const uint64_t pipes[2] = { 0xF0F0F0F0E1LL, 0xF0F0F0F0D2LL };
|
|
#define NRF24CHANNEL 75
|
|
|
|
struct nrfdata {
|
|
uint8_t steer;
|
|
uint8_t speed;
|
|
uint8_t commands; //bit 0 set = motor enable, 1=left,2=up,3=down,4=right
|
|
uint8_t checksum;
|
|
};
|
|
|
|
nrfdata lastnrfdata;
|
|
long last_nrfreceive = 0; //last time values were received and checksum ok
|
|
unsigned long nrf_delay = 0;
|
|
unsigned long last_nrfreceive_delay=0;
|
|
unsigned long max_last_nrfreceive_delay=0; //maximum value (used for display)
|
|
#define MAX_NRFDELAY 200 //ms. maximum time delay at which vehicle will disarm
|
|
|
|
boolean radiosendOk=false;
|
|
|
|
|
|
//command variables
|
|
boolean motorenabled = false; //set by nrfdata.commands
|
|
boolean setup_directon_press_left=false;
|
|
boolean setup_directon_press_up=false;
|
|
boolean setup_directon_press_down=false;
|
|
boolean setup_directon_press_right=false;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
int16_t set_speed = 0;
|
|
int16_t set_steer = 0;
|
|
#define NRFDATA_CENTER 127
|
|
|
|
//boolean armed = false;
|
|
boolean lastpacketOK = false;
|
|
|
|
//Gametrak
|
|
//boolean armed_gt = false;
|
|
|
|
uint8_t controlmode=0;
|
|
#define MODE_DISARMED 0
|
|
#define MODE_RADIONRF 1
|
|
#define MODE_GAMETRAK 2
|
|
|
|
|
|
// SD Logging
|
|
#include <SPI.h> //SCK=13, MISO=12, MOSI=11
|
|
#include <SD.h> //Format sd cart with FAT or FAT16
|
|
#define SDCHIPSELECT 16
|
|
boolean datalogging=true;
|
|
String datalogging_filename="UNKNOWN.txt";
|
|
|
|
|
|
void updateInputs(unsigned long loopmillis);
|
|
void updateDisplay(unsigned long loopmillis);
|
|
void display_show_stats();
|
|
void display_show_stats2();
|
|
void display_show_stats3();
|
|
void display_show_menu();
|
|
time_t getTeensy3Time();
|
|
String addLeadingZero(int number);
|
|
String getCurrentDatestring();
|
|
|
|
void setup() {
|
|
Serial.begin(SERIAL_BAUD); //Debug and Program
|
|
|
|
Wire.begin();
|
|
// SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
|
|
if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
|
|
Serial.println(F("SSD1306 allocation failed"));
|
|
for(;;); // Don't proceed, loop forever
|
|
}
|
|
|
|
// Show initial display buffer contents on the screen --
|
|
// the library initializes this with an Adafruit splash screen.
|
|
display.display();
|
|
|
|
display.clearDisplay();
|
|
display.setTextSize(1);
|
|
display.setTextColor(SSD1306_WHITE);
|
|
display.setCursor(0, 0);
|
|
display.println(F("Init. ESC"));
|
|
display.display(); // Show initial text
|
|
|
|
esc.init();
|
|
|
|
|
|
display.print(F("Init. SD.. ")); display.display();
|
|
|
|
Serial.print("Initializing SD card...");
|
|
// see if the card is present and can be initialized:
|
|
if (!SD.begin(SDCHIPSELECT)) {
|
|
Serial.println("Card failed, or not present");
|
|
display.print(F("Fail!")); display.display();
|
|
datalogging=false; //disable logging
|
|
delay(1000);
|
|
}else{
|
|
Serial.println("Card initialized.");
|
|
display.print(F("OK")); display.display();
|
|
if (datalogging){
|
|
int filenumber=0;
|
|
char buffer[6];
|
|
sprintf(buffer, "%04d", filenumber);
|
|
datalogging_filename="LOG_"+String(buffer)+".TXT";
|
|
while(SD.exists(datalogging_filename) && filenumber<10000) {
|
|
Serial.print(datalogging_filename); Serial.println(" exists");
|
|
filenumber++;
|
|
sprintf(buffer, "%04d", filenumber);
|
|
datalogging_filename="LOG_"+String(buffer)+".TXT";
|
|
|
|
}
|
|
Serial.print(datalogging_filename); Serial.println(" is free");
|
|
}
|
|
}
|
|
|
|
analogReadResolution(12);
|
|
|
|
pinMode(PIN_GAMETRAK_LENGTH_A, INPUT_PULLUP);
|
|
pinMode(PIN_GAMETRAK_LENGTH_B, INPUT_PULLUP);
|
|
pinMode(PIN_GAMETRAK_VERTICAL, INPUT_PULLUP);
|
|
pinMode(PIN_GAMETRAK_HORIZONTAL, INPUT_PULLUP);
|
|
|
|
btn_up.attachClick([]() {
|
|
button_up_click=true;
|
|
});
|
|
btn_down.attachClick([]() {
|
|
button_down_click=true;
|
|
});
|
|
btn_inc.attachClick([]() {
|
|
button_inc_click=true;
|
|
});
|
|
btn_dec.attachClick([]() {
|
|
button_dec_click=true;
|
|
});
|
|
|
|
|
|
|
|
display.print(F("Init. RTC.. ")); display.display();
|
|
|
|
setSyncProvider(getTeensy3Time); //See https://www.pjrc.com/teensy/td_libs_Time.html#teensy3
|
|
if (timeStatus()!= timeSet) {
|
|
Serial.println("Unable to sync with the RTC");
|
|
display.println(F("Fail")); display.display();
|
|
delay(1000);
|
|
} else {
|
|
Serial.println("RTC has set the system time");
|
|
display.println(F("OK")); display.display();
|
|
|
|
}
|
|
|
|
|
|
display.println(F("Init. NRF24")); display.display();
|
|
radio.begin();
|
|
|
|
|
|
Serial.println("RF24 set rate");
|
|
if (!radio.setDataRate( RF24_250KBPS )){ //set to slow data rate. default was 1MBPS
|
|
display.println(F(" Fail Data Rate!")); display.display();
|
|
}
|
|
//radio.setDataRate( RF24_1MBPS );
|
|
//Serial.println("set channel");
|
|
|
|
radio.setChannel(NRF24CHANNEL); //0 to 124 (inclusive)
|
|
|
|
//Serial.println("set retries and payload");
|
|
radio.setRetries(15, 15); // optionally, increase the delay between retries & # of retries
|
|
radio.setPayloadSize(8); // optionally, reduce the payload size. seems to improve reliability
|
|
|
|
//Serial.println("open pipe");
|
|
radio.openWritingPipe(pipes[0]); //write on pipe 0
|
|
radio.openReadingPipe(1, pipes[1]); //read on pipe 1
|
|
|
|
Serial.println("start listening");
|
|
radio.startListening();
|
|
|
|
|
|
|
|
display.println(F("Finished"));
|
|
display.display(); // Show initial text
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
void loop() {
|
|
last_loopmillis=loopmillis;
|
|
loopmillis=millis();
|
|
|
|
|
|
btn_up.tick();
|
|
btn_inc.tick();
|
|
btn_dec.tick();
|
|
btn_down.tick();
|
|
|
|
|
|
|
|
|
|
if (loopmillis - last_adcupdated > ADC_UPDATEPERIOD) { //update analog readings
|
|
|
|
|
|
int raw_length_a=analogRead(PIN_GAMETRAK_LENGTH_A);
|
|
int raw_length_b=analogRead(PIN_GAMETRAK_LENGTH_B);
|
|
|
|
raw_length_maxdiff=max(raw_length_maxdiff,abs(raw_length_a-raw_length_b));
|
|
|
|
int raw_length=(raw_length_a+raw_length_b)/2;
|
|
|
|
|
|
uint16_t gt_length_1 = GT_LENGTH_1_OFFSET+raw_length*GT_LENGTH_1_SCALE;
|
|
uint16_t gt_length_2 = GT_LENGTH_2_OFFSET+raw_length*GT_LENGTH_2_SCALE;
|
|
|
|
double crossovermapping=constrain(((raw_length-GT_LENGTH_CROSSOVERADC)/GT_LENGTH_CROSSOVER_FEATHER )/2.0+0.5, 0.0,1.0); //0 for first, 1 for second
|
|
|
|
|
|
gt_length = constrain( gt_length_1*(1-crossovermapping) + gt_length_2*crossovermapping , 0,GT_LENGTH_MAXLENGTH);
|
|
if (gt_length<=GT_LENGTH_MIN){
|
|
gt_length=0; //if below minimum measurable length set to 0mm
|
|
}
|
|
gt_vertical = constrain(map(analogRead(PIN_GAMETRAK_VERTICAL)-((int16_t)GT_VERTICAL_CENTER), -GT_VERTICAL_RANGE,+GT_VERTICAL_RANGE,-127,127),-127,127); //left negative
|
|
gt_horizontal = constrain(map(analogRead(PIN_GAMETRAK_HORIZONTAL)-((int16_t)GT_HORIZONTAL_CENTER), -GT_HORIZONTAL_RANGE,+GT_HORIZONTAL_RANGE,-127,127),-127,127); //down negative
|
|
|
|
last_adcupdated = millis();
|
|
|
|
|
|
/*
|
|
Serial.print("gt_length=");
|
|
Serial.print(gt_length);
|
|
Serial.print(", gt_vertical=");
|
|
Serial.print(gt_vertical);
|
|
Serial.print(", gt_horizontal=");
|
|
Serial.print(gt_horizontal);
|
|
|
|
|
|
|
|
Serial.print(" pl=");
|
|
Serial.print(raw_length_a);
|
|
Serial.print(", ");
|
|
Serial.print(raw_length_b);
|
|
|
|
Serial.print(", pv=");
|
|
Serial.print(analogRead(PIN_GAMETRAK_VERTICAL));
|
|
Serial.print(", ph=");
|
|
Serial.print(analogRead(PIN_GAMETRAK_HORIZONTAL));
|
|
Serial.print(" Ldiff=");
|
|
Serial.println(abs(raw_length_a-raw_length_b));
|
|
*/
|
|
|
|
|
|
/*
|
|
static int _rawlengtharray[40];
|
|
static int _rawlapos=0;
|
|
_rawlengtharray[_rawlapos++]=raw_length;
|
|
_rawlapos%=40;
|
|
|
|
int rawlengthfilter=0;
|
|
for (int p=0;p<40;p++) {
|
|
rawlengthfilter+=_rawlengtharray[p];
|
|
}
|
|
rawlengthfilter/=40;
|
|
|
|
static int maxldiff=0;
|
|
maxldiff=max(maxldiff,abs(raw_length_a-raw_length_b));
|
|
Serial.print("");
|
|
Serial.print(rawlengthfilter);
|
|
Serial.print(" maxldiff=");
|
|
Serial.println(maxldiff);*/
|
|
|
|
}
|
|
|
|
//NRF24
|
|
nrf_delay = loopmillis - last_nrfreceive; //update nrf delay
|
|
if ( radio.available() )
|
|
{
|
|
//Serial.println("radio available ...");
|
|
|
|
lastpacketOK = false; //initialize with false, if checksum ok gets set to true
|
|
//digitalWrite(PIN_LED, !digitalRead(PIN_LED));
|
|
radio.read( &lastnrfdata, sizeof(nrfdata) );
|
|
|
|
if (lastnrfdata.speed == NRFDATA_CENTER && lastnrfdata.steer == NRFDATA_CENTER) { //arm only when centered
|
|
controlmode = MODE_RADIONRF;//set radionrf mode at first received packet
|
|
}
|
|
|
|
|
|
|
|
|
|
uint8_t calcchecksum = (uint8_t)((lastnrfdata.steer + 3) * (lastnrfdata.speed + 13));
|
|
if (lastnrfdata.checksum == calcchecksum) { //checksum ok?
|
|
lastpacketOK = true;
|
|
last_nrfreceive_delay=loopmillis-last_nrfreceive; //for display purpose
|
|
max_last_nrfreceive_delay=max(last_nrfreceive_delay,max_last_nrfreceive_delay);
|
|
last_nrfreceive = loopmillis;
|
|
|
|
watchdogtimer=loopmillis; //reset watchdog
|
|
|
|
//parse commands
|
|
motorenabled = (lastnrfdata.commands & (1 << 0))>>0; //check bit 0 . Used for safety-off when remote released
|
|
if ((lastnrfdata.commands & (1 << 1))>>0) { //left
|
|
setup_directon_press_left = true;
|
|
Serial.println("RF Button Left");
|
|
}else if ((lastnrfdata.commands & (1 << 2))>>0) { //up
|
|
setup_directon_press_up = true;
|
|
Serial.println("RF Button Up");
|
|
}else if ((lastnrfdata.commands & (1 << 3))>>0) { //down
|
|
setup_directon_press_down = true;
|
|
Serial.println("RF Button Down");
|
|
|
|
}else if ((lastnrfdata.commands & (1 << 4))>>0) { //right
|
|
setup_directon_press_right = true;
|
|
Serial.println("RF Button Right");
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
|
|
if (abs(set_speed)<10 && abs(set_steer)<10) { //standstill
|
|
if (setup_directon_press_left || setup_directon_press_right) {
|
|
//Fast riding mode
|
|
speed_coefficient_nrf=1.0;
|
|
steer_coefficient_nrf=0.3;
|
|
filter_nrf_set_speed=0.015;
|
|
filter_nrf_set_steer=0.08;
|
|
}
|
|
if (setup_directon_press_up) {
|
|
//Remote controlled Aggro
|
|
speed_coefficient_nrf=1.0;
|
|
steer_coefficient_nrf=0.5;
|
|
filter_nrf_set_speed=0.08;
|
|
filter_nrf_set_steer=0.08;
|
|
}
|
|
if (setup_directon_press_down) {
|
|
//Remote controlled Walking
|
|
speed_coefficient_nrf=0.4;
|
|
steer_coefficient_nrf=0.5;
|
|
filter_nrf_set_speed=0.06;
|
|
filter_nrf_set_steer=0.06;
|
|
}
|
|
}
|
|
setup_directon_press_left=false;
|
|
setup_directon_press_up=false;
|
|
setup_directon_press_down=false;
|
|
setup_directon_press_right=false;
|
|
|
|
|
|
|
|
if (controlmode == MODE_RADIONRF && nrf_delay >= MAX_NRFDELAY) { //too long since last sucessful nrf receive
|
|
controlmode = MODE_DISARMED;
|
|
debug_count_disarmedbecauseofnrfdelay++;
|
|
#ifdef DEBUG
|
|
Serial.println("nrf_delay>=MAX_NRFDELAY, disarmed!");
|
|
#endif
|
|
}
|
|
if (controlmode == MODE_RADIONRF) { //is armed in nrf mode
|
|
|
|
|
|
if (lastpacketOK) { //if lastnrfdata is valid
|
|
static unsigned long last_controlupdate_validnrf=0;
|
|
if (loopmillis - last_controlupdate_validnrf > CONTROLUPDATEPERIOD) {
|
|
last_controlupdate_validnrf = loopmillis;
|
|
|
|
//out_speed=(int16_t)( (lastnrfdata.y-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
|
|
//out_steer=(int16_t)( -(lastnrfdata.x-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
|
|
int16_t new_set_speed = (int16_t)( ((int16_t)(lastnrfdata.speed) - NRFDATA_CENTER) * 1000 / 127 ); //-1000 to 1000
|
|
int16_t new_set_steer = (int16_t)( ((int16_t)(lastnrfdata.steer) - NRFDATA_CENTER) * 1000 / 127 ) * -1;
|
|
|
|
if (!motorenabled) { //when safety off, reason: remote relased
|
|
if (abs(new_set_speed) < abs(set_speed)) { //only allow going slower
|
|
set_speed = set_speed*(1.0-filter_nrf_set_speed) + new_set_speed*filter_nrf_set_speed; //simple Filter
|
|
}
|
|
if (abs(new_set_steer) < abs(set_steer)) { //only allow turning slower
|
|
set_steer = set_steer*(1.0-filter_nrf_set_steer) + new_set_steer*filter_nrf_set_steer;
|
|
}
|
|
|
|
}else{
|
|
set_speed = set_speed*(1.0-filter_nrf_set_speed) + new_set_speed*filter_nrf_set_speed; //simple Filter
|
|
set_steer = set_steer*(1.0-filter_nrf_set_steer) + new_set_steer*filter_nrf_set_steer;
|
|
|
|
|
|
//calculate speed l and r from speed and steer
|
|
|
|
int16_t _out_speedl,_out_speedr;
|
|
_out_speedl = constrain(set_speed * speed_coefficient_nrf + set_steer * speed_coefficient_nrf * steer_coefficient_nrf, -1500, 1500);
|
|
_out_speedr = constrain(set_speed * speed_coefficient_nrf - set_steer * speed_coefficient_nrf * steer_coefficient_nrf, -1500, 1500);
|
|
esc.setSpeed(_out_speedl,_out_speedr);
|
|
}
|
|
}
|
|
}//if pastpacket not ok, keep last out_steer and speed values until disarmed
|
|
|
|
|
|
#ifdef DEBUG
|
|
if (!lastpacketOK) {
|
|
Serial.println("Armed but packet not ok");
|
|
}
|
|
#endif
|
|
|
|
}
|
|
|
|
|
|
if (controlmode==MODE_DISARMED) { //check if gametrak can be armed
|
|
if (gt_length>gt_length_set && gt_length<gt_length_set+10) { //is in trackable length
|
|
controlmode=MODE_GAMETRAK; //enable gametrak mode
|
|
Serial.println("Enable Gametrak");
|
|
}
|
|
}else if (controlmode==MODE_GAMETRAK){ //gametrak control active and not remote active
|
|
//Gametrak Control Code
|
|
motorenabled=true;
|
|
watchdogtimer=loopmillis; //reset watchdog
|
|
if (gt_length<=GT_LENGTH_MIN){ //let go
|
|
//Serial.println("gametrak released");
|
|
controlmode=MODE_DISARMED;
|
|
motorenabled=false;
|
|
}
|
|
int16_t _gt_length_diff = gt_length-gt_length_set; //positive if needs to drive forward
|
|
if ((_gt_length_diff>-GT_LENGTH_MINDIFF) && (_gt_length_diff<GT_LENGTH_MINDIFF)){ //minimum difference to drive
|
|
_gt_length_diff=0; //threshold
|
|
}
|
|
|
|
set_steer=constrain((int16_t)(-gt_horizontal*gt_steer_p),-gt_steer_limit,gt_steer_limit); //steer positive is left //gt_horizontal left is negative
|
|
|
|
if (_gt_length_diff>0) { //needs to drive forward
|
|
set_speed = constrain((int16_t)(_gt_length_diff*gt_speed_p),0,gt_speed_limit);
|
|
}else{ //drive backward
|
|
if (_gt_length_diff > GT_LENGTH_MAXIMUMDIFFBACKWARD){ //only drive if not pulled back too much
|
|
set_speed = constrain((int16_t)(_gt_length_diff*gt_speedbackward_p),-gt_backward_speed_limit,0);
|
|
}else{
|
|
set_speed = 0; //stop
|
|
set_steer = 0;
|
|
}
|
|
}
|
|
|
|
static float safetymultiplier=1.0; //value to reduce output speed if gametrack is pointing too far down or up (string might be behind vehicle)
|
|
#define GT_SAFETY_THRESHOLD_H 120 //above which value (abs) safety slowdown should start
|
|
#define GT_SAFETY_THRESHOLD_V 120
|
|
#define SAFETYMULTIPLIER_UPDATE_INTERVAL 100
|
|
#define SAFETYMULTIPLIER_CHANGE_TIME_DOWN 2.0 //Time how long it should take to go from 100% to 0% when in safety slowdown
|
|
#define SAFETYMULTIPLIER_CHANGE_TIME_UP 1.0
|
|
static unsigned long last_safetymultiplier_update=0;
|
|
|
|
|
|
if (loopmillis-last_safetymultiplier_update > SAFETYMULTIPLIER_UPDATE_INTERVAL) {
|
|
if (abs(gt_vertical)>GT_SAFETY_THRESHOLD_V || abs(gt_horizontal)>GT_SAFETY_THRESHOLD_H) {
|
|
safetymultiplier-=1.0/(1000.0/SAFETYMULTIPLIER_UPDATE_INTERVAL)/SAFETYMULTIPLIER_CHANGE_TIME_DOWN;
|
|
}else{
|
|
safetymultiplier+=1.0/(1000.0/SAFETYMULTIPLIER_UPDATE_INTERVAL)/SAFETYMULTIPLIER_CHANGE_TIME_UP;
|
|
}
|
|
safetymultiplier=constrain(safetymultiplier,0.0,1.0);
|
|
last_safetymultiplier_update=loopmillis;
|
|
}
|
|
|
|
//calculate speed l and r from speed and steer
|
|
static unsigned long last_controlupdate_gt=0;
|
|
if (loopmillis - last_controlupdate_gt > CONTROLUPDATEPERIOD) {
|
|
last_controlupdate_gt = loopmillis;
|
|
int16_t _out_speedl,_out_speedr;
|
|
_out_speedl = constrain(set_speed * speed_coefficient_gt + set_steer * speed_coefficient_gt * steer_coefficient_gt, -1000, 1000)*safetymultiplier;
|
|
_out_speedr = constrain(set_speed * speed_coefficient_gt - set_steer * speed_coefficient_gt * steer_coefficient_gt, -1000, 1000)*safetymultiplier;
|
|
esc.setSpeed(_out_speedl,_out_speedr);
|
|
}
|
|
}
|
|
|
|
|
|
if (error > 0) { //disarm if error occured
|
|
controlmode = MODE_DISARMED; //force disarmed
|
|
}
|
|
|
|
|
|
if (controlmode == MODE_DISARMED){ //all disarmed
|
|
#define WATCHDOG_TIMEOUT 2000
|
|
if (millis()-watchdogtimer>WATCHDOG_TIMEOUT) {
|
|
set_speed=0;
|
|
set_steer=0;
|
|
}
|
|
static unsigned long last_controlupdate_disarmed=0;
|
|
if (loopmillis - last_controlupdate_disarmed > CONTROLUPDATEPERIOD) {
|
|
last_controlupdate_disarmed = loopmillis;
|
|
set_speed = set_speed*(1.0-filter_stop_set_speed); //simple Filter
|
|
set_steer = set_steer*(1.0-filter_stop_set_steer);
|
|
int16_t _out_speedl,_out_speedr;
|
|
_out_speedl = constrain(set_speed * speed_coefficient_nrf + set_steer * speed_coefficient_nrf * steer_coefficient_nrf, -1500, 1500);
|
|
_out_speedr = constrain(set_speed * speed_coefficient_nrf - set_steer * speed_coefficient_nrf * steer_coefficient_nrf, -1500, 1500);
|
|
esc.setSpeed(_out_speedl,_out_speedr);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
if (!motorenabled) {//motors disabled. Used for safety off when remote released
|
|
static unsigned long last_controlupdate_motordisabled=0;
|
|
if (loopmillis - last_controlupdate_motordisabled > CONTROLUPDATEPERIOD) {
|
|
last_controlupdate_motordisabled = loopmillis;
|
|
set_speed = set_speed*(1.0-filter_stop_set_speed); //simple Filter
|
|
set_steer = set_steer*(1.0-filter_stop_set_steer);
|
|
int16_t _out_speedl,_out_speedr;
|
|
_out_speedl = constrain(set_speed * speed_coefficient_nrf + set_steer * speed_coefficient_nrf * steer_coefficient_nrf, -1500, 1500);
|
|
_out_speedr = constrain(set_speed * speed_coefficient_nrf - set_steer * speed_coefficient_nrf * steer_coefficient_nrf, -1500, 1500);
|
|
esc.setSpeed(_out_speedl,_out_speedr);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
static unsigned long last_datalogging_write=0;
|
|
static boolean logging_headerWritten=false;
|
|
if (datalogging) {
|
|
#define LOGGINGINTERVAL 100
|
|
if (loopmillis-last_datalogging_write>LOGGINGINTERVAL)
|
|
{
|
|
last_datalogging_write=loopmillis;
|
|
|
|
File dataFile = SD.open(datalogging_filename, FILE_WRITE);
|
|
|
|
if (dataFile) { // if the file is available, write to it
|
|
if (!logging_headerWritten) {
|
|
dataFile.print("time,cmd_L,cmd_R,");
|
|
dataFile.print("current_L,current_R,");
|
|
dataFile.print("rpm_L,rpm_R,");
|
|
dataFile.print("temp,vbat,");
|
|
dataFile.println("trip,currentConsumed,motorenabled,disarmedByDelay");
|
|
dataFile.print("#TIMESTAMP:"); dataFile.println(now());
|
|
logging_headerWritten=true;
|
|
}
|
|
dataFile.print(String(loopmillis)); dataFile.print(";");
|
|
dataFile.print(esc.getCmdL()); dataFile.print(";");
|
|
dataFile.print(esc.getCmdR()); dataFile.print(";");
|
|
dataFile.print(esc.getFiltered_curL(),3); dataFile.print(";");
|
|
dataFile.print(esc.getFiltered_curR(),3); dataFile.print(";");
|
|
dataFile.print(esc.getFeedback_speedL_meas()); dataFile.print(";");
|
|
dataFile.print(esc.getFeedback_speedR_meas()); dataFile.print(";");
|
|
dataFile.print(esc.getFeedback_boardTemp()); dataFile.print(";");
|
|
dataFile.print(esc.getFeedback_batVoltage()); dataFile.print(";");
|
|
dataFile.print(esc.getTrip()); dataFile.print(";");
|
|
dataFile.print(esc.getCurrentConsumed(),3); dataFile.print(";");
|
|
dataFile.print(motorenabled); dataFile.print(";");
|
|
dataFile.print(debug_count_disarmedbecauseofnrfdelay);
|
|
dataFile.println("");
|
|
dataFile.close();
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
esc.update(loopmillis);
|
|
updateInputs(loopmillis);
|
|
updateDisplay(loopmillis);
|
|
}
|
|
|
|
void updateInputs(unsigned long loopmillis) {
|
|
switch (displaymode) {
|
|
case DISPLAY_STATS: case DISPLAY_STATS2: case DISPLAY_STATS3:
|
|
if (button_up_click) { //first button opens menu
|
|
displaymode=DISPLAY_MENU; // go to menu
|
|
}
|
|
if (button_inc_click) {
|
|
displaymode=DISPLAY_STATS;
|
|
}
|
|
if (button_dec_click) {
|
|
displaymode=DISPLAY_STATS2;
|
|
}
|
|
if (button_down_click) {
|
|
displaymode=DISPLAY_STATS3;
|
|
max_last_nrfreceive_delay=0; //reset when switching to this display
|
|
}
|
|
break;
|
|
case DISPLAY_MENU:
|
|
if (button_up_click) {
|
|
if (menu_entrypos>0) {
|
|
menu_entrypos--;
|
|
}else{
|
|
displaymode=DISPLAY_STATS; //when at top entry click up again to go back to exit menu
|
|
}
|
|
}
|
|
if (button_down_click) {
|
|
if (menu_entrypos< (MENU_ENTRIES-1)) {
|
|
menu_entrypos++;
|
|
}
|
|
}
|
|
|
|
|
|
if (menu_pagepos==0) {
|
|
switch(menu_entrypos) {
|
|
case 0: //Change pages
|
|
if (button_inc_click) {
|
|
if (menu_pagepos<MENU_PAGES-1) {
|
|
menu_pagepos++;
|
|
}
|
|
}
|
|
if (button_dec_click) {
|
|
if (menu_pagepos>0) {
|
|
menu_pagepos--;
|
|
}
|
|
}
|
|
break;
|
|
case 1: //Filter NRF Speed
|
|
if (button_inc_click) {
|
|
filter_nrf_set_speed+=0.005;
|
|
filter_nrf_set_speed = constrain(filter_nrf_set_speed, 0.01, 1.0);
|
|
}
|
|
if (button_dec_click) {
|
|
filter_nrf_set_speed-=0.005;
|
|
filter_nrf_set_speed = constrain(filter_nrf_set_speed, 0.01, 1.0);
|
|
}
|
|
break;
|
|
case 2: //Filter NRF Steer
|
|
if (button_inc_click) {
|
|
filter_nrf_set_steer+=0.01;
|
|
filter_nrf_set_steer = constrain(filter_nrf_set_steer, 0.01, 1.0);
|
|
}
|
|
if (button_dec_click) {
|
|
filter_nrf_set_steer-=0.01;
|
|
filter_nrf_set_steer = constrain(filter_nrf_set_steer, 0.01, 1.0);
|
|
}
|
|
break;
|
|
case 3:
|
|
if (button_inc_click) {
|
|
gt_speed_p+=0.05;
|
|
gt_speed_p = constrain(gt_speed_p, 0.05, 5.0);
|
|
}
|
|
if (button_dec_click) {
|
|
gt_speed_p-=0.05;
|
|
gt_speed_p = constrain(gt_speed_p, 0.05, 5.0);
|
|
}
|
|
break;
|
|
case 4:
|
|
if (button_inc_click) {
|
|
gt_steer_p+=0.05;
|
|
gt_steer_p = constrain(gt_steer_p, 0.05, 5.0);
|
|
}
|
|
if (button_dec_click) {
|
|
gt_steer_p-=0.05;
|
|
gt_steer_p = constrain(gt_steer_p, 0.05, 5.0);
|
|
}
|
|
break;
|
|
case 5:
|
|
if (button_inc_click) {
|
|
gt_speed_limit+=50;
|
|
gt_speed_limit = constrain(gt_speed_limit, 50, 1000);
|
|
}
|
|
if (button_dec_click) {
|
|
gt_speed_limit-=50;
|
|
gt_speed_limit = constrain(gt_speed_limit, 50, 1000);
|
|
}
|
|
break;
|
|
case 6:
|
|
if (button_inc_click) {
|
|
gt_steer_limit+=50;
|
|
gt_steer_limit = constrain(gt_steer_limit, 50, 1000);
|
|
}
|
|
if (button_dec_click) {
|
|
gt_steer_limit-=50;
|
|
gt_steer_limit = constrain(gt_steer_limit, 50, 1000);
|
|
}
|
|
break;
|
|
case 7:
|
|
if (button_inc_click) {
|
|
esc.resetStatistics();
|
|
}
|
|
break;
|
|
}
|
|
|
|
}else if(menu_pagepos==1) {
|
|
switch(menu_entrypos) {
|
|
case 0: //Change Pages
|
|
if (button_inc_click) {
|
|
if (menu_pagepos<MENU_PAGES-1) {
|
|
menu_pagepos++;
|
|
}
|
|
}
|
|
if (button_dec_click) {
|
|
if (menu_pagepos>0) {
|
|
menu_pagepos--;
|
|
}
|
|
}
|
|
break;
|
|
case 1: // speed_coefficient_nrf
|
|
if (button_inc_click) {
|
|
speed_coefficient_nrf+=0.05;
|
|
speed_coefficient_nrf = constrain(speed_coefficient_nrf, 0.05, 1.0);
|
|
}
|
|
if (button_dec_click) {
|
|
speed_coefficient_nrf-=0.05;
|
|
speed_coefficient_nrf = constrain(speed_coefficient_nrf, 0.05, 1.0);
|
|
}
|
|
break;
|
|
case 2: // steer_coefficient_nrf
|
|
if (button_inc_click) {
|
|
steer_coefficient_nrf+=0.05;
|
|
steer_coefficient_nrf = constrain(steer_coefficient_nrf, 0.05, 1.0);
|
|
}
|
|
if (button_dec_click) {
|
|
steer_coefficient_nrf-=0.05;
|
|
steer_coefficient_nrf = constrain(steer_coefficient_nrf, 0.05, 1.0);
|
|
}
|
|
break;
|
|
}
|
|
|
|
}else if(menu_pagepos==2) {
|
|
switch(menu_entrypos) {
|
|
case 0: //Change Pages
|
|
if (button_inc_click) {
|
|
if (menu_pagepos<MENU_PAGES-1) {
|
|
menu_pagepos++;
|
|
}
|
|
}
|
|
if (button_dec_click) {
|
|
if (menu_pagepos>0) {
|
|
menu_pagepos--;
|
|
}
|
|
}
|
|
break;
|
|
case 1: // speed_coefficient_gt
|
|
if (button_inc_click) {
|
|
speed_coefficient_gt+=0.05;
|
|
speed_coefficient_gt = constrain(speed_coefficient_gt, 0.05, 1.0);
|
|
}
|
|
if (button_dec_click) {
|
|
speed_coefficient_gt-=0.05;
|
|
speed_coefficient_gt = constrain(speed_coefficient_gt, 0.05, 1.0);
|
|
}
|
|
break;
|
|
case 2: // steer_coefficient_gt
|
|
if (button_inc_click) {
|
|
steer_coefficient_gt+=0.05;
|
|
steer_coefficient_gt = constrain(steer_coefficient_gt, 0.05, 1.0);
|
|
}
|
|
if (button_dec_click) {
|
|
steer_coefficient_gt-=0.05;
|
|
steer_coefficient_gt = constrain(steer_coefficient_gt, 0.05, 1.0);
|
|
}
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
break;
|
|
|
|
}
|
|
|
|
if (button_up_click || button_down_click || button_inc_click || button_dec_click) { //any button was pressed
|
|
flag_updatedisplay=true;
|
|
button_up_click=false;
|
|
button_down_click=false;
|
|
button_inc_click=false;
|
|
button_dec_click=false;
|
|
}
|
|
|
|
}
|
|
|
|
void updateDisplay(unsigned long loopmillis)
|
|
{
|
|
static unsigned long last_updatedisplay=0;
|
|
if (loopmillis-last_updatedisplay>DISPLAYUPDATE_INTERVAL || flag_updatedisplay) {
|
|
flag_updatedisplay=false;
|
|
|
|
|
|
if (displaymode==DISPLAY_STATS) {
|
|
display_show_stats();
|
|
}else if(displaymode==DISPLAY_STATS2) {
|
|
display_show_stats2();
|
|
}else if(displaymode==DISPLAY_STATS3) {
|
|
display_show_stats3();
|
|
}else if(displaymode==DISPLAY_MENU) {
|
|
display_show_menu();
|
|
}
|
|
|
|
last_updatedisplay=loopmillis;
|
|
}
|
|
}
|
|
|
|
void display_show_stats() {
|
|
display.clearDisplay();
|
|
display.setTextSize(1);
|
|
display.setTextColor(SSD1306_WHITE);
|
|
display.setCursor(0, 0);
|
|
display.print(F("MODE="));
|
|
|
|
switch(controlmode) {
|
|
case MODE_DISARMED:
|
|
display.println(F("DISARMED"));
|
|
break;
|
|
case MODE_RADIONRF:
|
|
display.println(F("RADIONRF"));
|
|
break;
|
|
case MODE_GAMETRAK:
|
|
display.println(F("GAMETRAK"));
|
|
break;
|
|
default:
|
|
display.println(F("UNDEF"));
|
|
break;
|
|
}
|
|
|
|
|
|
//updates only when display active
|
|
/* static float maxcurL=0;
|
|
static float maxcurR=0;
|
|
maxcurL=max(maxcurL,esc.getFiltered_curL());
|
|
maxcurR=max(maxcurR,esc.getFiltered_curR());
|
|
|
|
static float mincurL=0;
|
|
static float mincurR=0;
|
|
mincurL=min(mincurL,esc.getFiltered_curL());
|
|
mincurR=min(mincurR,esc.getFiltered_curR());*/
|
|
|
|
|
|
display.print(F("Bat=")); display.print(esc.getFeedback_batVoltage()); display.print(F(" min=")); display.println(esc.getMinBatVoltage());
|
|
display.print(F("Temp=")); display.print(esc.getFeedback_boardTemp()); display.print(F(" max=")); display.println(esc.getMaxBoardTemp());
|
|
|
|
display.print(F("DC max=")); display.print(esc.getMaxcurL(),1); display.print(F("/")); display.println(esc.getMaxcurR(),1); // display.print(F(" min=")); display.print(mincurL,1); display.print(F("/")); display.println(mincurR,1);
|
|
display.print(F("trip=")); display.print(esc.getTrip(),0); display.print(F(", ")); display.print(esc.getCurrentConsumed(),3); display.println(F("Ah"));
|
|
display.print(F("eff.=")); display.print(esc.getTrip()/esc.getCurrentConsumed(),0); display.println(F("m/Ah"));
|
|
|
|
display.print(F("RTC=")); display.print(getCurrentDatestring()); display.println(F(""));
|
|
|
|
if (datalogging) {
|
|
display.print(datalogging_filename); display.print(F(" ")); display.print(loopmillis/1000); display.println(F("s"));
|
|
}else{
|
|
display.print("No SD"); display.println(F(""));
|
|
}
|
|
|
|
|
|
display.display(); // Show initial text
|
|
}
|
|
|
|
void display_show_stats2() {
|
|
display.clearDisplay();
|
|
display.setTextSize(1);
|
|
display.setTextColor(SSD1306_WHITE);
|
|
display.setCursor(0, 0);
|
|
|
|
display.print(F("CMD=")); display.print(esc.getCmdL()); display.print(F(", ")); display.println(esc.getCmdR());
|
|
display.print(F("FBC=")); display.print(esc.getFeedback_cmd1()); display.print(F(", ")); display.println(esc.getFeedback_cmd2());
|
|
display.print(F("Speed=")); display.print(esc.getFeedback_speedL_meas()); display.print(F(",")); display.print(esc.getFeedback_speedR_meas()); display.print(F("=")); display.print(esc.getMeanSpeed()); display.println(F("ms"));
|
|
display.print(F("Length=")); display.println(gt_length);
|
|
display.print(F("H=")); display.print(gt_horizontal); display.print(F(" V=")); display.println(gt_vertical);
|
|
display.print(F("CMD=")); display.print(esc.getCmdL()); display.print(F(", ")); display.println(esc.getCmdR());
|
|
|
|
display.display(); // Show initial text
|
|
}
|
|
|
|
|
|
void display_show_stats3() {
|
|
display.clearDisplay();
|
|
display.setTextSize(1);
|
|
display.setTextColor(SSD1306_WHITE);
|
|
display.setCursor(0, 0);
|
|
|
|
display.print(F("looptime=")); display.println(loopmillis-last_loopmillis);
|
|
display.print(F("feedback=")); display.println(esc.getFeedbackInterval());
|
|
|
|
display.print(F("nrf_delay=")); display.print(last_nrfreceive_delay); display.print(F(" (")); display.print(max_last_nrfreceive_delay);display.println(F(")"));
|
|
display.print(F("maxdiff=")); display.println(raw_length_maxdiff);
|
|
|
|
display.print(F("nrfdata x,y=")); display.print(lastnrfdata.steer); display.print(F(", ")); display.println(lastnrfdata.speed);
|
|
display.print(F("nrfdata command=")); display.println(lastnrfdata.commands);
|
|
|
|
display.print(F("disarmed delay#=")); display.println(debug_count_disarmedbecauseofnrfdelay);
|
|
|
|
|
|
|
|
|
|
display.display(); // Show initial text
|
|
}
|
|
|
|
|
|
void display_show_menu() {
|
|
display.clearDisplay();
|
|
display.setTextSize(1);
|
|
display.setTextColor(SSD1306_WHITE);
|
|
display.setCursor(0, 0);
|
|
for (uint8_t i=0;i<MENU_ENTRIES;i++) {
|
|
if (i==menu_entrypos) {
|
|
display.print(F(">"));
|
|
}else{
|
|
display.print(F(" "));
|
|
}
|
|
if(menu_pagepos==0) {
|
|
switch(i) {
|
|
case 0:
|
|
//display.print(F("EXIT")); display.println();
|
|
display.print(F("# Page 0 / ")); display.print(MENU_PAGES-1); display.print(F(" #")); display.println();
|
|
break;
|
|
case 1:
|
|
display.print(F("Fltr spd=")); display.println(filter_nrf_set_speed,3);
|
|
break;
|
|
case 2:
|
|
display.print(F("Fltr str=")); display.println(filter_nrf_set_steer,3);
|
|
break;
|
|
case 3:
|
|
display.print(F("gt_speed_p=")); display.println(gt_speed_p);
|
|
break;
|
|
case 4:
|
|
display.print(F("gt_steer_p=")); display.println(gt_steer_p);
|
|
break;
|
|
case 5:
|
|
display.print(F("gt_speed_limit=")); display.println(gt_speed_limit);
|
|
break;
|
|
case 6:
|
|
display.print(F("gt_steer_limit=")); display.println(gt_steer_limit);
|
|
break;
|
|
case 7:
|
|
if ((loopmillis/1000)%2==0) {
|
|
display.print(F("Inc to reset stats"));
|
|
}else{
|
|
display.print(F("t=")); display.println(esc.getTripTime(loopmillis)/1000); display.println(F("s"));
|
|
}
|
|
break;
|
|
}
|
|
}else if(menu_pagepos==1) {
|
|
switch(i) {
|
|
case 0:
|
|
//display.print(F("EXIT")); display.println();
|
|
display.print(F("# Page 1 / ")); display.print(MENU_PAGES-1); display.print(F(" NRF #")); display.println();
|
|
break;
|
|
case 1:
|
|
display.print(F("speed_coeff_nrf=")); display.println(speed_coefficient_nrf,2);
|
|
break;
|
|
case 2:
|
|
display.print(F("steer_coeff_nrf=")); display.println(steer_coefficient_nrf,2);
|
|
break;
|
|
case 3:
|
|
|
|
break;
|
|
case 4:
|
|
|
|
break;
|
|
case 5:
|
|
|
|
break;
|
|
case 6:
|
|
|
|
break;
|
|
case 7:
|
|
|
|
break;
|
|
}
|
|
}else if(menu_pagepos==2) {
|
|
switch(i) {
|
|
case 0:
|
|
//display.print(F("EXIT")); display.println();
|
|
display.print(F("# Page 2 / ")); display.print(MENU_PAGES-1); display.print(F(" GT #")); display.println();
|
|
break;
|
|
case 1:
|
|
display.print(F("speed_coeff_gt=")); display.println(speed_coefficient_gt,2);
|
|
break;
|
|
case 2:
|
|
display.print(F("steer_coeff_gt=")); display.println(steer_coefficient_gt,2);
|
|
break;
|
|
case 3:
|
|
|
|
break;
|
|
case 4:
|
|
|
|
break;
|
|
case 5:
|
|
|
|
break;
|
|
case 6:
|
|
|
|
break;
|
|
case 7:
|
|
|
|
break;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
display.display(); // Show initial text
|
|
}
|
|
|
|
|
|
time_t getTeensy3Time()
|
|
{
|
|
return Teensy3Clock.get();
|
|
}
|
|
|
|
String addLeadingZero(int number) {
|
|
if (number<10) {
|
|
return "0"+String(number);
|
|
}
|
|
return String(number);
|
|
}
|
|
|
|
String getCurrentDatestring() {
|
|
return addLeadingZero(year())+addLeadingZero(month())+addLeadingZero(day())+"-"+addLeadingZero(hour())+addLeadingZero(minute())+addLeadingZero(second());
|
|
}
|
|
|