diff --git a/hoverbrettctrl/length_calibration_20220410.ods b/hoverbrettctrl/length_calibration_20220410.ods new file mode 100644 index 0000000..52e7a52 Binary files /dev/null and b/hoverbrettctrl/length_calibration_20220410.ods differ diff --git a/hoverbrettctrl/platformio.ini b/hoverbrettctrl/platformio.ini index 7befa2a..768a858 100644 --- a/hoverbrettctrl/platformio.ini +++ b/hoverbrettctrl/platformio.ini @@ -13,6 +13,7 @@ platform = teensy board = teensy31 framework = arduino +upload_protocol = teensy-cli monitor_speed = 115200 @@ -21,4 +22,7 @@ build_flags = lib_deps = - https://github.com/maniacbug/RF24 \ No newline at end of file + https://github.com/maniacbug/RF24 + adafruit/Adafruit SSD1306@^2.5.3 + adafruit/Adafruit BusIO@^1.11.3 + https://github.com/adafruit/Adafruit-GFX-Library \ No newline at end of file diff --git a/hoverbrettctrl/src/main.cpp b/hoverbrettctrl/src/main.cpp index 1c37394..3b0d23d 100644 --- a/hoverbrettctrl/src/main.cpp +++ b/hoverbrettctrl/src/main.cpp @@ -1,6 +1,18 @@ #include #define SERIAL_BAUD 115200 // [-] Baud rate for built-in Serial (used for the Serial Monitor) +#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 + uint8_t error = 0; #define IMU_NO_CHANGE 2 //IMU values did not change for too long @@ -24,12 +36,45 @@ long last_adcupdated=0; #define CONTROLUPDATEPERIOD 10 long last_controlupdate = 0; -#define GT_LENGTH_OFFSET 4090 //adc offset value (rolled up value) -#define GT_LENGTH_MIN 220 //length in mm at which adc values start to change -#define GT_LENGTH_SCALE -0.73 //(offset-adcvalue)*scale = length[mm] (+length_min) -//2720 at 1000mm+220mm -> 1370 for 1000mm -> -#define GT_LENGTH_MAXLENGTH 2500 //maximum length in [mm]. maximum string length is around 2m80 +#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) @@ -73,7 +118,8 @@ struct nrfdata { nrfdata lastnrfdata; long last_nrfreceive = 0; //last time values were received and checksum ok -long nrf_delay = 0; +unsigned long nrf_delay = 0; +unsigned long last_nrfreceive_delay=0; #define MAX_NRFDELAY 100 //ms. maximum time delay at which vehicle will disarm boolean radiosendOk=false; @@ -104,36 +150,65 @@ uint8_t controlmode=0; #define MODE_GAMETRAK 2 +void updateDisplay(unsigned long loopmillis); + + void setup() { Serial.begin(SERIAL_BAUD); //Debug and Program esc.init(); + analogReadResolution(12); - pinMode(PIN_GAMETRAK_LENGTH_A, INPUT); - pinMode(PIN_GAMETRAK_LENGTH_B, INPUT); - pinMode(PIN_GAMETRAK_VERTICAL, INPUT); - pinMode(PIN_GAMETRAK_HORIZONTAL, INPUT); + 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); + + 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(10, 0); + display.println(F("Radio Init")); + display.display(); // Show initial text radio.begin(); - //Serial1.println("set rate"); + Serial.println("RF24 set rate"); radio.setDataRate( RF24_250KBPS ); //set to slow data rate. default was 1MBPS //radio.setDataRate( RF24_1MBPS ); - //Serial1.println("set channel"); + //Serial.println("set channel"); radio.setChannel(NRF24CHANNEL); //0 to 124 (inclusive) - //Serial1.println("set retries and payload"); + //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 - //Serial1.println("open pipe"); + //Serial.println("open pipe"); radio.openWritingPipe(pipes[0]); //write on pipe 0 radio.openReadingPipe(1, pipes[1]); //read on pipe 1 - //Serial1.println("start listening"); + Serial.println("start listening"); radio.startListening(); + + display.clearDisplay(); + display.setTextSize(2); // Draw 2X-scale text + display.setTextColor(SSD1306_WHITE); + display.setCursor(10, 0); + display.println(F("Started")); + display.display(); // Show initial text } @@ -145,41 +220,82 @@ void loop() { - if (millis() - last_adcupdated > ADC_UPDATEPERIOD) { //update analog readings + if (loopmillis - last_adcupdated > ADC_UPDATEPERIOD) { //update analog readings - gt_length = constrain(( analogRead(PIN_GAMETRAK_LENGTH_A))*GT_LENGTH_SCALE - (GT_LENGTH_SCALE*GT_LENGTH_OFFSET) +GT_LENGTH_MIN, 0,GT_LENGTH_MAXLENGTH); + 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 + 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(); /* - Serial1.print("gt_length="); - Serial1.print(gt_length); - Serial1.print(", gt_vertical="); - Serial1.print(gt_vertical); - Serial1.print(", gt_horizontal="); - Serial1.println(gt_horizontal);*/ + Serial.print("gt_length="); + Serial.print(gt_length); + Serial.print(", gt_vertical="); + Serial.print(gt_vertical); + Serial.print(", gt_horizontal="); + Serial.print(gt_horizontal); + - /* - Serial1.print("PIN_GAMETRAK_LENGTH="); - Serial1.print(analogRead(PIN_GAMETRAK_LENGTH)); - Serial1.print(", PIN_GAMETRAK_VERTICAL="); - Serial1.print(analogRead(PIN_GAMETRAK_VERTICAL)); - Serial1.print(", PIN_GAMETRAK_HORIZONTAL="); - Serial1.println(analogRead(PIN_GAMETRAK_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 = millis() - last_nrfreceive; //update nrf delay + nrf_delay = loopmillis - last_nrfreceive; //update nrf delay if ( radio.available() ) { - //Serial1.println("radio available ..."); + //Serial.println("radio available ..."); lastpacketOK = false; //initialize with false, if checksum ok gets set to true //digitalWrite(PIN_LED, !digitalRead(PIN_LED)); @@ -195,7 +311,9 @@ void loop() { uint8_t calcchecksum = (uint8_t)((lastnrfdata.steer + 3) * (lastnrfdata.speed + 13)); if (lastnrfdata.checksum == calcchecksum) { //checksum ok? lastpacketOK = true; - last_nrfreceive = millis(); + last_nrfreceive_delay=loopmillis-last_nrfreceive; //for display purpose + last_nrfreceive = loopmillis; + //parse commands motorenabled = (lastnrfdata.commands & (1 << 0))>>0; //check bit 0 @@ -207,15 +325,15 @@ void loop() { if (controlmode == MODE_RADIONRF && nrf_delay >= MAX_NRFDELAY) { //too long since last sucessful nrf receive controlmode = MODE_DISARMED; #ifdef DEBUG - Serial1.println("nrf_delay>=MAX_NRFDELAY, disarmed!"); + Serial.println("nrf_delay>=MAX_NRFDELAY, disarmed!"); #endif } if (controlmode == MODE_RADIONRF) { //is armed in nrf mode if (lastpacketOK) { //if lastnrfdata is valid - if (millis() - last_controlupdate > CONTROLUPDATEPERIOD) { - last_controlupdate = millis(); + if (loopmillis - last_controlupdate > CONTROLUPDATEPERIOD) { + last_controlupdate = loopmillis; //out_speed=(int16_t)( (lastnrfdata.y-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX ); //out_steer=(int16_t)( -(lastnrfdata.x-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX ); @@ -237,7 +355,7 @@ void loop() { #ifdef DEBUG if (!lastpacketOK) { - Serial1.println("Armed but packet not ok"); + Serial.println("Armed but packet not ok"); } #endif @@ -247,18 +365,18 @@ void loop() { if (controlmode==MODE_DISARMED) { //check if gametrak can be armed if (gt_length>gt_length_set && gt_length-GT_LENGTH_MINDIFF & _gt_length_diff-GT_LENGTH_MINDIFF) && (_gt_length_diff