oled and nrf with teensy working
This commit is contained in:
parent
aa8570af42
commit
52cc7bfa1a
Binary file not shown.
|
|
@ -13,6 +13,7 @@ platform = teensy
|
|||
board = teensy31
|
||||
framework = arduino
|
||||
|
||||
upload_protocol = teensy-cli
|
||||
|
||||
monitor_speed = 115200
|
||||
|
||||
|
|
@ -22,3 +23,6 @@ build_flags =
|
|||
|
||||
lib_deps =
|
||||
https://github.com/maniacbug/RF24
|
||||
adafruit/Adafruit SSD1306@^2.5.3
|
||||
adafruit/Adafruit BusIO@^1.11.3
|
||||
https://github.com/adafruit/Adafruit-GFX-Library
|
||||
|
|
@ -1,6 +1,18 @@
|
|||
#include <Arduino.h>
|
||||
#define SERIAL_BAUD 115200 // [-] Baud rate for built-in Serial (used for the Serial Monitor)
|
||||
|
||||
#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
|
||||
|
||||
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,37 +150,66 @@ 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);
|
||||
|
||||
|
||||
|
||||
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));
|
||||
*/
|
||||
|
||||
|
||||
/*
|
||||
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));*/
|
||||
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_set+10) { //is in trackable length
|
||||
controlmode=MODE_GAMETRAK; //enable gametrak mode
|
||||
Serial1.println("Enable Gametrak");
|
||||
Serial.println("Enable Gametrak");
|
||||
}
|
||||
}else if (controlmode==MODE_GAMETRAK){ //gametrak control active and not remote active
|
||||
//Gametrak Control Code
|
||||
motorenabled=true;
|
||||
if (gt_length<=GT_LENGTH_MIN){ //let go
|
||||
Serial1.println("gametrak released");
|
||||
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
|
||||
if ((_gt_length_diff>-GT_LENGTH_MINDIFF) && (_gt_length_diff<GT_LENGTH_MINDIFF)){ //minimum difference to drive
|
||||
_gt_length_diff=0; //threshold
|
||||
}
|
||||
|
||||
|
|
@ -311,21 +429,21 @@ void loop() {
|
|||
if (!motorenabled) {//motors disabled
|
||||
esc.setSpeed(0,0);
|
||||
}
|
||||
last_send = millis();
|
||||
last_send = loopmillis;
|
||||
|
||||
|
||||
#ifdef DEBUG
|
||||
Serial1.print(" out_speedl=");
|
||||
Serial1.print(out_speedl);
|
||||
Serial1.print(" out_speedr=");
|
||||
Serial1.print(out_speedr);
|
||||
Serial1.print(" checksum=");
|
||||
Serial1.print(out_checksum);
|
||||
Serial.print(" out_speedl=");
|
||||
Serial.print(out_speedl);
|
||||
Serial.print(" out_speedr=");
|
||||
Serial.print(out_speedr);
|
||||
Serial.print(" checksum=");
|
||||
Serial.print(out_checksum);
|
||||
|
||||
Serial1.print(" controlmode=");
|
||||
Serial1.print(controlmode);
|
||||
Serial.print(" controlmode=");
|
||||
Serial.print(controlmode);
|
||||
|
||||
Serial1.println();
|
||||
Serial.println();
|
||||
|
||||
#endif
|
||||
}
|
||||
|
|
@ -333,4 +451,38 @@ void loop() {
|
|||
|
||||
|
||||
esc.update(loopmillis);
|
||||
updateDisplay(loopmillis);
|
||||
}
|
||||
|
||||
void updateDisplay(unsigned long loopmillis)
|
||||
{
|
||||
static unsigned long last_updatedisplay=0;
|
||||
if (loopmillis-last_updatedisplay>DISPLAYUPDATE_INTERVAL) {
|
||||
display.clearDisplay();
|
||||
display.setTextSize(1);
|
||||
display.setTextColor(SSD1306_WHITE);
|
||||
display.setCursor(1, 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;
|
||||
}
|
||||
display.print(F("nrf_delay=")); display.println(last_nrfreceive_delay);
|
||||
display.print(F("gt_length=")); display.println(gt_length);
|
||||
display.print(F("maxdiff=")); display.println(raw_length_maxdiff);
|
||||
|
||||
display.display(); // Show initial text
|
||||
last_updatedisplay=loopmillis;
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue