Well, thank you all for the help! I now have a fully working program that does what it's supposed to! For anyone interested here's the code:
/*
FH 2014 Throttle and Shift.ino
Bosch electronic throttle body control
for MSOE SAE Formula Hybrid 2014
Author: Austin R. Bartz
Last Revision Date: 4/6/2014
Hardware Connections:
-TPS 0: Pin A0 (Blue Wire)
-TPS 1: Pin A1 (Thin White Wire)
-Throttle Input 0: Pin A2 (White Wire)
-Throttle Input 1: Pin A3 (White Marked Wire)
-Error LED Pin A5 (Blue Wire)
-UpShift Relay: Pin 2 (Yellow Wire)
-DownShift Relay: Pin 3 (Pink Wire)
-UpShift Button: Pin 4 (Green Wire)
-DownShift Button: Pin 5 (Orange Wire)
-Ignition Cut Pin 6 (White with Green Stripe Wire)
-Neutral Switch Pin 7 (Brown Wire)
-L298N H-Bridge Enable A: Pin 9 (N/A)
-L298N H-Bridge Input 1: Pin 8 (Other Speaker Wire)
-L298N H-Bridge Input 2: Pin 11 (Gray Line Wire)
*/
//Pins assignments
#define LEDPin 6
#define pinI1 8
#define pinI2 11
#define speedPin 9
const int upRelay = 2;
const int upSwitch = 4;
const int downRelay = 3;
const int downSwitch = 5;
const int neutralSwitch = 7;
const int ignCut = 6;
// Function Variables
int upTime = 150;
int downTime = 150;
int neutralTime = 42;
int debounceDelay = 5;
int gearCount = 0;
boolean downLast = false;
boolean upLast = false;
boolean neutralLast = false;
//PID Library
#include <PID_v1.h>
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint,1,0,0, DIRECT);
void setup()
{
//Set PWM frequency to 31.37255 kHz
TCCR1B = TCCR1B & 0b11111000 | 0x01;
Input = 0;
Setpoint = 0;
myPID.SetMode(AUTOMATIC);
pinMode(pinI1,OUTPUT);
pinMode(pinI2,OUTPUT);
pinMode(speedPin,OUTPUT);
pinMode(LEDPin,OUTPUT);
digitalWrite(pinI2,LOW);
digitalWrite(pinI1,HIGH);
Serial.begin(9600);
pinMode(downSwitch, INPUT_PULLUP);
pinMode(upSwitch, INPUT_PULLUP);
pinMode(neutralSwitch, INPUT_PULLUP);
pinMode(downRelay, OUTPUT);
pinMode(upRelay, OUTPUT);
pinMode(ignCut, OUTPUT);
Serial.println(gearCount);
}
void loop()
{
//Uncomment to tune the PID loop
/*
float Kp = mapfloat(analogRead(4), 0, 1023, 0, 5);
Serial.print(Kp, DEC);
Serial.print(" ");
float Ki = mapfloat(analogRead(5), 0, 1023, 0, 5);
Serial.print(Ki, DEC);
Serial.print(" ");
myPID.SetTunings(Kp, Ki, 0.00);
*/
shiftUp();
shiftDown();
shiftNeutral();
digitalWrite(LEDPin, LOW);
//PID Loop tunings
myPID.SetTunings(0.15, 2.00, 0.00);
int TPS0 = constrain(analogRead(0), 663, 54); //Range: 663 - 54
int TPS1 = constrain(analogRead(1), 164, 970); //Range: 164 - 970
int diff0 = (TPS0-54)+map((TPS1-164), 0, 806, 0, 609);
while(analogRead(0) < 40)
{
analogWrite(speedPin,0);
digitalWrite(LEDPin, HIGH);
analogRead(0);
Serial.println("ERROR 1");
}
while(analogRead(1) > 1000)
{
analogWrite(speedPin,0);
digitalWrite(LEDPin, HIGH);
analogRead(1);
Serial.println("ERROR 2");
}
//PID Input from TPS
Input = map(constrain((TPS1 - 164), 0, 806), 0, 806, 0, 380);
int Pedal0 = constrain(analogRead(2), 0, 180); //Range: 0 - 180
int Pedal1 = constrain(analogRead(3), 0, 380); //Range: 0 - 380
int diff1 = Pedal0 - map(Pedal1, 0, 380, 0, 180);
while(analogRead(2) > 200)
{
analogWrite(speedPin,0);
digitalWrite(LEDPin, HIGH);
analogRead(2);
Serial.println("ERROR 3");
}
while(analogRead(3) > 400)
{
analogWrite(speedPin,0);
digitalWrite(LEDPin, HIGH);
analogRead(3);
Serial.println("ERROR 4");
}
while(diff1 > 50)
{
analogWrite(speedPin,0);
digitalWrite(LEDPin, HIGH);
int Pedal0 = analogRead(2); //Range: 0 - 180
int Pedal1 = analogRead(3); //Range: 0 - 380
int diff1 = Pedal0 - map(Pedal1, 0, 380, 0, 180);
Serial.println("ERROR 5");
}
//PID Setpoint from Throttle Pedal
Setpoint = Pedal1;
//Set throttle to 0
if(Setpoint <= 5)
{
analogWrite(speedPin,0);
}
else
{
myPID.Compute();
analogWrite(speedPin,Output);
}
/*
Serial.print(diff0, DEC);
Serial.print(" ");
Serial.print(diff1, DEC);
Serial.print(" ");
Serial.print(TPS0, DEC);
Serial.print(" ");
Serial.print(TPS1, DEC);
Serial.print(" ");
Serial.print(Pedal0, DEC);
Serial.print(" ");
Serial.print(Pedal1, DEC);
Serial.print(" ");
Serial.print(Input, DEC);
Serial.print(" ");
Serial.print(Setpoint, DEC);
Serial.print(" ");
Serial.println(Output, DEC);
*/
}
float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
// Downshift Sequence
void shiftDown()
{
if(digitalRead(downSwitch) == LOW && downLast == false)
{
digitalWrite(downRelay, HIGH);
delay(downTime);
digitalWrite(downRelay, LOW);
if(gearCount > 1)
{
gearCount --;
}
Serial.println(gearCount);
downLast = true;
}
else
{
digitalWrite(downRelay, LOW);
}
if(digitalRead(downSwitch) == LOW)
{
delay(debounceDelay);
downLast = false;
}
}
// Upshift Sequence
void shiftUp()
{
if(digitalRead(upSwitch) == LOW && upLast == false)
{
digitalWrite(ignCut, HIGH);
digitalWrite(upRelay, HIGH);
delay(upTime);
digitalWrite(upRelay, LOW);
digitalWrite(ignCut, LOW);
if(gearCount < 5)
{
gearCount ++;
}
Serial.println(gearCount);
upLast = true;
}
else
{
digitalWrite(upRelay, LOW);
}
if(digitalRead(upSwitch) == LOW)
{
delay(debounceDelay);
upLast = false;
}
}
// Neutral Shift Sequence
void shiftNeutral()
{
if(digitalRead(neutralSwitch) == LOW && neutralLast == false)
{
digitalWrite(downRelay, HIGH);
delay(neutralTime);
digitalWrite(downRelay, LOW);
gearCount = 0;
Serial.println(gearCount);
neutralLast = true;
}
else
{
digitalWrite(downRelay, LOW);
}
if(digitalRead(downSwitch) == LOW)
{
delay(debounceDelay);
neutralLast = false;
}
}