#include #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 //for teensy rtc time_t getTeensy3Time(); #include #include #include #include #include //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 #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 //SCK=13, MISO=12, MOSI=11 #include //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_MINDIFF) && (_gt_length_diff0) { //needs to drive forward set_speed = constrain((int16_t)(_gt_length_diff*gt_speed_p),0,gt_speed_limit); }else{ //drive backward if (_gt_length_diff > GT_LENGTH_MAXIMUMDIFFBACKWARD){ //only drive if not pulled back too much set_speed = constrain((int16_t)(_gt_length_diff*gt_speedbackward_p),-gt_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_pagepos0) { 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_pagepos0) { 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_pagepos0) { 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")); }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()); }