Arduino Mega & Trinket Code
Arduino Mega
#include <Wire.h>
#include <PID_v1.h>
#include <PID_AutoTune_v0.h>
#define pwm 12
#define throttlepin 5
#define setpin 6
#define led 13
#define trig 11
#define echo 10
#define onled 22
#define cruiseled 23
#define distancewarnled 24
#define ebrakeled 25
#define easymodeled 26
#define easymodepbutton 27
double Kp = 0.1;
double Ki = 0.1;
double Kd = 0.0;
double dKp = 1;
double dKi = .1;
double dKd = 0.1;
double min = 1500;
double max = 2000;
volatile int prev_time = 0;
volatile int prev_distime = 0;
int rxsig;
int setsig;
int cruisesig;
int temprxsig;
int tempsetsig;
int tempdistance;
int ebrakedistance = 75;
int distancewarn;
int easymodelimit = 1630;
int easymodereverselimit = 1300;
double cruisespeed;
double pwmamount;
unsigned long rawdistance;
double distance = 500;
double safedistance = 150;
int cruisespeedwdist;
double cruisespeedwspeed;
int wheelspeed;
int wheelspeedsum;
byte setbutton;
byte setbuttonold;
byte cruiseset;
byte ebrake;
byte speedcount;
byte speedcounttemp;
byte avgcount;
byte easymodeon = 1;
double avgwheelspeed;
byte direction;
unsigned long prevmillis;
unsigned long speedcycle;
unsigned long easymodemillis;
unsigned long looptime;
unsigned long safedismillis;
byte sregRestore;
PID speedPID(&avgwheelspeed, &pwmamount, &cruisespeed,Kp,Ki,Kd, DIRECT);
PID distancePID(&distance, &pwmamount, &safedistance,dKp,dKi,dKd, REVERSE);
void setup()
{
Serial.begin(9600);
Wire.begin();
pinMode(throttlepin, INPUT);
pinMode(setpin, INPUT);
//pinMode(18, INPUT);
pinMode(led, OUTPUT);
pinMode(echo, INPUT);
pinMode(trig, OUTPUT);
//pinMode(easymodepbutton, INPUT);
pinMode(easymodeled, OUTPUT);
pinMode(onled, OUTPUT);
pinMode(cruiseled, OUTPUT);
pinMode(distancewarnled, OUTPUT);
pinMode(ebrakeled, OUTPUT);
digitalWrite(onled, HIGH);
speedPID.SetMode(AUTOMATIC);
speedPID.SetOutputLimits(min, max);
speedPID.SetSampleTime(200);
distancePID.SetMode(AUTOMATIC);
distancePID.SetOutputLimits(min, max);
distancePID.SetSampleTime(50);
setsig = pulseIn(setpin, HIGH);
if(setsig > 1900)
{setbuttonold = 1;}
if(setsig < 1100)
{setbuttonold = 0;}
distancewarn = safedistance + 100;
//attachInterrupt(4, distanceread, CHANGE);
attachInterrupt(5, speedcal, CHANGE);
attachInterrupt(4, easymode, FALLING);
}
void loop()
{
//For When the Sensor Glitches and return 0
if(distance == 0)
{distance = 500;}
Serial.print("distance:");
Serial.println(distance);
if((millis() - speedcycle) > 25)
{
wheelspeed = speedcount;
speedcount = 0;
speedcycle = millis();
avgcount += 1;
wheelspeedsum = wheelspeed + wheelspeedsum;
}
if (avgcount == 5)
{avgwheelspeed = (wheelspeedsum/5);
wheelspeedsum = 0;
avgcount = 0;
}
Serial.print("avgwheelspeed:");
Serial.print(avgwheelspeed);
double gap = abs(cruisespeed-avgwheelspeed); //distance away from setpoint
if(gap<4)
{ //we're close to setpoint, use conservative tuning parameters
speedPID.SetTunings(Kp, Ki, Kd);
}
else if ((gap>4) && (gap<8))
{
//we're far from setpoint, use aggressive tuning parameters
speedPID.SetTunings(.5, 0.5, 0);
}
else
{
//we're far from setpoint, use aggressive tuning parameters
speedPID.SetTunings(2, 1, 0);
}
if(avgwheelspeed < 3)
{speedPID.SetTunings(4, 1, 0);}
//Determines Cruise control behaviour
if(cruiseset == 1)
{
digitalWrite(cruiseled, HIGH);
if(rxsig < 1400) //Turns off cruise control if brake
applied
{
cruiseset = 0;
digitalWrite(distancewarnled, LOW);
}
else if(rxsig > pwmamount) //Allows car to accelerate even
when cruise control is set
{
pwmamount = rxsig;
digitalWrite(distancewarnled, LOW);
}
else if(distance < distancewarn) //Switches to Distance PID
to maintain distance
{
digitalWrite(distancewarnled, HIGH);
safedismillis = millis();
distancePID.Compute();
}
else //Sets speed to cruise
speed
{
if((millis()-safedismillis) <2000)
{speedPID.SetTunings(0.1, 0.1, 0);}
speedPID.Compute();
cruisespeedwdist = pwmamount;
digitalWrite(distancewarnled, LOW);
distancePID.SetOutputLimits(min, cruisespeedwdist);
//Max speed in Distance PID mode
}
}
else
{
digitalWrite(cruiseled, LOW);
digitalWrite(distancewarnled, LOW);
cruisespeedwdist = 0;
pwmamount = rxsig;
}
setsig = pulseIn(setpin, HIGH); //Read Signal of The Cruiseset button
rxsig = pulseIn(throttlepin, HIGH); //Reads Signal of Throttle Trigger
//Button Toggle for setting cruise control
if(setsig > 1900)
{setbutton = 1;}
if(setsig < 1100)
{setbutton = 0;}
if(setbutton != setbuttonold)
{
if(cruiseset == 0)
{
cruiseset = 1;
cruisespeed = avgwheelspeed;
cruisespeedwdist = rxsig;
cruisespeedwspeed = rxsig;
pwmamount = rxsig;
}
else if (rxsig > pwmamount)
{
cruiseset = 1;
cruisespeed = avgwheelspeed;
cruisespeedwdist = rxsig;
cruisespeedwspeed = rxsig;
pwmamount = rxsig;
}
else
{cruiseset = 0;}
}
setbuttonold = setbutton;
Serial.print("cruisespeed:");
Serial.print(cruisespeed);
//Emergency Braking
if(ebrake == 1)
{if ((rxsig < 1525) && (avgwheelspeed < 3))
{ebrake = 0;
digitalWrite(ebrakeled, LOW);}
else
{pwmamount = 1500;}
}
else if((distance < ebrakedistance) && (avgwheelspeed > 4))
if(pwmamount > 1470)
{cruiseset = 0;
ebrake = 1;
avgwheelspeed = 0;
Wire.beginTransmission(4); // transmit to device #4
Wire.write(165); // sends pwmamountbyte
Wire.endTransmission();
delay(60);
Wire.beginTransmission(4); // transmit to device #4
Wire.write(100); // sends pwmamountbyte
Wire.endTransmission(); // stop transmitting
digitalWrite(ebrakeled, HIGH);
delay(1000);
}
//Easy Mode Limits max speed of Car
if(easymodeon == 1)
{digitalWrite(easymodeled, HIGH);
max = easymodelimit;
if(pwmamount >easymodelimit)
{pwmamount = easymodelimit;}
else if (pwmamount < easymodereverselimit)
{pwmamount = easymodereverselimit;}
}
else
{digitalWrite(easymodeled, LOW);
max = 2000;}
//Send PWM info to trinker to create signal
Wire.beginTransmission(4); // transmit to device #4
Wire.write(int(pwmamount / 10)); // sends divides by 10 to fit in byte
Wire.endTransmission(); // stop transmitting
//Generate pulse for Distance Sensor
if((millis() - prevmillis) > 25)
{
digitalWrite(trig, HIGH);
delayMicroseconds(20);
digitalWrite(trig, LOW);
rawdistance = pulseIn(echo, HIGH, 30000); //Measesures
distance time
distance = abs(rawdistance/ 58); //To prevent negative values
when sensor glitches
prevmillis = millis();
}
}
void speedcal()
{speedcount = speedcount + 1;}
void easymode()
{if((millis()-easymodemillis) > 2000)
{easymodemillis = millis();
if(easymodeon == 1)
{easymodeon = 0;}
else
{easymodeon = 1;}}
}
Trinket
#include <TinyWireS.h>
#define pwm 1
int pwmamount = 1500;
int pwmamountbyte = 150;
unsigned long prevmicros;
// the setup routine runs once when you press reset:
void setup() {
// initialize the digital pin as an output.
pinMode(pwm, OUTPUT);
TinyWireS.begin(4);
noInterrupts();
}
// the loop routine runs over and over again forever:
void loop() {
if(TinyWireS.available() > 0)
{pwmamountbyte = TinyWireS.receive();
pwmamount = pwmamountbyte * 10;}
if((micros()-prevmicros) > 20000)
{noInterrupts();
prevmicros = micros();
digitalWrite(pwm, HIGH);
delayMicroseconds(pwmamount);}
digitalWrite(pwm, LOW);
interrupts();
}
