DIY adaptive cruise control

I hesitate to even mention it, but it is so cool...My background in Avionics and autopilots have given me the confidence to try this, it is not for everybody and no substitue for driving your car. I guess you could damage your car or get yourself killed...but for me at every step of the way I have been suprised at my own success, and how well it works. I have spend around $350 on this project but very many hours tinkering. I found a good used 50m rangefinder on ebay for $175, but normally they are closer to $400.

On my 2008 Prius I have mounted a laser rangefinder (50m range) in the grill, connected to an Arduino Uno with 5 relays under control. The arduino code activates relays to CC set, resume, accelerate, coast based on whether approaching or separating from traffic.


One of the hardest parts was getting the rangefinder to point straight...

i have been testing the system since November '15 and it ia really working good from 24 mph up to 89 mph.

The arduino is under the drivers seat, and the 2 switch controls are mounted in front cup holder.

If I can do it, why isnt this readily available as aftermarket mod for all CC equipped cars? I wonder if liability is too great...IMHO we only live once, and this has been a lot of fun putting together. -BG

 

I thought that might be a problem too. I am using the Parallax SF02/f laser, which is the weakest type. For example, the risk to the human eye is only if you look at beam through an optical instrument such as a binocular or telescope. The beam is invisible, I guess because it is pulsing no less than 30 times a second...maybe also because of its spectrum. The laser works very well in bright sunlight, at night, and in light rain. In my experience, fog is its biggest enemy.

Thanks for your interest. The Prius Cruise Control manual switches on the stearing column use a bank of resistors in parallel, to send switch actuation info the the cars computer, for example at 12V, 10.5V, 9V, 7.5V, and 0.0VDC for example. So for my Arduino Uno R3 to mimic the mechanical switches on the stearing column, I spliced two wires into the stearing column with a quick disconnect connector (I did this early on so I could yank the wires away if I ever had an unexpected problem. In practice this was never necessary).

So the only electrical connection directly to the Prius are the two wires into the stearing column, and the 12V/GND from the cigarette lighter pot.

The laser is a Parallax SF02/F that I mounted into an approxmately 5" x 2.5" x 1.25" aluminum box from Fry's Electronics. I cut a small piece of glass to protect the laser glass, and RTV'd it into the front of the aluminum box to protect the SF02/f from the elements.

I used cat-5 network cable through the firewall to the laser sensor, providing switched +5VDC, GND, and serial data connections.

After 29 revisions of my arduino code I am really liking its operation, and miss it when I drive any other car. The ardunino code is looking for at least 35M seperation. If this isn't the case, the CC goes into COAST mode. Once 35M is obtained (with hystersis factored in), the CC is automaticall switched to RESUME. If the laser/arduino senses seperation beyond, I think it was 38M, then the CC is automatically switching to ACCEL. So in theory the CC will go as fast as car in front of you. When the system senses a car closing in rapidly from 50M or less, it automically goes to COAST mode to slow down (hitting brake always cancels CC).

I installed two swiches in a cardboard tube with end caps, the size of a coffee cup. This is in my front cupholder. The first switch turns off the laser sensor, and the second switch allows normal CC operations (ignore Arduino), and Active postion which makes adaptive cruise control work.

Let me know if you need more. I am at a conference this week so have limited ability to post photos. My wiring diagrams are chicken scratch, so to share I would have to improve. Anyone in the N. Dallas area is welcome to check in with me for a demonstration.

It was pretty funny the first time I demonstrated it working to my wife, she said, "I never thought it would work".

 

An update since my last post -

Several months went by without any software changes to my Prius DIY adaptive cruise control. The hardware has been very sturdy during the hot Texas summer. So far I haven't had to re-position the sensor, a pleasant surprise. That tells me my mount in the grill is correct. In the meantime, I have given much thought to software/logic improvements. I didn't like that the Arduino was programmed to not accelerate at all unless the car in front separated past 35 meters, since I would never "lock on" to a quickly accelerating vehicle before it went out of range. I am happy to say I figured out a neat solution, which seems obvious in hindsight. As long as the next vehicle is accelerating faster than me, distance is increasing at least 20 centimeters between each measurement, then it is OK if I am also accelerating (RESUME+ relay activated). I guess this is very simple for an experienced programmer, but for me it was a real "Eureka" moment. Lately I have been really amazed at how well it works, I use it over 90% of the time on my daily 2 hours of commuting. It is really amazing, no joke.

 

This is a shared response to a private request -

Thanks for your interest in this subject, it is one I have been passionate about for some time.

Use this information at your own risk. For me it is working great. Now it has been over a year since I installed, but to the best of my memory:

The rangefinder is model SF02/f from Lightware distributed by Parallax. I mounted a OSEPP 16x2 Arduino-comptaible shield for the display. For the relays, I am using Velleman I/O Shield for Arduino, Fry's Electronics #: 7726598. So I snapped the 16x2 display shield onto the Arduino Uno R3, then hard wired the Velleman shield to the opposite side of the aduino making a "clump" of electronics. To this day, the Arudino clump is under my drivers seat housed in a primitive Lego-brick box (it works great). The combo of the Arduino, Relays, and 2X16 Shield is connected to a 25-pin D-sub connector for quick install/removal. Also I needed a prototype board to mount three potentiometers. These potentiometers are setting up the voltage drops across relay-switched resistors to mimic the operation of the CC switch on the steering column. My memory is that 0 VDC cancels CC, +7.5 is Set/Coast, +9.0 VDC is Acc+/Resume, 12V is do nothing (or something to that effect). You have to connect a voltmeter to the car's CC control wires and verify for yourself. Then, your Arduino when automatically doing the switching, should be putting a potentiometer resistance into the circuit matching the ECU expected voltage drop. This is not as hard as it sounds, it worked for me the first time.

From under the drivers seat the Arduino "clump", I have wires going to:

2 wires to the steering column, soldered in parallel with the the ground and switch wire coming from CC switch. Yes, this does mean the system will act weird if Arduino is giving CC command at the same time you hit the steering wheel CC switch, it seems to always result in CC being cancelled. This is a small area for improvement, ideally there should be some isolation between the Arduino relays and the CC switch. I used a small connector under the steering column that I could quickly pull apart if there was ever an unexpected response from system (this has never happened, but I guess is a remote possibility) You would have to look up in a Prius wiring diagram which are the correct wires, my memory is that one was red and one was black, but you have to check to be sure. Rather then cut the cars CC control wires, I used a razor to remove some insulation, then I soldered my 2 wires, then covered with electric tape.

4 wires going to two switches in the front cup holder, one to activate relay to engage +5v to laser rangefinder, and the 2nd switch to go between "Norm" CC and "Adaptive CC".

4 wires going through the firewall to the range finder. I used CAT5 cable. This is the serial wires (2 wires) and +5v and ground needed at the laser rangefinder. I route these wires under the drivers floormat.

2 wires going to Cigarette lighter port, 12V & ground is needed to drive the Velleman relays, if my memory is correct, this is also used by Arduino regulated down to +5v by Arduino for laser rangefinder.

My arduino code is sloppy, I am not a professional programmer. There may be variables in the code that are not used. Here is my current arduino code (most of the remarks should be accurate, but after many revisions some may be in error or obsoleted):


/*
Adaptive Cruise Control Experiment

Wiring Diagram:

Pin_GND #2 On SF02 Laser Rangefinder - GND (Vss On Some Boards)
Pin_+5V #3 On SF02 Laser Rangefinder - +5V (Vdd On Some Boards)
Pin_TXD #8 On SF02 Laser Rangefinder - Arduino RX Pin (2)
Pin_RXD #9 On SF02 Laser Rangefinder - Arduino TX Pin (3)

*/


#include <Arduino.h>
#include <avr/wdt.h>
#include <SoftwareSerial.h>
#include <LiquidCrystal.h>
LiquidCrystal lcd(8, 9, 4, 5, 6, 7);


#define serial_Rxd 2 // These pin definitions can be changed to match your wiring
#define serial_Txd 3
#define sf02_baud_rate 9600 // SF02 baud rate (Can be changed, refer to the SF02 manual)
int autoTrack;
int switchPOSA;
int switchPOSB;
int onOff = 1; //1 is laser off, 0 is on;
int oldOnOff = 0; //record of onOff prior to switch change
int goAgain = 0; //in coast mode, 0 means ok to SET- for new speed, 1 means don't do it again.
float distance = 0.0;
float distance2 = 0.0;
float rate = 0.0;
int standardDelay = 175;
bool trackDistance; //0 is for highway, far tracking and 1 is for boulevards for closer tracking
int mode = 0; // mode A=0, mode B=1, mode C=2, mode D=3, mode E=4 , mode Y=5, mode Z=6

SoftwareSerial sf02_serial(serial_Rxd, serial_Txd); // The pins used for the second serial port
char sf02_string[32], c; // Search the ASCII string from the SF02 using the second serial port

void setup()
{
wdt_enable(WDTO_2S); //set watchdog timer to 2S, if passing wdt_reset(); for more than 2 seconds, reset device.
pinMode(13, OUTPUT);
pinMode(12, OUTPUT);
pinMode(A1, OUTPUT);
pinMode(A2, OUTPUT);
digitalWrite(A1, HIGH); //A1 is turn relays laser on/off toggle
digitalWrite(A2, LOW); //A2 is set- relay

digitalWrite(12, LOW); //12 is CC resume+
digitalWrite(13, LOW); //13 is CC Cancel
lcd.begin(16, 2);
lcd.clear();
lcd.print("ACC Rev 39");
delay(1250);
wdt_reset();
delay(1250);
wdt_reset();
lcd.clear();
sf02_serial.begin(sf02_baud_rate); // Open the second serial port to connect to the SF02
lcd.clear();
}
void loop()
{
wdt_reset();
switchPOSA = analogRead(A4); //switchPOSA down,turn 5V off to rangefinder
if (switchPOSA < 500) {
onOff = 1;
}
else if (switchPOSA >= 500) { //940 (>900) is HIGH (switch in up position), rangefinder is on.
onOff = 0;
}
switchPOSB = analogRead(A3); //switchPOSB down, no auto CC speed set & only resume and cancel control, up fully auto tracking permitted
if (switchPOSB < 500) {
autoTrack = 0;
}
else if (switchPOSB >= 500) {
autoTrack = 1;
}

delay(25);
if ((oldOnOff == 1) && (onOff == 0)) { //onOff switch moved from Off position to On position or startup
digitalWrite(A1, HIGH);
lcd.clear();
lcd.print("Powering up...");
delay(1500);
wdt_reset();
delay(1200);
wdt_reset();
lcd.clear(); //initialize rangefinder
oldOnOff = 0; //make sure this only happens once
}
if ((oldOnOff == 0) && (onOff == 1)) { //onOff switch moved from On to Off position
lcd.clear();
digitalWrite(A1, LOW);
delay(250);
oldOnOff == 1; //ensure this only happens once
}

if ((oldOnOff == 0) && (onOff == 0)) {
sf02_serial.write("d"); // Send "D" trigger to receive data back from sensor
while (!sf02_serial.available()); // Wait until the next distance measurement is ready
// Prepare to read the serial port...
int i = 0; // indexer for the string storage variable
int c = 0; // c holds the latest ASCII character from the SF02

while (c != 10) // Read the ASCII string from the SF02 until a line feed character (\n) is detected
{
while (!sf02_serial.available()); // Wait here for the next character
c = sf02_serial.read(); // store the values in c
sf02_string = c; // Add the character to the existing string from the SF02
i++; // Add the next character, until full string is captured
} // Once the string has been captured...
sf02_string[i - 2] = 0; // Create a null terminated string and remove the \r\n characters from the end
float distance2 = atof(sf02_string);
rate = (distance2 - distance);
lcd.clear();
lcd.setCursor(0, 0);

if (autoTrack == 0) {
lcd.print("normal_cruise_c");
digitalWrite(13, LOW); //release Resume+
delay(50);
digitalWrite(A2, LOW); //release Set-
delay(50);
digitalWrite(12, LOW); //release Cancel
}
else if (autoTrack == 1) { //this routine will maintain a gap by speeding up and slowing down proactively, as long as signal is clear
lcd.print("auto_accelerate_");

mode = 0;
if (distance < 0.10) { //mode is 0 if no laser report
mode = 0;
}
if (rate < -15.0) { //mode is 0 if someone pulls in front (they are likely separating) and ignore lose of signal
rate = 0;
}
if (rate > 15.0) { //mode is 0 if someone pulls in front (they are likely separating) and ignore lose of signal
rate = 0;
}


if (rate < -0.05) { //approaching
if(distance > 32.0) { // if distance is under 34M, and approaching is pretty quick, then SET-
if (rate < -0.20) {
mode = 1; // CC -SET as rate slows, car is coming closer, makes sense to hit -SET to slow further and get ready to resume
}
else if (rate > -0.20) { //if between -0.05 and -0.155, then don't activate a relay, let car come closer
mode = 0;
}
}
if(distance < 32.0) { // if distance is less than 34.0M, then SET-, really don't need to be so close.
mode = 1;
}

}
if (rate > 0.05) { //separating
if(distance < 34.0) { // if distance is under 34M, and separation is reasonable, was .2, seemed too slow, changed to .25
if (rate > 0.30) {
mode = 2;
}
else if (rate < 0.30) { //if between 0.06 and 0.25, then coast -SET, need to back off before speeding up
mode = 1;
}
}
if(distance > 34.0) { // if distance is over 34M, and any separation detected, resume
mode = 2;
}

}






lcd.setCursor(0, 1);
lcd.print(mode);
lcd.setCursor(3, 1);
lcd.print(rate);
lcd.setCursor(9, 1);
lcd.print(distance);
switch (mode) {
case 0:
digitalWrite(13, LOW); //release Resume+
delay(50);
digitalWrite(A2, LOW); //release Set-
delay(50);
digitalWrite(12, LOW); //release Cancel
delay(standardDelay);
break;
case 1:
digitalWrite(12, LOW); //release Resume+
delay(50);
digitalWrite(13, LOW); //release Cancel
delay(50);
digitalWrite(A2, HIGH); //activate SET- relay to start slowing down
delay(standardDelay);
break;
case 2:
digitalWrite(13, LOW);
delay(50);
digitalWrite(A2, LOW);
delay(50);
digitalWrite(12, HIGH); //activate resume+ to speed up
delay(standardDelay);
break;
}
distance = distance2;
}
}
else if ((oldOnOff == 1) && (onOff == 1)) {
digitalWrite(13, LOW); //relax Cancel switch to enable manual CC on/off
delay(50);
digitalWrite(12, LOW); //release Resume+
delay(50);
digitalWrite(A2, LOW); //release Set-
delay(50);
digitalWrite(A1, LOW); //release rangefinder power relay
delay(50);
lcd.setCursor(0, 0);
lcd.print("Laser and CC");
lcd.setCursor(0, 1);
lcd.print("Relays: OFF");
delay (standardDelay);
}
oldOnOff = onOff;
}