#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
Adafruit_SSD1306 display = Adafruit_SSD1306(128, 64, &Wire);
//Screen init
#define TPSpin A0 // TPS
#define MPHpin A1 // MPH input
#define Apin 8 // Solenoid A
#define Bpin 7 // Solenoid B
#define Tpin 9 // Solenoid(s) TCC
#define Dpin 10 // Solenoid Line Pressure PWM
#define Spin 6 // Solenoid 3-2 downshift
#define FSTpin A6
#define LOTpin A2
#define WOTpin A3
#define SCRpin 12
const float DownShiftSlack = 0.9; // difference between upshift and downshift MPH
const int ShiftPoint = 45; // low throttle 3>4 shift
const float GearSpread = 3; // (GearSpread)*(ShiftPoint)=theoretical MAX MPH in 3rd
const int WOTthresh = 80; // WOT threshhold for 1-2 shift
const int FirstRangeLow = 10; //Fst knob range
const int FirstRangeHigh = 60; //Fst knob range
const int ShiftRangeLow = 50; // Low & High shift knob low range
const int ShiftRangeHigh = 150; // Low & High shift knob high range
const int LockDelay = 5; // MPH adder for lockup after 4th
const int LineMinPres = 30; // idle line duty
const int LineMaxTPS = 50; // max TPS for line reduction
int TPS;
int MPH;
int Gear = 1;
int Lock = 0;
int Line = 0;
int TPSmin = 200; // selfcal TPS
int TPSmax = 800; // selfcal TPS
int FSToffset; // dynamic 1st gear slip
int LOToffset; // dynamic low throttle shift
int WOToffset; // dynamic WOT shift
float GearRatio[5] {0, 3.06, 1.63, 1.00, 0.70};
void PrintXY_n (int value, int x, int y) {
display.setCursor(x, y);
display.print(value);
}
void PrintXY_s (String value, int x, int y) {
display.setCursor(x, y);
display.print(value);
}
int ScaleTPS (int TPSraw) {
if (TPSraw < TPSmin) TPSmin = TPSraw;
if (TPSraw > TPSmax) TPSmax = TPSraw;
return map(TPSraw, TPSmin, TPSmax, 0, 100);
}
void GearSol() {
if (Gear == 1) {
digitalWrite(Apin, HIGH);
digitalWrite(Bpin, HIGH);
}
if (Gear == 2) {
digitalWrite(Apin, LOW);
digitalWrite(Bpin, HIGH);
}
if (Gear == 3) {
digitalWrite(Apin, LOW);
digitalWrite(Bpin, LOW);
}
if (Gear == 4) {
digitalWrite(Apin, HIGH);
digitalWrite(Bpin, LOW);
}
}
int ArrayMPH (int WhatGear) {
float GearOffset = 1 / GearRatio[WhatGear];
int GearLow = ShiftPoint * GearOffset * LOToffset / 100; //always devide last in int calc
int GearHigh = ShiftPoint * GearOffset * WOToffset * GearSpread / 100;
if (TPS<15) return GearLow;
if (TPS>85) return GearHigh;
return map(TPS, 15, 85, GearLow, GearHigh);
}
int ConvSlip (){
return 100 - FSToffset;
}
void GearSelector () {
if (Gear == 4 && MPH < ArrayMPH(3)*DownShiftSlack) Gear = 3;
if (Gear == 3 && MPH < ArrayMPH(2)*DownShiftSlack) Gear = 2;
if (Gear == 2 && MPH < ArrayMPH(1)*DownShiftSlack) Gear = 1;
if (Gear == 1 && TPS > WOTthresh && MPH > ArrayMPH(1)*ConvSlip()/100 ) Gear = 2;
else if (Gear == 1 && MPH > ArrayMPH(1) ) Gear = 2;
if (Gear == 2 && MPH > ArrayMPH(2) ) Gear = 3;
if (Gear == 3 && MPH > ArrayMPH(3) ) Gear = 4;
}
void LockUpSelector () {
if (Gear == 4) {
if (MPH > (ArrayMPH(3) + LockDelay)) Lock = 1;
if (MPH < (ArrayMPH(3)*DownShiftSlack + LockDelay)) Lock = 0;
}
else Lock = 0;
}
void LockUpSol (){
if (Lock == 1) digitalWrite(Tpin, HIGH);
else digitalWrite(Tpin, LOW);
}
void LineSelector () {
if (TPS > LineMaxTPS) Line=0;
else Line = LineMinPres - map(TPS, 0, LineMaxTPS, 0, LineMinPres);
}
void LineSol () {
analogWrite(Dpin, Line);
}
void DownShiftSol(){
if (Gear == 1) analogWrite(Spin, 0); //if in first gear 0%
else analogWrite(Spin, 230); //all other gears 90%
}
void print1 (){
display.clearDisplay();
PrintXY_s ("TPS=", 0, 0);
PrintXY_n (TPS, 24, 0);
PrintXY_s ("MPH=",49, 0);
PrintXY_n (MPH, 73, 0);
PrintXY_s("Gear=", 49, 11);
PrintXY_n(Gear, 79, 11);
PrintXY_s("L=", 103, 0);
PrintXY_n(Lock,117, 0);
PrintXY_s("Line=", 0, 11);
PrintXY_n (Line, 30, 11);
PrintXY_s ("ConverterSlip=", 0, 33);
PrintXY_n (FSToffset, 85, 33);
PrintXY_s ("LOT=", 0, 44);
PrintXY_n (LOToffset, 24, 44);
PrintXY_s ("WOT=", 49, 44);
PrintXY_n (WOToffset, 73, 44);
display.display();
}
void setup()
{
display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
display.clearDisplay();
display.setTextSize(1);
display.setTextColor(WHITE);
TCCR0B = TCCR0B & B11111000 | B00000101; // for PWM frequency of 61.04 Hz D5 & D6
pinMode(Apin, OUTPUT);
pinMode(Bpin, OUTPUT);
pinMode(Tpin, OUTPUT);
pinMode(Dpin, OUTPUT);
pinMode(TPSpin, INPUT_PULLUP);
pinMode(MPHpin, INPUT_PULLUP);
pinMode(LOTpin, INPUT_PULLUP);
pinMode(WOTpin, INPUT_PULLUP);
pinMode(FSTpin, INPUT_PULLUP);
pinMode(SCRpin, INPUT_PULLUP);
}
void loop()
{
TPS = ScaleTPS(analogRead(TPSpin));
MPH = analogRead(MPHpin)/6.6;
FSToffset = map(analogRead(FSTpin), 0, 1023, FirstRangeLow, FirstRangeHigh);
LOToffset = map(analogRead(LOTpin), 0, 1023, ShiftRangeLow, ShiftRangeHigh);
WOToffset = map(analogRead(WOTpin), 0, 1023, ShiftRangeLow, ShiftRangeHigh);
GearSelector ();
LockUpSelector ();
LineSelector ();
GearSol();
LockUpSol();
LineSol();
DownShiftSol();
print1();
}