Thank you for your suggestions! I have changed the code to have the correct safety features and have looked up PID tuning. I have tried the manual tuning method described on Wikipedia with some success. It no-longer jitters, but it takes a while to reach the set point sometimes. I have tried to use the PID Autotune program, but I can't seem to get it to work. Here is my code so far:
/*
PID_Basic_Throttle.ino
Bosch electronic throttle body control
for MSOE SAE Formula Hybrid 2014
Author: Austin R. Bartz
Last Revision Date: 4/3/2014
Hardware Connections:
-L298N H-Bridge Enable A: Pin 9
-L298N H-Bridge Input 1: Pin 8
-L298N H-Bridge Input 2: Pin 11
-TPS 0: Pin A0
-TPS 1: Pin A1
-Throttle Input: Pin A2
*/
//H-Brdige Pins
#define pinI1 8
#define pinI2 11
#define speedPin 9
//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);
//TPS0 Idle= 863 WOT= 56
//TPS1 Idle= 164 WOT= 969
void setup()
{
TCCR1B = TCCR1B & 0b11111000 | 0x01;
int TPS0 = -1*(analogRead(0)-862);
int TPS1 = (analogRead(1)-163);
int TPS = (TPS1 + TPS0)/2;
if(abs(TPS0 - TPS1) >= 5)
{
analogWrite(speedPin,0);
}
//initialize the variables we're linked to
Input = TPS;
Setpoint = map(analogRead(3), 85, 1023, 0, 800);
//turn the PID on
myPID.SetMode(AUTOMATIC);
pinMode(pinI1,OUTPUT);
pinMode(pinI2,OUTPUT);
pinMode(speedPin,OUTPUT);
digitalWrite(pinI2,LOW);
digitalWrite(pinI1,HIGH);
Serial.begin(9600);
}
void 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);
myPID.SetTunings(0.15, 2.00, 0.00);
int TPS0 = constrain(-1*(analogRead(0)-867), 0, 810);
int TPS1 = constrain((analogRead(1)-153), 0, 810);
if(abs(TPS0 - TPS1) >= 10)
{
analogWrite(speedPin,0);
}
int TPS = (TPS1 + TPS0)/2;
Input = TPS;
int TP0 = map(analogRead(3), 85, 1023, 0, 800);
int TP1 = map(analogRead(2), 555, 1023, 0, 800);
int TP = ((TP0+TP1)/2);
int TPdiff = abs(TP0-TP1);
while(TPdiff >= 5)
{
analogWrite(speedPin,0);
int TP0 = map(analogRead(3), 85, 1023, 0, 800);
int TP1 = map(analogRead(2), 555, 1023, 0, 800);
TPdiff = abs(TP0-TP1);
}
Setpoint = TP0;
if(Setpoint <= 5)
{
analogWrite(speedPin,0);
}
else
{
myPID.Compute();
analogWrite(speedPin,Output);
}
Serial.print("TPS0:");
Serial.print(TPS0, DEC);
Serial.print(" ");
Serial.print("TPS1:");
Serial.print(TPS1, DEC);
Serial.print(" ");
Serial.print("Input:");
Serial.print(Input, 0);
Serial.print(" ");
Serial.print("Pot0:");
Serial.print(TP0);
Serial.print(" ");
Serial.print("Pot1:");
Serial.print(TP1);
Serial.print(" ");
Serial.print("Setpoint:");
Serial.print(Setpoint, 0);
Serial.print(" ");
Serial.print("Output:");
Serial.println(Output, 0);
}
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;
}