Teensy 3.2 code conversion to 4.0

Jortsa22420 · Dec 6, 2023

Hello! I would need some suggestions regarding my project. I have code made for Teensy 3.2. It reads data from AEM Canbus and displays it on the screen. The code works with Teensy 3.2 and compiles fine with 4.0 Teensyduino. What should be changed to make it work with 4.0, since acquiring 3.2 Teensy is almost impossible?

Code is posted below

Code:

#include <mcp_can.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Fonts/FreeMonoOblique18pt7b.h>



// <--- Begin Display Defintions

#define TCAADDR 0x70
#define OLED_RESET -1
#define SSD1306_128_32

Adafruit_SSD1306 display(OLED_RESET);

// End Display Definitions --->

// <--- Begin Display Math for Gauges

int i = 0;
float rawval = 0;                        // Setup raw sensor value
int kpaval = 0;                          // Setup kPa value
int boost = 0;                           // Setup boost value
int barboost = 0;                        // Setup value for boost bar
int vac = 0;                             // Setup vacuum value
int peak = 0;                            // Setup peak value
float lambda1_raw = 0 ;                  // Raw Lambda Sensor Value 1
int lambda1 = 0 ;                        // Lambda
int afr1 = 0 ;                           // AFR
float lambda2_raw = 0 ;                  // Raw Lambda Sensor Value 2
int lambda2 = 0 ;                        // Lambda
int afr2 = 0 ;                           // AFR
float e85_raw = 0 ;                      // E85 Raw
int e85 = 0 ;                            // E85 Percentage

// End Display Math for Gauges --->

// <--- Begin Button Setup

uint8_t gauge_switch = 24;                // the number of the pushbutton pin
uint8_t peak_switch = 23;                 // the number of the pushbutton pin

volatile int gaugeselect = 0;            // Set a rolling variable to select screen display

// End Button Setup --->

// <--- Begin Warning Alert Setup

static int injdutywarning = 90;          // Warning Level for Injector Duty Cycle (%)
static int injpwwarning = 22;            // Warning Level for Injector Pulsewidth (ms)

// End Warning Alert Setup --->

// <--- Begin Boot Logo Display Definitions

unsigned long ford_logo_time = 2500;     // milliseconds for Ford logo at boot
unsigned long stang_logo_time = 5000;    // milliseconds for Mustang logo at boot
int boot_complete = 0;                   // Have we done the boot logos?

// <--- 'Ford_Logo_jpg'
const unsigned char Ford_Logo [] PROGMEM = {
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0xff, 0xff, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x3f, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xfe, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x07, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xe0, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x7f, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xfe, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x03, 0xf8, 0x00, 0x0f, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1f, 0xc0, 0x00, 0x00,
  0x00, 0x00, 0x1f, 0x80, 0x00, 0xff, 0xff, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf8, 0x00, 0x00,
  0x00, 0x00, 0x7c, 0x00, 0x03, 0xe0, 0x07, 0xff, 0xc0, 0x01, 0xe0, 0x00, 0x00, 0x3e, 0x00, 0x00,
  0x00, 0x01, 0xe0, 0x00, 0x07, 0x80, 0x00, 0x7f, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x07, 0xc0, 0x00,
  0x00, 0x07, 0x80, 0x00, 0x0e, 0x00, 0x01, 0xc3, 0xff, 0xff, 0xc0, 0x00, 0x00, 0x00, 0xf0, 0x00,
  0x00, 0x1c, 0x00, 0x00, 0x1c, 0x00, 0x03, 0x80, 0x3f, 0xff, 0x00, 0x00, 0x00, 0x00, 0x38, 0x00,
  0x00, 0x78, 0x00, 0x00, 0x3c, 0x04, 0x07, 0x00, 0x03, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00,
  0x00, 0xe0, 0x00, 0x00, 0x3c, 0x0c, 0x0e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0x00,
  0x01, 0x80, 0x00, 0x00, 0x3c, 0x18, 0x1c, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xc0,
  0x03, 0x00, 0x00, 0x00, 0x3e, 0x78, 0x38, 0xd0, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x00, 0x00, 0xe0,
  0x06, 0x00, 0x00, 0x00, 0x3f, 0xf0, 0x70, 0x90, 0x00, 0x00, 0x00, 0x00, 0x38, 0x00, 0x00, 0x60,
  0x0c, 0x00, 0x00, 0x00, 0x1f, 0xe0, 0x60, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x78, 0x00, 0x00, 0x30,
  0x0c, 0x00, 0x00, 0x00, 0x0f, 0x83, 0xff, 0xc0, 0x00, 0x00, 0x00, 0x00, 0xf0, 0x00, 0x00, 0x38,
  0x18, 0x00, 0x00, 0x00, 0x00, 0x1f, 0xc0, 0x60, 0x00, 0x00, 0x00, 0x00, 0xe0, 0x00, 0x00, 0x18,
  0x18, 0x00, 0x00, 0x00, 0x00, 0x63, 0x80, 0x00, 0x00, 0x00, 0x00, 0x01, 0xe0, 0x00, 0x00, 0x18,
  0x18, 0x00, 0x00, 0x01, 0x80, 0x07, 0x80, 0x00, 0x0f, 0x0c, 0x00, 0x03, 0xc0, 0x00, 0x00, 0x1c,
  0x18, 0x00, 0x00, 0x07, 0x80, 0x07, 0x01, 0xe6, 0x1f, 0xbf, 0x07, 0xe3, 0x80, 0x00, 0x00, 0x1c,
  0x18, 0x00, 0x00, 0x0f, 0x00, 0x0f, 0x07, 0xc6, 0x3f, 0xff, 0x0f, 0xf7, 0x80, 0x00, 0x00, 0x18,
  0x18, 0x00, 0x00, 0x1e, 0x00, 0x1e, 0x0f, 0x06, 0x73, 0xef, 0x1e, 0x3f, 0x00, 0x00, 0x00, 0x18,
  0x1c, 0x00, 0x00, 0x1c, 0x00, 0x3c, 0x1e, 0x07, 0xe3, 0xce, 0x3c, 0x3f, 0x00, 0x00, 0x00, 0x38,
  0x0c, 0x00, 0x00, 0x3c, 0x00, 0x7c, 0x1c, 0x07, 0xc7, 0xc0, 0xf8, 0x3e, 0x00, 0x00, 0x00, 0x30,
  0x06, 0x00, 0x00, 0x38, 0x00, 0xf8, 0x3c, 0x07, 0x87, 0xc1, 0xf0, 0x3c, 0x00, 0x00, 0x00, 0x70,
  0x07, 0x00, 0x00, 0x3c, 0x01, 0xf0, 0x38, 0x0f, 0x0f, 0x63, 0xf0, 0x3c, 0x00, 0x00, 0x00, 0xe0,
  0x03, 0x80, 0x00, 0x3c, 0x03, 0xe0, 0x78, 0x0e, 0x0e, 0x7f, 0xe0, 0x78, 0x30, 0x00, 0x01, 0xc0,
  0x01, 0xc0, 0x00, 0x3e, 0x0f, 0xc0, 0x78, 0x1c, 0x1e, 0x7d, 0xe0, 0xf0, 0x60, 0x00, 0x03, 0x80,
  0x00, 0x70, 0x00, 0x1f, 0xff, 0xc0, 0xf8, 0x3c, 0x3c, 0x01, 0xe1, 0xf0, 0xc0, 0x00, 0x0e, 0x00,
  0x00, 0x3c, 0x00, 0x1f, 0xff, 0x01, 0xbc, 0x78, 0x38, 0x01, 0xe3, 0xe1, 0x80, 0x00, 0x3c, 0x00,
  0x00, 0x0f, 0x00, 0x0f, 0xfe, 0x01, 0x3f, 0xf0, 0x78, 0x00, 0xff, 0xf7, 0x00, 0x00, 0xf0, 0x00,
  0x00, 0x03, 0xc0, 0x07, 0xfc, 0x00, 0x1f, 0xc0, 0xf0, 0x00, 0xfe, 0xfe, 0x00, 0x03, 0xc0, 0x00,
  0x00, 0x00, 0xf8, 0x00, 0x00, 0x00, 0x07, 0x00, 0xe0, 0x00, 0x38, 0x78, 0x00, 0x1f, 0x00, 0x00,
  0x00, 0x00, 0x1f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfc, 0x00, 0x00,
  0x00, 0x00, 0x07, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xe0, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x7f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x0f, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1f, 0xf0, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x7f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xff, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x03, 0xff, 0xff, 0x80, 0x01, 0xff, 0xff, 0xc0, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};
// End Ford Logo --->

// <--- 'Stang_Logo_jpg'
const unsigned char Stang_Logo [] PROGMEM = {
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x70, 0x10, 0x00, 0x78, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x70, 0x7b, 0x81, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0xf0, 0xff, 0xdf, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0xf9, 0xff, 0xff, 0xff, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x01, 0xff, 0xff, 0xff, 0xff, 0xc0, 0x00, 0x07, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x01, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x01, 0xff, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x01, 0xff, 0xff, 0xff, 0xff, 0xfc, 0x3f, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf8, 0x00, 0xfe, 0x00, 0x00, 0x08,
  0x00, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc, 0x01, 0xff, 0xf0, 0x00, 0x1c,
  0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x03, 0xff, 0xff, 0x00, 0x7c,
  0x00, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x83, 0xff, 0xff, 0xff, 0xf8,
  0x00, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xc7, 0xff, 0xff, 0xff, 0xf0,
  0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xc0,
  0x00, 0x3f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x1f, 0xff, 0x80,
  0x00, 0x3f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc, 0x01, 0xfc, 0x00,
  0x00, 0x7f, 0xff, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf8, 0x00, 0x00, 0x00,
  0x00, 0xff, 0xfe, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x00, 0x00,
  0x00, 0xff, 0xfc, 0x01, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00,
  0x01, 0xff, 0xf8, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x80, 0x00, 0x00, 0x00,
  0x01, 0xff, 0xe0, 0x00, 0x7f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xc0, 0x00, 0x00, 0x00, 0x00,
  0x03, 0xff, 0x80, 0x00, 0x3f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00,
  0x03, 0xff, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x03, 0xff, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x01, 0xfe, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x01, 0xfc, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x70, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x1f, 0xff, 0xff, 0xe1, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x3f, 0xff, 0xf8, 0x01, 0xff, 0xff, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x3f, 0xff, 0xff, 0x01, 0xff, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x7e, 0x3f, 0xff, 0x81, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x7c, 0x1f, 0xff, 0x03, 0xff, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0xfe, 0x1f, 0xfe, 0x07, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xfc, 0x1f, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xf8, 0x7f, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0xff, 0xef, 0xf9, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x3c, 0x03, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xf8, 0x1f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};
// End Stang Logo --->s
// End Boot Logo Display Definitions --->

// <--- Begin Warning Logo Definitions

// <--- 'warning_trinagle_bw'
const unsigned char Warning_Triangle [] PROGMEM = {
  0x00, 0x00, 0x00, 0x03, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xe0, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x07, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xf0, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x1e, 0x78, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1c, 0x38, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x3c, 0x3c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x38, 0x1c, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x78, 0x1e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x70, 0x0e, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0xf0, 0x0f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xe0, 0x07, 0x80, 0x00, 0x00,
  0x00, 0x00, 0x01, 0xc0, 0x03, 0x80, 0x00, 0x00, 0x00, 0x00, 0x03, 0xc0, 0x03, 0xc0, 0x00, 0x00,
  0x00, 0x00, 0x03, 0x80, 0x01, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x07, 0x80, 0x01, 0xe0, 0x00, 0x00,
  0x00, 0x00, 0x07, 0x01, 0x80, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x0f, 0x07, 0xe0, 0xf0, 0x00, 0x00,
  0x00, 0x00, 0x1e, 0x07, 0xe0, 0x78, 0x00, 0x00, 0x00, 0x00, 0x1c, 0x07, 0xe0, 0x38, 0x00, 0x00,
  0x00, 0x00, 0x3c, 0x07, 0xe0, 0x3c, 0x00, 0x00, 0x00, 0x00, 0x38, 0x07, 0xe0, 0x1c, 0x00, 0x00,
  0x00, 0x00, 0x78, 0x07, 0xe0, 0x1e, 0x00, 0x00, 0x00, 0x00, 0xf0, 0x07, 0xe0, 0x0f, 0x00, 0x00,
  0x00, 0x00, 0xf0, 0x03, 0xc0, 0x0f, 0x00, 0x00, 0x00, 0x01, 0xe0, 0x03, 0xc0, 0x07, 0x80, 0x00,
  0x00, 0x01, 0xc0, 0x03, 0xc0, 0x03, 0x80, 0x00, 0x00, 0x03, 0xc0, 0x03, 0xc0, 0x03, 0xc0, 0x00,
  0x00, 0x03, 0x80, 0x01, 0xc0, 0x01, 0xc0, 0x00, 0x00, 0x07, 0x80, 0x01, 0x80, 0x01, 0xe0, 0x00,
  0x00, 0x0f, 0x00, 0x01, 0x80, 0x00, 0xf0, 0x00, 0x00, 0x0e, 0x00, 0x01, 0x80, 0x00, 0x70, 0x00,
  0x00, 0x1e, 0x00, 0x01, 0x80, 0x00, 0x78, 0x00, 0x00, 0x1c, 0x00, 0x00, 0x00, 0x00, 0x38, 0x00,
  0x00, 0x3c, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x00, 0x00, 0x38, 0x00, 0x01, 0x80, 0x00, 0x1c, 0x00,
  0x00, 0x78, 0x00, 0x07, 0xe0, 0x00, 0x1e, 0x00, 0x00, 0xf0, 0x00, 0x07, 0xe0, 0x00, 0x0f, 0x00,
  0x00, 0xe0, 0x00, 0x07, 0xe0, 0x00, 0x07, 0x00, 0x01, 0xe0, 0x00, 0x07, 0xe0, 0x00, 0x07, 0x80,
  0x01, 0xc0, 0x00, 0x07, 0xe0, 0x00, 0x03, 0x80, 0x03, 0xc0, 0x00, 0x01, 0x80, 0x00, 0x03, 0xc0,
  0x03, 0x80, 0x00, 0x00, 0x00, 0x00, 0x01, 0xc0, 0x07, 0x80, 0x00, 0x00, 0x00, 0x00, 0x01, 0xe0,
  0x07, 0x80, 0x00, 0x00, 0x00, 0x00, 0x01, 0xe0, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0,
  0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xc0
};
// End Warning Triangle --->s
// End Warning Logo Definitions --->

// <--- Begin CAN Bus Definitions

long unsigned int rxId;
unsigned char len = 0;
unsigned char rxBuf[8];
char msgString[128];                        // Array to store serial string

#define CAN0_INT 14                         // Set INT to pin 26
MCP_CAN CAN0(10);                           // Set CS to pin 10

// End CAN Bus Definitions --->

// <--- Begin AEMNet Calculations per AEM CAN PDF Guide

const float RPM_SCALE = .39063;
const float ENG_LOAD_SCALE = .0015259;
const float ENG_THROTTLE_SCALE = .0015259;
const float ANALOG_SCALE = .00007782;
const float O2_SCALE = .00390625;
const float SPEED_SCALE = .00390625;
const float IGN_SCALE = .35156;
const float BATT_VOLTAGE_SCALE = .0002455;
const float TPS_SCALE = .0015259;
const float MAP_SCALE = .014504;
const float FUELPRESS_SCALE = .580151;
const float OILPRESS_SCALE = .580151;
const float LAMBDATARGET_SCALE = .00390625;
const float INJDUTY_SCALE = .392157;
const float FLEXFUEL_SCALE = .392157;
const float SPARKCUT_SCALE = .39063;
const float FUELCUT_SCALE = .39063;

int8_t coolantC;
int8_t iatC;
int8_t oilC;

double MAP;
double VE;
double fuelpress;
double oilpress;
double lambdatgt;
double injpw;
double injpwpeak;
double injdutycycle;
double injdutypeak;
double flex;
double sparkcut;
double fuelcut;
double oilF;
double rpm;
double load;
double loadpeak;
double tps;
double coolantF;
double iatF;
double o2_a;
double o2_b;
double vehicleSpeed;
double vehicleSpeedpeak;
double timing;
double volts;

byte gear;

// End AEMNet Calculations per AEM CAN PDF Guide --->

// <--- Function for TCA Multiplexer for I2C

void tcaselect(uint8_t i) {
  if (i > 7) return;

//  Wire.setClock(1000000);
  Wire.setClock(100000);
  Wire.beginTransmission(TCAADDR);
  Wire.write(1 << i);
  Wire.endTransmission();
}

// End TCA Multiplexer for I2C --->

void setup()
{
  Serial.begin(2000000);                          // Super fast because Teensy is USB anyway

  // Initialize MCP2515 running at 8MHz with a baudrate of 500kb/s in STDEXT mode to allow filters.
  if (CAN0.begin(MCP_STDEXT, CAN_500KBPS, MCP_8MHZ) == CAN_OK)
    Serial.println("MCP2515 Initialized Successfully!");
  else
    Serial.println("Error Initializing MCP2515...");


  CAN0.init_Mask(0, 1, 0x1FFFFFF0);                // Set CAN filter mask 0
  CAN0.init_Mask(1, 1, 0x1FFFFFF0);                // Set CAN filter mask 1

  CAN0.init_Filt(0, 1, 0x01F0A000);                // Filter for CAN IDs 0-F

  CAN0.setMode(MCP_NORMAL);                        // Set operation mode to normal so the MCP2515 sends acks to received data.

  pinMode(CAN0_INT, INPUT);                        // Configuring pin for /INT input
  pinMode(gauge_switch, INPUT);                    // Configuring pin for Switch input
  pinMode(peak_switch, INPUT);                    // Configuring pin for Switch input

  attachInterrupt(gauge_switch, gauge_button_check, FALLING);  // Interrupt for button press to select gauges
  attachInterrupt(peak_switch, peak_button_check, FALLING);  // Interrupt for button press to select gauges

  attachInterrupt(CAN0_INT, canbus_read, FALLING); // Interrupt for CAN bus data receipt

}

// Function to intialize displays so that we can use them later

void initialize_displays()
{
  for (static int gauge_start = 0; gauge_start < 5; gauge_start++)
  {
    int tca = (gauge_start - 1);
    tcaselect(tca);
    display.begin(SSD1306_SWITCHCAPVCC);
    display.display();
    display.clearDisplay();
  }
}

// Function to display logos on boot up

void boot_logo(int gauge_id)
{
  if (boot_complete != 1)
  {
    display.display();
    display.clearDisplay();

    static unsigned long startMillis = millis();
    unsigned long currentMillis = millis();

    if (currentMillis - startMillis <= ford_logo_time)
    {
      tcaselect(gauge_id);
      display.drawBitmap(0, 0, Ford_Logo, 128, 64, WHITE);
    }
    else if ((currentMillis - startMillis > ford_logo_time) && (currentMillis - startMillis <= stang_logo_time))
    {
      tcaselect(gauge_id);
      display.drawBitmap(0, 0, Stang_Logo, 128, 64, WHITE);
    }
    else if (currentMillis - startMillis > stang_logo_time)
    {
      Serial.println("Displays booted!");
      boot_complete = 1;;
    }
  }
}

// Button Check Functions

void gauge_button_check()
{
  //  Serial.println("Interrupted!");

  volatile int CurrentSwitch = digitalRead(gauge_switch);

  if (CurrentSwitch == LOW)
  {
    if (gaugeselect < 3)
    {
      gaugeselect = gaugeselect + 1;
      //      Serial.println(gaugeselect);
    }
    else
    {
      gaugeselect = 0;
      //      Serial.println(gaugeselect);
    }
  }
}


void peak_button_check()
{
  Serial.println("Interrupted!");
  volatile int CurrentSwitch = digitalRead(peak_switch);

  if (CurrentSwitch == LOW)
  {
    Serial.println("Cleared");
    injpwpeak = 0;
    injdutypeak = 0;
  }
}

// Gauge Functons - Some Test functions exist here, will add more later

void boost_gauge(int tca)
{
  tcaselect(tca);
  boost = ((MAP * 0.04) - 14.23); // Calculate psi from raw value
  barboost = ((MAP * 0.19) - 69.45); // Calculate boost value for the bargraph
  vac = boost * -2.036020; // Used 'minus' 2.036020 so that the figure printed wont have a minus symbol in front of it

  display.setTextColor(WHITE);
  display.setTextSize(2);
  display.setCursor(5, 0);
  display.println(MAP, 0);
  display.setCursor(75, 0);
  display.println("kPa");
  display.setCursor(5, 19);
  display.println(boost, 1);
  display.setCursor(75, 19);
  display.println("PSI");
  display.drawRoundRect(1, 38, 125, 10, 5, WHITE); //Border of the bar chart
  display.fillRoundRect(1, 38, barboost, 10, 5, WHITE); //Draws the bar depending on the sensor value
  display.setTextSize(1);
  display.setCursor(20, 55);
  display.println(peak, 1); // Prints the peak value
  display.setCursor(60, 55);
  display.println("PSI Peak");
  display.display();
  display.clearDisplay();
}

void wideband_gauge_1(int tca)
{
  tcaselect(tca);
  afr1 = (o2_a * 14.7); // Calculate AFR

  display.setTextColor(WHITE);
  display.setTextSize(2);
  display.setCursor(5, 0);
  display.println(o2_a, 2);
  display.setCursor(75, 0);
  display.println("L1");
  display.setCursor(5, 19);
  display.println(afr1, 1);
  display.setCursor(75, 19);
  display.println("AFR1");
  display.drawRoundRect(1, 38, 125, 10, 5, WHITE); //Border of the bar chart
  display.fillRoundRect(1, 38, barboost, 10, 5, WHITE); //Draws the bar depending on the sensor value
  display.setTextSize(1);
  display.setCursor(2, 55);
  display.println("Lean"); // Prints the peak value
  display.setCursor(100 , 55);
  display.println("Rich");
  display.display();
  display.clearDisplay();
}

void wideband_gauge_2(int tca)
{
  tcaselect(tca);
  afr2 = (o2_b * 14.7); // Calculate
  if (boost > peak) peak = boost ; // Store new peak value in peak memory

  display.setTextColor(WHITE);
  display.setTextSize(2);
  display.setCursor(5, 0);
  display.println(o2_b, 2);
  display.setCursor(75, 0);
  display.println("L2");
  display.setCursor(5, 19);
  display.println(afr2, 1);
  display.setCursor(75, 19);
  display.println("AFR2");
  display.drawRoundRect(1, 38, 125, 10, 5, WHITE); //Border of the bar chart
  display.fillRoundRect(1, 38, barboost, 10, 5, WHITE); //Draws the bar depending on the sensor value
  display.setTextSize(1);
  display.setCursor(2, 55);
  display.println("Lean"); // Prints the peak value
  display.setCursor(100 , 55);
  display.println("Rich");
  display.display();
  display.clearDisplay();
}

void e85_gauge(int tca)
{
  tcaselect(tca);
  e85_raw = flex; // Read MAP sensor raw value on analog port 0
  int e90 = 71;
  char buff[10];
  sprintf(buff, "%3d", e90);

  display.setFont();
  display.setTextColor(WHITE);
  display.setTextSize(1);
  display.setCursor(18, 1);
  display.println("Ethanol Content");
  display.setTextSize(4);
  display.setCursor(4, 28);
  display.println(buff);
  display.setCursor(82, 28);
  display.println("%");
  display.display();
  display.clearDisplay();
  display.setFont();
}

void inj_dc_gauge(int tca)
{
  tcaselect(tca);

// Begin Temp Testing

  if (injdutypeak <=98)
  {
    injdutypeak++;
  }
      
// End Temp Testing

  display.setFont();
  display.setTextColor(WHITE);
  display.setTextSize(1);
  display.setCursor(0, 1);
  if (injdutypeak >= injdutywarning)
  {
    display.println("Inj Duty Peak:");
    display.setCursor(95, 1);
    display.println(injdutypeak, 0);
    display.setCursor(108, 1);
    display.println("%");       
    display.drawBitmap(30, 15, Warning_Triangle, 64, 48, WHITE);
  }
  else
  {
    display.println("Injector Duty Cycle");
    display.setTextSize(1);
    display.setCursor(3, 22);
    display.println("Current:");
    display.setTextSize(2);   
    display.setCursor(80, 19);
//    display.println(injdutycycle, 0);
    display.println("30");
    display.setCursor(110, 19);
    display.println("%");   
    display.drawFastHLine(0, 40, 128, WHITE);
    display.setTextSize(1);   
    display.setCursor(3, 52);
    display.println("Peak:");
    display.setTextSize(2);     
    display.setCursor(80, 49);
    display.println(injdutypeak, 0);
    display.println("82");
    display.setCursor(110, 49);
    display.println("%"); 
  }
  display.display();
  display.clearDisplay();
  display.setFont();
}

void inj_pw_gauge(int tca)
{
  tcaselect(tca);
 
  display.setFont();
  display.setTextColor(WHITE);
  display.setTextSize(1);
  display.setCursor(5, 1);
  display.println("Injector Pulsewidth");
  if (injpwpeak >= injpwwarning)
  {
    display.drawBitmap(30, 15, Warning_Triangle, 64, 48, WHITE);
  }
  else
  {
    display.setTextSize(1);
    display.setCursor(3, 22);
    display.println("Current:");
    display.setTextSize(2);   
    display.setCursor(70, 19);
//    display.println(injpw, 0);
    display.println("14");
    display.setCursor(100, 19);
    display.println("ms");   
    display.drawFastHLine(0, 40, 128, WHITE);
    display.setTextSize(1);   
    display.setCursor(3, 52);
    display.println("Peak:");
    display.setTextSize(2);     
    display.setCursor(70, 49);
//    display.println(injpwpeak, 0);
    display.println("21"); 
    display.setCursor(100, 49);
    display.println("ms"); 
  }
  display.display();
  display.clearDisplay();
  display.setFont();
}

void tach_gauge(int tca)
{
  tcaselect(tca);
 
  display.setFont();
  display.setTextColor(WHITE);
  display.setTextSize(1);
  display.setCursor(33, 1);
  display.println("Engine RPM");
  display.setTextSize(1);
  display.setCursor(3, 22);
  display.println("Current:");
  display.setTextSize(2);   
  display.setCursor(75, 19);
//    display.println(rpm, 0);
  display.println("1500");
  display.drawFastHLine(0, 40, 128, WHITE);
  display.setTextSize(1);   
  display.setCursor(3, 52);
  display.println("Peak:");
  display.setTextSize(2);     
  display.setCursor(75, 49);
//    display.println(rpmpeak, 0);
  display.println("7000");   
  display.display();
  display.clearDisplay();
  display.setFont();
}

void voltage_gauge(int tca)
{
  tcaselect(tca);

  display.setTextColor(WHITE);
  display.setTextSize(1);
  display.setCursor(30, 5);
  display.println("ECU Voltage");
  display.setTextSize(4);
  display.setCursor(0, 28);
//  display.println(volts, 1);
  display.println("14.8");
  display.setCursor(103, 28);
  display.println("V");
  display.display();
  display.clearDisplay();
}

void gear_gauge(int tca)
{
  tcaselect(tca);

  display.setTextColor(WHITE);
  display.setTextSize(1);
  display.setCursor(24, 2);
  display.println("Current Gear");
  display.setTextSize(4);
  display.setCursor(50, 28);
//  display.println(gear, 0);
  display.println("4");
  display.display();
  display.clearDisplay();
}

void tps_gauge(int tca)
{
  tcaselect(tca);

  display.setTextColor(WHITE);
  display.setTextSize(1);
  display.setCursor(10, 2);
  display.println("Throttle Position");
  display.setTextSize(4);
  display.setCursor(12, 28);
//  display.println(tps, 0);
  display.println("100");
  display.setCursor(90, 28);
  display.println("%");
  display.display();
  display.clearDisplay();
}

void timing_gauge(int tca)
{
  tcaselect(tca);

  int igntime = 30;
  char buff[10];
  sprintf(buff, "%3d", igntime);

  display.setTextColor(WHITE);
  display.setTextSize(1);
  display.setCursor(22, 2);
  display.println("Engine Timing");
  display.setTextSize(4);
  display.setCursor(31, 28);
  display.println(igntime);
  display.setTextSize(1); 
  display.setCursor(86, 28);
  display.println("*");
  display.display();
  display.clearDisplay();
}

void ve_gauge(int tca)
{
  tcaselect(tca);

  display.setTextColor(WHITE);
  display.setTextSize(1);
  display.setCursor(10, 2);
  display.println("Engine VE");
  display.setTextSize(4);
  display.setCursor(12, 28);
//  display.println(ve, 0);
  display.println("92");
  display.display();
  display.clearDisplay();
}

void speedo_gauge(int tca)
{
  tcaselect(tca);

  static int spd = 7;
  static int Speedpeak = 191;
 
  display.setFont();
  display.setTextColor(WHITE);
  display.setTextSize(1);
  display.setCursor(27, 1);
  display.println("Vehicle Speed");
  display.setTextSize(1);
  display.setCursor(3, 22);
  display.println("Current:");
  display.setTextSize(2);
  if (spd < 10)
  {
  display.setCursor(86, 19);
  }
  else if ((spd > 10) && (spd < 100))
  {
  display.setCursor(73, 19);
  }
  else if (spd >= 100)
  {
  display.setCursor(60, 19);   
  }     
//  display.println(vehicleSpeed, 0);
  display.println(spd);
  display.setTextSize(1);       
  display.setCursor(105, 23);
  display.println("MPH"); 
  display.drawFastHLine(0, 40, 128, WHITE);
  display.setTextSize(1);   
  display.setCursor(3, 52);
  display.println("Peak:");
  display.setTextSize(2);     
  if (Speedpeak < 10)
  {
  display.setCursor(86, 49);
  }
  else if ((Speedpeak > 10) && (Speedpeak < 100))
  {
  display.setCursor(73, 49);
  }
  else if (Speedpeak >= 100)
  {
  display.setCursor(60, 49);   
  }  //  display.println(vehicleSpeedpeak, 0);
  display.println(Speedpeak);
  display.setTextSize(1);     
  display.setCursor(105, 53);
  display.println("MPH");     
  display.display();
  display.clearDisplay();
  display.setFont();
}

void engine_load_gauge(int tca)
{
  tcaselect(tca);

  static int loadtest = 13;
  static int loadtestpeak = 100;
 
  display.setFont();
  display.setTextColor(WHITE);
  display.setTextSize(1);
  display.setCursor(30, 1);
  display.println("Engine Load");
  display.setTextSize(1);
  display.setCursor(3, 22);
  display.println("Current:");
  display.setTextSize(2);
  if (loadtest < 10)
  {
  display.setCursor(91, 19);
  }
  else if ((loadtest > 10) && (loadtest < 100))
  {
  display.setCursor(78, 19);
  }
  else if (loadtest == 100)
  {
  display.setCursor(65, 19);   
  }     
//  display.println(load, 0);
  display.println(loadtest);
  display.setCursor(105, 19);
  display.println("%"); 
  display.drawFastHLine(0, 40, 128, WHITE);
  display.setTextSize(1);   
  display.setCursor(3, 52);
  display.println("Peak:");
  display.setTextSize(2);     
  if (loadtestpeak < 10)
  {
  display.setCursor(91, 49);
  }
  else if ((loadtestpeak > 10) && (loadtestpeak < 100))
  {
  display.setCursor(78, 49);
  }
  else if (loadtestpeak == 100)
  {
  display.setCursor(65, 49);   
  }   
//    display.println(loadpeak, 0);
  display.println(loadtestpeak);
  display.setCursor(105, 49);
  display.println("%");     
  display.display();
  display.clearDisplay();
  display.setFont();
}

void limiter_gauge(int tca)
{
  tcaselect(tca);

  display.setTextColor(WHITE);
  display.setTextSize(1);
  display.setCursor(40, 1);
  display.println("Spark Cut");
  display.setTextSize(2);
  display.setCursor(15, 16);
//  display.println(sparkcut, 0);
  display.println("7300");
  display.setCursor(85, 16);
  display.println("RPM");
  display.setTextSize(1);
  display.setCursor(40, 36);
  display.println("Fuel Cut");
  display.setTextSize(2);
  display.setCursor(15, 50);
//  display.println(fuelcut, 0);
  display.println("7400");
  display.setCursor(85, 50);
  display.println("RPM"); 
  display.display();
  display.clearDisplay();
}

void pressure_gauge(int tca)
{
  tcaselect(tca);

  display.setTextColor(WHITE);
  display.setTextSize(1);
  display.setCursor(30, 1);
  display.println("Oil Pressure");
  display.setTextSize(2);
  display.setCursor(25, 16);
//  display.println(oilpress, 0);
  display.println("71");
  display.setCursor(75, 16);
  display.println("PSI");
  display.setTextSize(1);
  display.setCursor(27, 36);
  display.println("Fuel Pressure");
  display.setTextSize(2);
  display.setCursor(25, 50);
//  display.println(fuelpress, 0);
  display.println("58");
  display.setCursor(75, 50);
  display.println("PSI"); 
  display.display();
  display.clearDisplay();
}

void temp_gauge(int tca)
{
  tcaselect(tca);
 
  display.setTextColor(WHITE);
  display.setTextSize(1);
  display.setCursor(3, 4);
  display.println("Intake (F):");
  display.setTextSize(2); 
  display.setCursor(90, 1);
//  display.println(iatF, 0);
  display.println("120");
  display.drawFastHLine(0, 19, 128, WHITE);
  display.setTextSize(1);
  display.setCursor(3, 28);
  display.println("Coolant (F):");
  display.setTextSize(2);   
  display.setCursor(90, 25);
//  display.println(coolantF, 0);
  display.println("185");
  display.drawFastHLine(0, 43, 128, WHITE);
  display.setTextSize(1);   
  display.setCursor(3, 52);
  display.println("Oil (F):");
  display.setTextSize(2);     
  display.setCursor(90, 49);
//  display.println(oilF, 0);
  display.println("205");
  display.display();
  display.clearDisplay();
}

// End Gauges --->

// <--- Begin Test Gauges

void refresh_test_gauge(int tca)   // Test Refesh Speed on Display
{
  static int test = 1;

  tcaselect(tca);

  display.setTextColor(WHITE);
  display.setTextSize(1);
  display.setCursor(5, 1);
  display.println("Let's test refresh!");
  display.setTextSize(3);
  if (test < 100)
  {
  display.setCursor(45, 15);
  }
  else if ((test > 100) && (test < 1000))
  {
  display.setCursor(40, 15);   
  }
  display.println(test);
  display.display();
  display.clearDisplay();
  test++;
}

void alert_test_gauge(int tca)   // Demo "Alert" for Trouble
{
  tcaselect(tca);

  display.setFont();
  display.setTextColor(WHITE);
  display.setTextSize(1);
  display.setCursor(7, 1);
  display.println("Imaginary Problem");
  display.drawBitmap(30, 15, Warning_Triangle, 64, 48, WHITE);
  display.display();
  display.clearDisplay();
  display.setFont();
}

// End Test Gauges --->

// <--- Begin CAN Bus function

void canbus_read()
{
  if (!digitalRead(CAN0_INT))                     
  {
    CAN0.readMsgBuf(&rxId, &len, rxBuf);     
    
     unsigned char canId = rxId;


    if (canId == 0) {
      uint16_t rawRPM = (uint16_t)rxBuf[0] << 8;
      rawRPM |= rxBuf[1];
      rpm = rawRPM * RPM_SCALE;

      uint16_t rawLoad = (uint16_t)rxBuf[2] << 8;
      rawLoad |= rxBuf[3];
      load = rawLoad * ENG_LOAD_SCALE;
    
      if (load > loadpeak) loadpeak = load ; // Store new peak value in peak memory

      uint16_t rawTPS = (uint16_t)rxBuf[4] << 8;
      rawTPS |= rxBuf[5];
      tps = rawTPS * TPS_SCALE;

      iatC = rxBuf[6];

      iatF = ((double)iatC * 1.8) + 32;

      coolantC = rxBuf[7];

      coolantF = ((double)coolantC * 1.8) + 32;

      //            Serial.print("RPM: ");
      //            Serial.print(rpm);
      //            Serial.print("  Load: ");
      //            Serial.print(load);
      //            Serial.print("  TPS: ");
      //            Serial.print(tps);
      //            Serial.print("  IAT (C): ");
      //            Serial.print(iatC);
      //            Serial.print("  Temp (C): ");
      //            Serial.print(coolantC);
      //            Serial.println();
    }

    else if (canId == 3) {
      uint8_t rawo2_a = (uint8_t)rxBuf[0];
      o2_a = rawo2_a * O2_SCALE + 0.5;

      uint8_t rawo2_b = (uint8_t)rxBuf[1];
      o2_b = rawo2_b * O2_SCALE + 0.5;

      uint16_t rawSpeed = (uint16_t)rxBuf[2] << 8;
      rawSpeed |= rxBuf[3];
      vehicleSpeed = rawSpeed * SPEED_SCALE;

      if (vehicleSpeed > vehicleSpeedpeak) vehicleSpeedpeak = vehicleSpeed ; // Store new peak value in peak memory     

      gear = rxBuf[4];

      uint8_t rawign = (uint8_t)rxBuf[5];
      timing = rawign * IGN_SCALE - 17;

      uint16_t rawVolts = (uint16_t)rxBuf[6] << 8;
      rawVolts |= rxBuf[7];
      volts = rawVolts * BATT_VOLTAGE_SCALE;

      //            Serial.print("WBO2 1: ");
      //            Serial.print(o2_a);
      //            Serial.print("  WBO2 1: ");
      //            Serial.print(o2_b);
      //            Serial.print("  Speed: ");
      //            Serial.print(vehicleSpeed);
      //            Serial.print("  Gear: ");
      //            Serial.print(gear);
      //            Serial.print("  Timing *: ");
      //            Serial.print(timing);
      //            Serial.print("  Volts: ");
      //            Serial.print(volts);
      //            Serial.println();
    }

    else if (canId == 4) {
      uint16_t rawMAP = (uint16_t)rxBuf[0] << 8;
      rawMAP |= rxBuf[1];
      MAP = rawMAP * MAP_SCALE;

      uint8_t rawVE = (uint8_t)rxBuf[2];
      VE = rawVE;

      uint8_t rawfuelpress = (uint8_t)rxBuf[3];
      fuelpress = rawfuelpress * FUELPRESS_SCALE;

      uint8_t rawoilpress = (uint8_t)rxBuf[4];
      oilpress = rawoilpress * OILPRESS_SCALE;

      uint8_t rawlambda = (uint8_t)rxBuf[5];
      lambdatgt = rawlambda * LAMBDATARGET_SCALE + 0.5;

      //            Serial.print("MAP: ");
      //            Serial.print(MAP);
      //            Serial.print("  VE: ");
      //            Serial.print(VE);
      //            Serial.print("  Fuel Pressure: ");
      //            Serial.print(fuelpress);
      //            Serial.print("  Oil Pressure: ");
      //            Serial.print(oilpress);
      //            Serial.print("  Lambda Target: ");
      //            Serial.print(lambdatgt);
      //            Serial.println();
    }

    else if (canId == 6) {
      uint8_t rawinjpw = (uint8_t)rxBuf[0];
      injpw = rawinjpw * 0.1;
 
      if (injpw > injpwpeak) injpwpeak = injpw ; // Store new peak value in peak memory

      uint8_t rawinjdc = (uint8_t)rxBuf[2];
      injdutycycle = rawinjdc * INJDUTY_SCALE;

//      if (injdutycycle > injdutypeak) injdutypeak = injdutycycle ; // Store new peak value in peak memory     

      //            Serial.print("Injector PW: ");
      //            Serial.print(injpw);
      //            Serial.print("  Injector Duty: ");
      //            Serial.print(injdutycycle);
      //            Serial.println();
    }

    else if (canId == 7) {
      uint8_t rawflex = (uint8_t)rxBuf[4];
      flex = rawflex * FLEXFUEL_SCALE;

      uint8_t rawoilC = (uint8_t)rxBuf[6];
      oilC = rawoilC - 50;

      oilF = ((double)oilC * 1.8) + 32;

      //            Serial.print("E85 %: ");
      //            Serial.print(flex);
      //            Serial.print("   Oil Temp: ");
      //            Serial.print(oilF);
      //            Serial.println();
    }

    else if (canId == 8) {
      uint16_t rawsparkcut = (uint16_t)rxBuf[1] << 8;
      rawsparkcut |= rxBuf[2];
      sparkcut = rawsparkcut * SPARKCUT_SCALE;

      uint16_t rawfuelcut = (uint16_t)rxBuf[3] << 8;
      rawfuelcut |= rxBuf[4];
      fuelcut = rawfuelcut * FUELCUT_SCALE;

      //            Serial.print("Spark Cut RPM: ");
      //            Serial.print(sparkcut);
      //            Serial.print("   Fuel Cut RPM: ");
      //            Serial.print(fuelcut);
      //            Serial.println();
    }

  }
}
// End CAN Bus Function --->


// Run main

void loop()
{

  while (boot_complete != 1)
  {
    static int boot = 0;

    initialize_displays();

    boot_logo(boot);
    if (boot < 4)
    {
      boot++;
    }
    else
    {
      boot = 0;
    }
  }

  // Run Gauges
  // Available Gauges:
  // boost_gauge
  // wideband_gauge_1
  // wideband_gauge_2
  // e85_gauge
  // inj_dc_gauge
  // inj_pw_gauge
  // tps_gauge
  // tach_gauge
  // voltage_gauge
  // gear_gauge
  // pressure_gauge
  // limiter_gauge
  // temp_gauge
  // engine_load_gauge
  // speedo_gauge
  // timing_gauge
  // ve_gauge
  // refresh_test_gauge
  // alert_test_gauge
 
  if (boot_complete == 1) // Don't display gauges until boot is complete!
  {
    if (gaugeselect == 1)
    {
      gear_gauge(0);
      pressure_gauge(1);
      temp_gauge(2);
      engine_load_gauge(3);
    }
    else if (gaugeselect == 0)
    {
      wideband_gauge_1(0);
      wideband_gauge_2(1);
      boost_gauge(2);
      speedo_gauge(3);
    }
    else if (gaugeselect == 2)
    {
      e85_gauge(0);
      voltage_gauge(1);
      limiter_gauge(2);
      timing_gauge(3);
    }
    else if (gaugeselect == 3)
    {
      inj_dc_gauge(0);
      inj_pw_gauge(1);
      tach_gauge(2);
      tps_gauge(3);
    }   
  }
}

PaulStoffregen · Dec 6, 2023

Have you tried running it on a Teensy 4.0? Looks like this should probably just work, assuming the mcp_can library uses SPI to talk to that chip. SPI uses the same pins, and this code looks like I2C is used for the display, which also uses the same pins between Teensy 3.2 to Teensy 4.0. Unless there's something really strange about that mcp_can library, really looks like this program ought to just work if you load it onto a Teensy 4.0.

One minor possible issue is Teensy 4.0 runs a lot faster and uses more power than Teensy 3.2. You can reduce Teensy 4.0 speed by clicking Tools > CPU Speed before you upload the code.

Jortsa22420 · Dec 6, 2023

Thank you for good and fast reply, really appreciate it since I am complete beginner. Power draw won't be an issue, but it could be more efficient since this code doesn't need much performance. It seems like the Teensy 4.0 is the same size and the pins that I will be utilizing are in the correct place. I have PCB for this design, it uses MPC2515 and TCA9548A IIC/I2C 8 channel Multiplexer for combined 3 screens. I have attached a photo showing routing for the components which utilize SPI. The PCB has it's own 2 regulators, since this will be mounted on a car, 5v for the pcb itself and 3v for the i2c displays. I have not been able to test it yet, since im waiting for the components.

Teensy 3.2 code conversion to 4.0

Jortsa22420

New member

PaulStoffregen

Well-known member

Jortsa22420

New member