Self driving car – Intro

For the past 8 months, I have designed, built, programmed, and tested from scratch a Lidar based active cruise control system for my Mazda6, pictured above. This level 1 autonomous system maintains a constant distance to the vehicle ahead, similar to systems available on many modern vehicles. I call my system Lidar Cruise. Below I will detail its basic workings, everyday operation, and a build section showing major milestones and pictures.


So how does the driver use this system? Everyday operation is very straightforward. After turning on cruise control, the driver presses the Lidar Cruise button next to the wheel. The system maintains distance.

If the brake is pressed, the cruise control turns off and Lidar Cruise waits, but still remembers its set speed. When the driver sets cruise control at whatever speed, Lidar Cruise brings the vehicle back up to the Lidar Cruise set speed and maintains distance.

If the driver wants to adjust set speed while Lidar Cruise on, they can simply press the steering wheel buttons.The Lidar Cruise button does not have to be turned off unless the driver wants to change set speed quickly, as this would require many button presses.

In all, Lidar Cruise operates much like a normal system, integrating fairly seamlessly into the existing cruise control.


How Does it work?

At the core, the system functions by connecting to the current cruise control system in my vehicle by way of the steering wheel buttons. By pressing the ACCEL and COAST buttons, basic control of vehicle speed is achieved. Although this sounds simple, in order to safely make decisions, 4 different Arduino microprocessors gather data from a wide array of sensors: Lidar, photogate, photodiode, Bluetooth, and voltage dividers. I will trace the flow of data through the system to give an idea how it “thinks”.

It is important to note how the system is controlled. Arduino 2, 4, and the Bluetooth module are always powered on when the car is running. This allows their functionality at all times. Arduino 1 and 3 are powered on by a switch next to the steering wheel.


First, a note on Safety

From conceptualization of this project, the priority was safety. Several weeks were spent to make sure the idea was viable to create without unnecessary danger. To this end, Lidar Cruise does not touch several key cruise control and vehicle safety systems. First, touching the brake still disarms the cruise control system, allowing slowing of the vehicle. Second, Lidar Cruise does not integrate with the brakes, leaving this safety system intact. Finally, Lidar Cruise does not integrate with the transmission, allowing shifting to neutral as a last resort if problems occur.

In addition, Lidar Cruise only sends commands when the driver has manually activated the cruise control system, as well as physically sent power to the system through a switch. Finally, as Lidar Cruise is only a level 1 autonomous system, the driver is ultimately responsible for the vehicle at all times.


Arduino 1

(Switch powered)

The first Arduino’s job is to control a stepper motor, on top of which a Lidar unit is mounted. A photogate mounted next to the motor is triggered as the motor turns, providing a start position. The motor then rotates 60 degrees and reverses direction, completing a sweep back and forth every 2/3rds of a second. A second photogate acts a a backup, stopping the entire system if it rotates too far. This prevents the wires attached to the Lidar from catastrophically twisting. At the beginning of each cycle, this Arduino sends a command to Arduino 3, acting as a clock to keep the system synchronized.

Lidar sensor mounted to motor. A photogate can be seen in white at the bottom.

Custom made mounting hardware for the motor. At the top of this image are the Arduino and motor controller. The hardware slides into the tube at the bottom.

Lidar hardware mounted safely in its waterproof enclosure. Aluminum brackets secure the hardware below the crash beam in the front bumper.

Arduino 2

(Always on)

The second Arduino is wired behind the radio and completes a simple task, reading the vehicle speed wire, or VSS. This data is sent when requested to Arduino 3.

 Arduino 2 behind the radio. Note the USB power connection, which is plugged into a 12V socket. This also allows easy reprogramming of the Arduino without tearing out the radio

Arduino 3

(switch powered)

The third Arduino acts as the central brain, receiving data from a multitude of sensors. It is triggered to cycle once through all commands by the first Arduino. When triggered, Arduino 3 first gathers data from the Lidar during a sweep in one direction. As the motor cycles back the other direction, all other processing below is completed. From the Lidar, an array of distances and times since start is created. Since the motor rotates at a known rate, this can be translated to polar (distance and angle) and xy coordinates for easier processing.

Next, speed data from the vehicle is gathered from Arduino 2. Since Lidar Cruise works by pressing the ACCEL and COAST buttons, from the vehicle’s point of view, the cruise control set speed is constantly changing. To move the cruise control back to a set speed when no obstacles are present, Lidar Cruise needs the vehicle’s current speed and a Lidar Cruise set speed. When the system is first powered on, the initial speed is used as a Lidar Cruise set speed, additional readings are used as current speed.

The next check is simple, but vital to the safety of the system. On my Mazda6, the cruise control system consists of four buttons. First is ON/OFF, which arms the system. Second is SET/COAST. When the system is armed, this button sets the speed. When the system is set, this coasts the car. The third button is RES/ACCEL. When the system is armed, it resumes the previous set speed. When the system is set, it accelerates the car. The dual nature of buttons 2 and 3 present an issue if the Lidar Cruise system is not aware if the system is armed or set. Take the following case when the driver taps the brake, changing the cruise from set to armed. In this case, Lidar Cruise could press the ACCEL button, but the cruise control interprets this press as RES, re-setting the set speed. The driver must brake again, or continuously brake to stop this from occurring. Since this is a dangerous feedback loop, Lidar Cruise must not react unless the cruise control is set. To accomplish this, a simple photodiode is wired into the instrument cluster next to the CRUISE LED. From this, Arduino 3 can read if the cruise control is in the proper state to react.

At this point, Arduino 3 has enough information to begin making decisions. Through a complex equation, Lidar Cruise takes into account current speed, set speed, distance to obstacle, obstacle shape, and closing speed. Lidar Cruise sends one of six commands to Arduino 4: nothing (if cruise control is off), no reaction, short press accel, short press coast, long press accel, and long press coast.

Arduino 3 finishes is commands by sending data over Bluetooth to an custom android app on my phone. During test drives, this allows me to see distance, speed, and Lidar Cruise’s reaction.

Left to right: Bluetooth, Arduino 3, power source / surge protector (number 1). Arduino 4 and its components are mounted below. The V shaped structure slides into the tube at the top.

The waterproof tube mounted behind the crash bar. The Lidar, motor, and Arduino 1 are mounted in another tube below this structure.

The instrument cluster circuit board. The cluster of LEDs on the far left include the CRUISE LED

The photodiode mounted to the plastic cover over the circuit board,

Arduino 4

(always on)

The fourth Arduino controls the interaction with the current cruise control hardware on the vehicle. The cruise control buttons on the steering wheel are a simple set of resistors, with two wires connected. By cutting the wires and connecting the steering wheel side to Arduino 4 with a voltage divider, Lidar Cruise can read which button is being pressed. The ECU side of the wires is connected to a set of 4 relays with different resistors controlled by Arduino 4. Thus, Arduino 4 can read from the steering wheel and write to the ECU acting as the original steering wheel buttons.

Arduino 4 receives commands from Arduino 3, placing Arduino 4 in one of two modes.

If the system has received a command recently, only the ON/OFF and CANCEL steering wheel buttons are passed through to the relays. The ACCEL and COAST relays are controlled based on which of the five commands Arduino 3 sent. Any driver inputs on the steering wheel ACCEL and COAST buttons are sent as commands back to Arduino 3, adjusting the Lidar Cruise set speed that was initially decided when Arduino 3 was powered on.

If the system has not received a command from Arduino 3 in >2/3rd of a second, Arduino 3 must be off or cruise must be off. In this state, any steering wheel buttons presses are directly passed through to the relays. In this way, the driver does not even realize Lidar Cruise is installed.


From left to right: Power source / surge protector (number 2), Arduino 4, IMU (not used), relays, resistors

Android APP

As mentioned in the Arduino 3 section, I built an Android app that allows me to see what the Lidar Cruise is thinking. This app, although not required to run, proves invaluable during test drives.



Buildlog / Changelog

As this project was completed on my (only) personal vehicle and while away at college, I only could work on this project on weekends when I traveled home. In addition, I couldn’t disassemble the car too much as I had to drive back and forth. This lengthened the build time.

Early October 2016

Initial brainstorming and conceptualization of Lidar Cruise. Planning of connection to vehicle and decision logic completed. Components needed to build chosen. Initial code written in Matlab to test if range of Lidar hardware is sufficient.

Lidar simulation in Matlab

Late October

Lidar and motor hardware constructed (Arduino 1). First design consisted of 45 degree rotating mirror, with Lidar hard mounted to frame.

First version of Arduino 1, Lidar, and motor


Planning and purchasing of hardware needed to build.

December – Early January

Majority of build completed during winter break. Car disassembled, including bumper, wheel, fender liner, and instrument cluster. Hardware for Arduino 1 installed. Hardware for Arduino 3 and 4 built and installed. Before leaving to go back to college, three major problems remain.

First, Lidar cannot see through plastic window mounted to the curved PVC pipe. At short (<10 ft) distances objects are seen, but at longer distances, the Lidar reads the distance to the window.

Curved plastic window covering Lidar hardware


Second, GPS (originally planned to use GPS speed) cannot reliably read through the structure.

Far lower left: GPS unit

Third, alignment of the system is tricky, and currently pointed way too low.

Problems 2 and 3 were somewhat expected, with some idea how to fix. Problem 1 was very tricky, taking many different attempts to fix.

Mid January

 First attempt to fix problem 1 involved reprogramming of the Lidar to ignore short distances and only focus on long distance. Proved impossible to complete and gather data at the same rate as before. Second attempt involved the creation of a first surface mirror, where the signal does not pass through glass before being reflected.  This created a small improvement, but not to the distance needed to work.

Creating a first surface mirror by stripping coating from normal mirror with acetone

Third attempt involved the elimination of the mirror and the mounting of the Lidar directly to the motor. This necessitated a change in motor logic, as the motor cycles back and forth instead of rotating to avoid twisting the wires on the Lidar unit. Again, not enough improvement.

Lidar mounted directly to motor for the first time

At this point, it is clear that the curved window must change. Through trial and error, it is discovered that a flat piece of plastic held at and angle allows the Lidar to look ahead.

Late January

In order to keep the system waterproof, an adapter that holds a window and connects to the PVC pipe is modeled in Blender. This was printed on a 3D printer while back at college. Problem 1 is solved.



Adapter in the process of 3D printing

Adapter attached to PVC pipe and window


Problem 2, GPS speed, could be solved in two ways. First, since the system already has Bluetooth, an app that sends GPS speed from a phone to the system could be written. This had the advantage of requiring no additional hardware. However, Bluetooth is flaky at best and requires a phone connection to let Lidar Cruise operate. Option two was used instead, which involved tapping into the VSS wire behind the radio. Arduino 2 was installed and sent data using wireless transmitters to the front … which was quickly scrapped as the engine block gets in the way of sending signals to the front bumper. This problem is solved in April.

Arduino 2 with wireless attachment, later removed


Downtime due to a spring break trip to Dubai. Check out my photography section for some awesome pictures of the region.


New wires are routed from Arduino 2 to the front, creating a reliable wired speed source for Lidar Cruise. This is much better than the wireless connection from February.

Additional wires (red and white) used for Arduino 2 connection, sending speed data to Lidar Cruise


Problem 3, alignment, was attempted to be solved several times by tweaking the angle that the Lidar is mounted on the motor. However, the system was still highly sensitive to pointing too high or low, especially at max range. An additional support, attached to a turnbuckle, allows fine tuning of alignment easily and quickly, solving the final problem. Lidar Cruise is complete!

Additional support, with turnbuckle on the right allowing for fine tuning of alignment