Showing posts with label Fast RC Car. Show all posts
Showing posts with label Fast RC Car. Show all posts

Wednesday, September 19, 2012

Lap Timer Build Along Part 3 - Timer Based Transponder

This post continues the RCArduino Lap Timer Build Along Series.

Previous steps can be found here -

Initial build along and videos -
http://rcarduino.blogspot.com/2012/07/lap-timer-build-along-part-one.html

Introduction to the timer based transponder and the required parts list -
http://rcarduino.blogspot.com/2012/08/lap-timer-build-along-part-2-transponder.html

Here are two quick demos of the system in action at the Pro RC Track in Dubai, the first shows the bread board build along lap timer and an enclosed stripboard version. The second video shows external audio enabled for lap time countdowns and fastest lap/not fastest lap indicator tones.




Transponder Overview
The transponder works by generating a simple infrared signal very similar to the one your TV Remote generates. This is an established technique which is widely used at RC Tracks, kart tracks and even automobile race tracks. If your track has an existing IR Transponder you can use the existing signal and do not need to build your own transponder. Part 4 will show you how to do this.


If you plan to build your own transponder read on.

The transponder can be powered in one of two ways -

1) Using dedicated power - If you are using the transponder in an automobile or kart, you can use a 9 Volt battery or 4 AA batteries to power the circuit directly.

2) Using buffered/shared power - The 555 Timer is a notoriously noisy component, if you intent to share power with any sensitive equipment such as an RC Receiver, you will need to buffer the shared power from the 555 timer. A cheap and effective solution is to use a 7805 voltage regulator, the final stages of this post show how to add this component.

RC Car Power
The transponder design has been extensively tested in many cars at the ProRC Track in Dubai. The transponder can easily be connected to the balance plug of a LIPO battery using a 3 pin section of PCB Header or a male balance plug if you have one. When sharing power in this way it is important to use the 7805 regulator circuit to prevent interference with the models RC Systems.

One of the test cars - The current m-chassis lap record holder at the Pro RC Track - lots of carbon, anodized aluminium and threaded oil shocks all around.

 
If you are not using shared power you will not need to complete the final few steps of the build (stop at step10) however if you expect to share power with any sensitive equipment at some point in the future it is worth including these steps.

How does the transponder work ?

The transponder uses the same technique and components as your TV Remote - an Infra Red light emitting diode is switched on and off 38,000 times per second, this is our carrier frequency. The carrier is generated by one of the 555 timers however in order to transmit a signal or data we need to alter the carrier in some way. The transponder uses a very simple signal - a pulse of 500 microseconds, this is generated by the second timer which simply switches the first timer on and off.

In the final part of the build series we will add the IR Detector to our Arduino circuit, the IR Detector is tuned to the 38Khz signal of the transponder. Remember we are using two timers ? One to generate the 38Khz signal and another to pulse it at 1Khz ? This provides a very simple encoding which allows us to look for pulses 500us long and know that we are receiving transponder signals rather than reflected sunlight, TV Remote signals or signals from other track side transponders.


Some Alternative Transponders

The job of the transponder is to 1) Switch the diode on and off at a frequency of 38,000 times a second. 2) To enable and disable 1) At a frequency of 1,000 times a second. 

In order to do this we use two connected 555 timers, the first generates the 38Khz signal and the second one enables and disables this signal at the much lower frequency of 1Khz. 

The transponder circuit can be replaced with any simple circuit capable of generating two fixed frequency square waves. 

Options include microcontrollers, NAND oscillators, op-amp oscillators - anything that can generate a squarewave.



Building The 555 Timer Transponder - Before and After
The transponder can be built on a piece of strip board 16 holes wide by 14 deep. To make sure that this is the final size of you board, remember that the rows and columns you cut will be unusable, you need 16*14 inside the cut area.

The copper strips should be running from left to right across your board - this is the orientation used throughout the build.
The Schematic
The circuit is reasonably simple with only 11 components required for pit wall use and an additional four if you plan to use the circuit in an RC vehicle.

Due to the angle in some of the pictures, its useful to double check your component placement against the schematic, also have a look at pictures from later stages for a cross reference.
1) Placing The 555 Timers
 Place the two timers with the notch facing the top of your circuit. Leave four rows of space above the top timer, one row between the top and the bottom and you should be left with one more empty row below the second timer.

The timers should be placed with six free columns on either side. See the picture for reference.

Important: The copper tracks on your strip board should be running from left to right confirm this before soldering.

2) Add Decoupling Capacitors
Add a 0.1uf ceramic capacitor as a decoupling capacitor across pins 1 and 8 (top left and top right) of each 555 Timer. I have soldered mine directly to pins 1 and 8 to save space.

 3) Add Positive Power
Add the connections to positive power.

When we add the 7805 regulator in a later step, the third row (counting from the top) will be our 5 volt power supply rail. Connect this to PIN 8 (top right) of both 555 Timers.

In this case I am using orange wire for the connections to the 5 volt power rail.

At this point we can also connect PIN 4 (bottom right) of the top 555 Timer to 5 volts. This is the reset pin, it is active low, so by connecting it to +5volts we ensure that the timer is never reset.
4.1) Add Ground Connections
4.2) Add Timing Resistors
Two steps in one here -
1) Add ground connections. Pin 1 (top left) of each 555 timer needs to be connected to the ground power rail. When we add the 7805 regulator, the second row (counting from the top) will be ground, connect this row to Pin 1 of each timer, I have used brown wire for this connection in the picture.

2) Timing resistors -
Now we add the first of our timing components. In order from top to bottom we add a 1K resistor (brown,black,red) from the 5volt power rail (third row down) to pin 7 of the top 555 Timer. Next we add a 10K resistor (brown, black, orange) from the same pin 7 to the row which we left free between the two 555 timers. The final resistor is a 1K (brown, black, red) resistor connected from pin 7 of the bottom 555 timer to the very bottom row of our board.

5) Add Timing Resistors Continued
Next we need to add two more resistors to the second timer.

Connect a 220 resistor (red, red, brown) from pin 8 to pin 7 of the lower 555 timer.

Connect a 680 resistor (blue, green, brown) from pin 7 to pin 7. Yes, thats right from pin 7 to 7, its the blue resistor in the picture and for it to have any effect we need to cut the copper track beneath the resistor. Its also a good opportunity to mention that we need to cut the tracks which would otherwise connect the 555 timer pins left to right. Refer to the picture later in the post.

6.1) Add Timing Capacitors
6.2) Add Timing Connections
Add the 1uf electrolytic capacitor (big black can) between pins 2 and 1 of the top 555 timer. Note that these capacitors are directional, there is a strip with a minus sign printed on it which indicates which side should connect to ground (pin 1) in the picture the strip is facing away from the camera. The other side is connected to pin 2.

Next we connect the 0.01uf ceramic capacitor between pins 1 and 2 of the lower timer. This capacitor is not directional.

Finally we need to add connections between pin 2 and 6 (trigger and threshold) of the timers. In the picture I have used yellow wire for this, it would have been possible to route the wire directly across the chips from pin 2 to 6 however in this case I have chosen to route two wires, one down from pin 6 to the empty row immediately below the chip and one back up from from the empty row to pin 2. Repeat this for each of the two timers.


Electrolytic Capacitor Orientation
Larger value capacitors tend to be available as electrolytic capacitors, these have an appearance something like a can. Electrolytic capacitors are directional, to ensure that they are correctly placed, the cans have a band marked with a minus sign to indicate the side/pin which should be connected to ground.



7) Connect The Timers Together
 This one is easy. We want to connect the output of the first timer (top) to the enable of the second timer (bottom).

This allows the top timer which is running at 1000hz to switch the bottom timer on and off to generate our coded 38Khz signal.

This connection is shown by the white wire connecting pin 3 (output) of the top timer to pin 4 (reset) of the bottom timer.
8) LED Current Limiting Resistor
As with any LED we need to limit the current passing through our infra red LED. As we are pulsing the LED it is only on for very short durations and so we have the option of passing more current through it. In this case I am using a 100 Ohm resistor (brown, black, brown) to limit the current to 50 milli amps, you could use a lower resistor for more power which would give a greater lap detection distance however on a narrow RC Track which loops back on itself a 100 Ohm resistor is perfect.

The 100 Ohm resistor is connected from pin 3 (output) of the lower 555 timer to pin 1 (ground). In the next step we will add the LED and cut the track between pin 3 and the resistor.

9) Double Check
Its a good time to double check progress. Here is the circuit constructed so far shown from the left side, double check yours against this and the previous images.


10) Add The IR LED
Not a great picture, but its an easy step.

Like all LEDs, the infra red ones are directional. This means the LED will only light if connected the correct way. As we cannot see infra red, its worth a little extra explanation to make sure we get this part right.

Most LEDs have on leg which is longer than the other, the long one should be connected to the positive (closest to the chip) and the short to negative (the path to ground is provided by the 100 Ohm resistor). However I have recently bought a batch of LEDs where the legs were cut incorrectly. To double check you LED placement, look at the inside of the LED you should see to metal plates which are roughly triangular. The smaller of the two plates is the positive side of the LED and should be connected nearest to pin 3 of the bottom timer. The other leg should be connect next to the 100 Ohm resistor where it joins the same row as pin 3. You will need to leave at least one spare hole in the board between the LED legs, you will need to bend one of the legs into an S/Z shape to do this.

As both ends of the LED are connected through the strip of copper running from the resistor to pin 3, we need to cut the copper strip to force the electricity to pass through the LED. Cut the track in the spare hole you left between the two LED Legs - refer to the underside picture for reference.

Almost Finished !
The original transponder design is finished here and if you intend to use the transponder for motor racing or kart racing, just add a positive power connection to row 3 (third row down counting from the top and linked to the orange positive power wires) and a ground connection to row 2 (second row down connected to the brown ground wires).

Add a 9 volt battery or even better 4 AA Batteries and go racing !

If you intend to use the transponder in an RC Vehicle and want to share the RC Power you will need to buffer your RC Receiver from the switching noise generated by the 555 timer. The most effective way I have found to do this is by using a 7805 regulator.

11) Adding The Regulator
The 7805 regulator prevents the switching noise in the transponder circuit from being transmitted through the power circuit and into your RC Receiver.

For the regulator to function it also needs two capacitors as supporting components which we will add in a later step.

For now place the 7805 regulator in the 10th hole counting from left to right. The regulator should be orientated so that the heat sink is on the right side of the circuit and the legs point down towards the 555 timers.


You might want to cut the heat sink off the 555 timer, this makes it easier to mount in your cars window, do this before soldering so that you do not damage the circuit while cutting.


13.1) Add The Capacitors
13.2) A Diode
13.3) Power Connection
Lets do this in the order in which power enters our circuit.

1) Solder your positive power wire ( the red wire) to the very top row of the circuit, is the top left most hole.

2) Solder your ground (black wire) connection to the second row, if we have followed the steps correctly this should also be the same row as the middle pin of the 7805 regulator.

3) Connect a 0.1uf decoupling capacitor between the positive and negative power rails, in the picture you can see that I added mine to the left of the wires, you can add yours to the right as long as its immediately next to the incoming power connections.

4) Add a diode. Diodes are directional components, they only conduct electricity in the one direction, the direction is indicated by a band printed on the diode. In the picture the silver band indicates the direction in which electricity is allowed to flow. The band is facing the 7805 regulator meaning that electricity can flow into our circuit but if electricity is connected in the reverse direction the diode will prevent it from flowing and damaging our circuit.

The diode is connected to the positive power track at the top left of our circuit, once we cut the copper track underneath the diode the electricity will have to travel through the diode to reach our circuit. If we accidentally connect power in the wrong way, the diode will prevent damage to the rest of the circuit from the reverse power.

5) Add the 7805 Supporting capacitors. In order to regulate the power to our circuit, the 7805 requires some additional external components, in this case we are using two 100uf electrolytic capacitors. Like all electrolytic capacitors these are directional components, a band on the capacitor indicates the side which should be connected to ground. The middle pin of the 7805 is the ground, it should be soldered in the second track down on our board which will be the ground rail. The outer pins are the input power supply and the 5volt regulated output, each of these should be connected to the center ground pin through one of the 100uf Capacitors - make sure that the band indicating the negative side of each capacitor is connected to the center ground pin of the 7805.

Final Steps
Our final step is to cut the copper tracks under the 555 timers so that the left and right pins are not shorted together, a 3mm drill bit will do this nicely.

Final checks and trouble shooting -
1) The copper tracks on the underside of the circuit should be cut in the following locations -

In the picture the copper tracks are running from left to right across the underneath of the board. The red marks indicate where a track should be cut, in order from top to bottom these points are -

Diode - We need to cut the track directly underneath the diode in the top left of the circuit, this will force the electricity through the diode ensuring that if we connect power the wrong way around, no electricity will flow and no damage will occur.

555 Timers - We need to cut the tracks between the left and right pins or the two timers, this means 4 cuts on the first time and four on the bottom so that the pins are not shorted by the copper tracks.

680 Ohm Resistor - This resistor is connected to pin 7 of the second (lower) 555 timer and runs horizontally along the board. As this is the same direction as the copper tracks under the board, we need to cut the track under the resistor otherwise the electricity will by pass it through the copper track.

IR LED - This component is also connected horizontally across the board. In order for the electricity to pass through the LED we need to cut the track between its two legs, in the picture the cut is marked one hole to the left of where the positive leg of the LED is soldered next to pin 3 (output) of the lower 555 timer.


2) Confirm that all of the components are correctly orientated.

3) Test the circuit - in order to test the circuit we need an IR Detector, a simple test circuit is provided in the following link, if you can light the red LED with the signal from your transponder, well done you have a working transponder.

http://learn.adafruit.com/ir-sensor/testing-an-ir-sensor

Additional References -

NE 555 Datasheet with pinout
http://www.ti.com/lit/ds/symlink/ne555.pdf

7805 Regulator Datasheet
http://www.sparkfun.com/datasheets/Components/LM7805.pdf

Next time - Adding the IR Detector to our lap timer circuit.

Bonus Step -

As an optional step you might want to consider adding the amplifier circuit shown here -

http://rcarduino.blogspot.com/2012/08/adding-audio-to-arduino-projects.html

The external audio mode can be switched on and off through the lap timer menu for extra volume when you need it. The new lap time countdown mode that you can hear in the video can also be switched on and off through the system menu.


External Audio and best lap time countdown demo -


 Stay tuned ...

Duane B

Saturday, July 7, 2012

RCArduino Yaw Control - Part 2

 
Creative Commons License
RCArduinoYawControl by DuaneB is licensed under a Creative Commons Attribution-NonCommercial-ShareAlike 3.0 Unported License.
Based on a work at rcarduino.blogspot.com.


The Project Pitch - 

Do you have an RC Car thats just too unpredictable to really enjoy ?
Are you a beginner driver in the rear wheel drive classes and could use some help ?
Do you race in a class where electronic assistance is allowed ?

Existing solutions are not helping you as they fail to address the root cause of RC Car instability. 

Race orientated RC Cars are powerful enough to reach scale speeds of 600 miles per hour. Stability solutions currently available try to address this excess power by actively steering into skids however this approach cannot hope to match up to the vast amounts of power resulting in frustration, broken cars and lost interest.

The RCArduino solution is the first complete solution to RC Car yaw control and provides the perfect introduction to driving the fastest and most challenging RC Race cars -
 - Learn to drive high powered RC Cars using the RCArduino adjustable assistance system.
 - Bring the fun back into high powered RC
 - Reduce the sensitivity as your skill level increases.
 - Avoid unecessary crashes and breakage while learning


40Km/h Arduino -Passing The Controls Over To Get Feedback From Other Track users



The Project

In Yaw Control Part 1 I discussed how gyroscopes can be applied in Radio Controlled cars to reduce yaw under braking and acceleration.

Since this first post I have developed an Arduino based yaw control system which goes beyond the original goals to address the major flaw in the gyro based approach.

Anti-Yaw Control Part 1 -  A Flawed Approach

Background - Why do we need Yaw Control ?

Radio controlled cars have many times the power to weight ratio of conventional cars. In front wheel drive and four wheel drive RC Cars this can be seen in the form of wheel spin and power slides. Few of the rear wheel drive RC Car designs are able to cope with this excess power resulting in a car that spins out under acceleration. All of my RWD Cars - Tamiya M04, F103GT, 3Racing F109 and Sand Scorcher will spin out under modest acceleration.

A similar problem exists with braking, in a RWD RC Car, the motor provides braking, but as the motor is only attached to the rear wheels the effect is similar to pulling the handbrake (e-brake) in a road car. Great for j-turns, but its not going to get you around the track very quickly.

The original and flawed concept

My original design was based on an established idea which is to use a RC Helicopter gyroscope to counter steer into a skid before it can develop into a spin. These gyroscopes are readily available and are used in RC Helicopters to automatically increase or decrease the tail rotor speed to compensate for changes in the main rotor speed which would otherwise cause the helicopter to rotate. This type of gyro is known as a rate gyro.

Rate Gryo Fitted To My F103GT RWD RC Race Car
To use a rate gyro in a car, the gyro is connected between the receiver and the steering servo. The gyro listens to the incoming steering signal and generates an output based on this and the rate of rotation. When the model is accelerating or braking with no steering input, the gyro will detect the car starting to rotate and automatically signal the steering servo to steer into the skid and bring it under control.



Sounds good, whats the problem ?

If we go back to the statement under 'Why do we need Yaw Control' -

'Radio controlled cars have many times the power to weight ratio of conventional cars.'

It is fundamentally a power problem. Attempting to address the problem through steering alone will only get us so far.

So what is the solution ?

The solution is to take active control of both steering and throttle channels,  this way we can address the original cause of the problem (reduce the excess power) and the resulting effect (steer into the slide before it becomes a spin).

Unfortunately RC Gyros are primarily designed for use in helicopters and so while we can use them to add active counter steer to our cars, they are not able to interface with the throttle channel and are therefore unable to address the root cause of our problem.

Fortunately there is a interesting micro controller project to build this dual channel active control system.

The RCArduino Solution

Note : For a background on the techniques used to read the RC Receiver, Output the RC Signal and Calibrate with the RC Transmitter refer to the previous RCArduino posts listed below -


http://rcarduino.blogspot.com/2012/01/how-to-read-rc-receiver-with.html



The RCArduino approach to yaw control implements tuneable control of both the throttle and steering channels. Separate adjustments are provided for each channel ranging from no intervention to very aggressive intervention.

Features -

 - One touch calibration
 - Fully and independently adjustable throttle intervention
 - Fully and independently adjustable steering  intervention

 - No need to cut or otherwise alter existing connections to your car electronics
 - Fail safe mode
 - Unique software specifically designed for high powered RC Cars
Version 0.1 - Big, but easy to work on.



How does it work ?

The unit reads the incoming signals and generates corresponding output signals. When the unit detects the car rotating it will calculate an intervention component for the output to reduce the rotation and allow the driver to retain control. The degree of intervention is adjustable from no intervention to very aggressive.

A further adjustment is provided to control the 'throttle intervention recovery period', when this is set to a low value throttle intervention is applied something like and on/off switch, with a higher value throttle intervention fades away over a period of upto 2 seconds providing for a gradual transition from full intervention to full throttle.

The intervention recovery period provides for a tuneable degree of drift. With a long recovery period the car will be very stable through the corner and onto the straight. With a minimal recovery period the throttle channel will be continually 'blipped' throughout the corner and a moderate drift can be maintained. With low recovery periods, transistions can be very aggressive, as soon as the system detects that rotation has stopped it will apply full power, this can happen in the transistion of an S bend or when exiting a corner onto the straight. This is a useful tuning technique for learning where a car can be driven hard and which points on the circuit require a more restrained approach.

Whats coming in version 1.0 ?

Version 1.0 is smaller and adds LED level meters which display -
1) The rotation rate being reported by the gyro
2) The level of steering intervention currently being applied
3)  The level of throttle intervention being applied
4) An additional tuning option on the steering channel
5) An additional tuning option for braking

Next Steps -

The system has been road tested on various surfaces and tyre combinations for around 10 hours without any unpredictable behaviour, the next step is to change the motor from the current entry level silver can motor (<>15,000 RPM) to a Sport Tuned Motor (<>22,000 RPM).

Version 0.1 Code - Lots of obvious optimization needed and lacking features being tested in 1.0, but in the interest of sharing, here it is - 

#include <Servo.h>
#include <EEPROM.h>

// RCArduinoYawControl
//
// RCArduinoYawControl by DuaneB is licensed under a Creative Commons Attribution-NonCommercial-ShareAlike 3.0 Unported License.
// Based on a work at rcarduino.blogspot.com.
//
// rcarduino.blogspot.com
//
// A simple approach for reading two RC Channels from a hobby quality receiver
// and outputting to the common motor driver IC the L293D to drive a tracked vehicle
//
// We will use the Arduino to mix the channels to give car like steering using a standard two stick
// or pistol grip transmitter. The Aux channel will be used to switch and optional momentum mode on and off
//
// See related posts -
//
// Reading an RC Receiver - What does this signal look like and how do we read it -
// http://rcarduino.blogspot.co.uk/2012/01/how-to-read-rc-receiver-with.html
//
// The Arduino library only supports two interrupts, the Arduino pinChangeInt Library supports more than 20 -
// http://rcarduino.blogspot.co.uk/2012/03/need-more-interrupts-to-read-more.html
//
// The Arduino Servo Library supports upto 12 Servos on a single Arduino, read all about it here -
// http://rcarduino.blogspot.co.uk/2012/01/can-i-control-more-than-x-servos-with.html
//
// The wrong and then the right way to connect servos to Arduino
// http://rcarduino.blogspot.com/2012/04/servo-problems-with-arduino-part-1.html
// http://rcarduino.blogspot.com/2012/04/servo-problems-part-2-demonstration.html
//
// Using pinChangeInt library and Servo library to read three RC Channels and drive 3 RC outputs (mix of Servos and ESCs)
// http://rcarduino.blogspot.com/2012/04/how-to-read-multiple-rc-channels-draft.html
//
// rcarduino.blogspot.com
//

// Two channels in
// Two channels out
// One program button
// One throttle intervention LED
// One Steering Intervention LED
// One Steering Sensitivity POT
// One Throttle Sensitivity POT
// One decay/damping POT

#define RC_NEUTRAL 1500
#define RC_MAX 2000
#define RC_MIN 1000
#define RC_DEADBAND 1

#define ROTATION_CENTER 380

uint16_t unSteeringMin = RC_MIN;
uint16_t unSteeringMax = RC_MAX;
uint16_t unSteeringCenter = RC_NEUTRAL;

uint16_t unThrottleMin = RC_MIN;
uint16_t unThrottleMax = RC_MAX;
uint16_t unThrottleCenter = RC_NEUTRAL;

uint16_t unRotationCenter = ROTATION_CENTER; // the gyro I am using outputs a center voltage of <> 1.24 volts
// full range is 0 to 2*1.24 = 2.48
// Using the Arduino voltage reference of 3.3 volts gives a center point of (1.24/(3.3/1024)) = 384
// In tests I see 380 so I will use 380 as my default value.

//////////////////////////////////////////////////////////////////
// PIN ASSIGNMENTS
//////////////////////////////////////////////////////////////////
// ANALOG PINS
//////////////////////////////////////////////////////////////////
#define THROTTLE_SENSITIVITY_PIN 0
#define STEERING_SENSITIVITY_PIN 1
#define THROTTLE_DECAY_PIN 2
#define STEERING_DECAY_PIN 3
#define GYRO_PIN 5
//////////////////////////////////////////////////////////////////
// DIGITAL PINS
//////////////////////////////////////////////////////////////////
#define PROGRAM_PIN 9
#define INFORMATION_INDICATOR_PIN 5
#define ERROR_INDICATOR_PIN 6
#define THROTTLE_IN_PIN 2
#define STEERING_IN_PIN 3
#define THROTTLE_OUT_PIN 8
#define STEERING_OUT_PIN 7

// These bit flags are set in bUpdateFlagsShared to indicate which
// channels have new signals
#define THROTTLE_FLAG 1
#define STEERING_FLAG 2

// holds the update flags defined above
volatile uint8_t bUpdateFlagsShared;

// shared variables are updated by the ISR and read by loop.
// In loop we immediatley take local copies so that the ISR can keep ownership of the
// shared ones. To access these in loop
// we first turn interrupts off with noInterrupts
// we take a copy to use in loop and the turn interrupts back on
// as quickly as possible, this ensures that we are always able to receive new signals
volatile uint16_t unThrottleInShared;
volatile uint16_t unSteeringInShared;

// These are used to record the rising edge of a pulse in the calcInput functions
// They do not need to be volatile as they are only used in the ISR. If we wanted
// to refer to these in loop and the ISR then they would need to be declared volatile
uint32_t ulThrottleStart;
uint32_t ulSteeringStart;

// used to ensure we are getting regular throttle signals
uint32_t ulLastThrottleIn;

#define MODE_FORCEPROGRAM 0
#define MODE_RUN 1
#define MODE_QUICK_PROGRAM 2
#define MODE_FULL_PROGRAM 3
#define MODE_ERROR 4

uint8_t gMode = MODE_RUN;
uint32_t ulProgramModeExitTime = 0;

// Index into the EEPROM Storage assuming a 0 based array of uint16_t
// Data to be stored low byte, high byte
#define EEPROM_INDEX_STEERING_MIN 0
#define EEPROM_INDEX_STEERING_MAX 1
#define EEPROM_INDEX_STEERING_CENTER 2
#define EEPROM_INDEX_THROTTLE_MIN 3
#define EEPROM_INDEX_THROTTLE_MAX 4
#define EEPROM_INDEX_THROTTLE_CENTER 5
#define EEPROM_INDEX_ROTATION_CENTER 6

Servo servoThrottle;
Servo servoSteering;

uint16_t unThrottleSensitivity = 0;
uint16_t unSteeringSensitivity = 0;
uint16_t unThrottleDecay = 0;
uint16_t unSteeringDecay = 0;

void setup()
{
  Serial.begin(9600);

  pinMode(PROGRAM_PIN,INPUT);
  pinMode(INFORMATION_INDICATOR_PIN,OUTPUT);
  pinMode(ERROR_INDICATOR_PIN,OUTPUT);

  attachInterrupt(0 /* INT0 = THROTTLE_IN_PIN */,calcThrottle,CHANGE);
  attachInterrupt(1 /* INT1 = STEERING_IN_PIN */,calcSteering,CHANGE);

  readAnalogSettings();
 
  servoThrottle.attach(THROTTLE_OUT_PIN);
  servoSteering.attach(STEERING_OUT_PIN);
   
  if(false == readSettingsFromEEPROM())
  {
    gMode = MODE_FORCEPROGRAM;
  }
 
  ulLastThrottleIn = millis();
}

void loop()
{
  // create local variables to hold a local copies of the channel inputs
  // these are declared static so that thier values will be retained
  // between calls to loop.
  static uint16_t unThrottleIn;
  static uint16_t unSteeringIn;
  // local copy of rotation rate
  static uint16_t unRotation;
  // local copy of update flags
  static uint8_t bUpdateFlags;

  static uint16_t unThrottleInterventionPeak;
  static uint16_t unSteeringInterventionPeak;
 

  uint32_t ulMillis = millis();
 
  // check shared update flags to see if any channels have a new signal
  if(bUpdateFlagsShared)
  {
    noInterrupts(); // turn interrupts off quickly while we take local copies of the shared variables

      // take a local copy of which channels were updated in case we need to use this in the rest of loop
    bUpdateFlags = bUpdateFlagsShared;

    // in the current code, the shared values are always populated
    // so we could copy them without testing the flags
    // however in the future this could change, so lets
    // only copy when the flags tell us we can.

    if(bUpdateFlags & THROTTLE_FLAG)
    {
      unThrottleIn = unThrottleInShared;
    }

    if(bUpdateFlags & STEERING_FLAG)
    {
      unSteeringIn = unSteeringInShared;
    }

    // clear shared copy of updated flags as we have already taken the updates
    // we still have a local copy if we need to use it in bUpdateFlags
    bUpdateFlagsShared = 0;

    interrupts(); // we have local copies of the inputs, so now we can turn interrupts back on
    // as soon as interrupts are back on, we can no longer use the shared copies, the interrupt
    // service routines own these and could update them at any time. During the update, the
    // shared copies may contain junk. Luckily we have our local copies to work with :-)
  }

  if(false == digitalRead(PROGRAM_PIN) && gMode != MODE_FULL_PROGRAM)
  {
    // give 10 seconds to program
    gMode = MODE_QUICK_PROGRAM;
 
    // turn indicators off for QUICK PROGRAM mode
    digitalWrite(INFORMATION_INDICATOR_PIN,LOW);   
    digitalWrite(ERROR_INDICATOR_PIN,LOW);   

    // wait two seconds and test program pin again
    // if pin if still held low, enter full program mode
    // otherwise read sensitivity pots and return to run
    // mode with new sensitivity readings.
    delay(2000);
   
    if(false == digitalRead(PROGRAM_PIN))
    {
      gMode = MODE_FULL_PROGRAM;
      ulProgramModeExitTime = ulMillis + 10000;
      
      unThrottleMin = RC_NEUTRAL;
      unThrottleMax = RC_NEUTRAL;
      unSteeringMin = RC_NEUTRAL;
      unSteeringMax = RC_NEUTRAL;
   
      unThrottleCenter = unThrottleIn;
      unSteeringCenter = unSteeringIn;
    }
    else
    {
      gMode = MODE_RUN; 
      ulMillis = millis();   
      ulLastThrottleIn = ulMillis;
    }
   
    // Take new sensitivity and decay readings for quick program and full program modes here
    readAnalogSettings();
  }
 
  if(gMode == MODE_FULL_PROGRAM)
  {
   if(ulProgramModeExitTime < ulMillis)
   {
     // set to 0 to exit program mode
     ulProgramModeExitTime = 0;
     gMode = MODE_RUN;

     analogRead(GYRO_PIN);
     uint32_t ulTotal = 0;
     for(int nCount = 0;nCount < 50;nCount++)
     {
       ulTotal += analogRead(GYRO_PIN);
     }
     unRotationCenter = ulTotal/50;

     writeSettingsToEEPROM();
    
     ulLastThrottleIn = ulMillis;
   }
   else
   {
     if(unThrottleIn > unThrottleMax && unThrottleIn <= RC_MAX)
     {
       unThrottleMax = unThrottleIn;
     }
     else if(unThrottleIn < unThrottleMin && unThrottleIn >= RC_MIN)
     {
       unThrottleMin = unThrottleIn;
     }
    
     if(unSteeringIn > unSteeringMax && unSteeringIn <= RC_MAX)
     {
       unSteeringMax = unSteeringIn;
     }
     else if(unSteeringIn < unSteeringMin && unSteeringIn >= RC_MIN)
     {
       unSteeringMin = unSteeringIn;
     }
   }
  }
  else if(gMode == MODE_RUN)
  {
    if((ulLastThrottleIn + 500) < ulMillis)
    {
      gMode = MODE_ERROR;
    }
    else
    {
      // we are checking to see if the channel value has changed, this is indicated
      // by the flags. For the simple pass through we don't really need this check,
      // but for a more complex project where a new signal requires significant processing
      // this allows us to only calculate new values when we have new inputs, rather than
      // on every cycle.
      if(bUpdateFlags)
      {
       unRotation = analogRead(GYRO_PIN);
      }

      if(bUpdateFlags & THROTTLE_FLAG)
      {
        ulLastThrottleIn = ulMillis;
       
        // A good idea would be to check the before and after value,
        // if they are not equal we are receiving out of range signals
        // this could be an error, interference or a transmitter setting change
        // in any case its a good idea to at least flag it to the user somehow
        uint16_t unThrottleIntervention = 0;
       
        if(unRotation != unRotationCenter)
        {
          uint32_t ulRotationWithGain = 0;
          if(unRotation > unRotationCenter)
          {
           ulRotationWithGain = (long)(unRotation - unRotationCenter)*unThrottleSensitivity;
           unThrottleIntervention = map(ulRotationWithGain,0,(long)unRotationCenter*128L,0,500);
          }
          else
          {
           ulRotationWithGain = (long)(unRotationCenter - unRotation)*unThrottleSensitivity;
           unThrottleIntervention = map(ulRotationWithGain,0,(long)unRotationCenter*128L,0,500);
          }
        }

        if(unThrottleIntervention > unThrottleInterventionPeak)
        {
          unThrottleInterventionPeak = unThrottleIntervention;
        }
        else
        {
          if(unThrottleInterventionPeak >= unThrottleDecay)
          {
            unThrottleInterventionPeak -= unThrottleDecay;
          }
          else
          {
            unThrottleInterventionPeak = 0;
          }
         
          if(unThrottleIntervention < unThrottleInterventionPeak)
          {
            unThrottleIntervention = unThrottleInterventionPeak;
          }
        }
       
        if(unThrottleIn >= unThrottleCenter)
        {
          unThrottleIn = constrain(unThrottleIn - unThrottleIntervention,unThrottleCenter,unThrottleIn);
        }
        else
        {
          unThrottleIn = constrain(unThrottleIn + unThrottleIntervention,unThrottleMin,unThrottleCenter);
        }

        servoThrottle.writeMicroseconds(unThrottleIn);
      }
    }
   
    if(bUpdateFlags & STEERING_FLAG)
    {
      uint16_t unSteeringIntervention = 0;
      uint8_t bInvert = false;
       
      if(unRotation != unRotationCenter)
      {
        uint32_t ulRotationWithGain = 0;
       
        // note steering offers gain from 0 to 4
        uint16_t unInterventionMaxMinusInput = 0;
        if(unSteeringIn >= unSteeringCenter)
        {
          unInterventionMaxMinusInput = (unSteeringMax-unSteeringCenter) - (unSteeringIn - unSteeringCenter);
          unInterventionMaxMinusInput = constrain(unInterventionMaxMinusInput,0,unSteeringMax-unSteeringCenter);
        }
        else
        {
          unInterventionMaxMinusInput = (unSteeringCenter - unSteeringMin) - (unSteeringCenter - unSteeringIn);
          unInterventionMaxMinusInput = constrain(unInterventionMaxMinusInput,0,unSteeringCenter - unSteeringIn);
        }
      
        if(unRotation > unRotationCenter)
        {
         bInvert = true;
         ulRotationWithGain = (long)(unRotation - unRotationCenter)*unSteeringSensitivity;
         unSteeringIntervention = map(ulRotationWithGain,0,(long)unRotationCenter*128L,0,unInterventionMaxMinusInput);
        }
        else
        {
         ulRotationWithGain = (long)(unRotationCenter - unRotation)*unSteeringSensitivity;
         unSteeringIntervention = map(ulRotationWithGain,0,(long)unRotationCenter*128L,0,unInterventionMaxMinusInput);
        }
      }
       
      if(bInvert)
      {
        unSteeringIn = constrain(unSteeringIn - unSteeringIntervention,unSteeringMin,unSteeringMax);
      }
      else
      {
        unSteeringIn = constrain(unSteeringIn + unSteeringIntervention,unSteeringMin,unSteeringMax);
      }
      servoSteering.writeMicroseconds(unSteeringIn);
    }
  }
  else if(gMode == MODE_ERROR)
  {
    servoThrottle.writeMicroseconds(unThrottleCenter);
   
    // allow steering to get to safety
    if(bUpdateFlags & STEERING_FLAG)
    {
      unSteeringIn = constrain(unSteeringIn,unSteeringMin,unSteeringMax);

      servoSteering.writeMicroseconds(unSteeringIn);
    }
  }
 
  bUpdateFlags = 0;
 
  animateIndicatorsAccordingToMode(gMode,ulMillis);
}

void animateIndicatorsAccordingToMode(uint8_t gMode,uint32_t ulMillis)
{
  static uint32_t ulLastUpdateMillis;
  static boolean bAlternate;

  if(ulMillis > (ulLastUpdateMillis + 1000))
  {
    ulLastUpdateMillis = ulMillis;
    bAlternate = (!bAlternate);
    switch(gMode)
    {
      // flash alternating info and error once a second
      case MODE_FORCEPROGRAM:
         digitalWrite(ERROR_INDICATOR_PIN,bAlternate);   
         digitalWrite(INFORMATION_INDICATOR_PIN,false == bAlternate);   
      break;
      // steady info, turn off error
      case MODE_RUN:
        digitalWrite(ERROR_INDICATOR_PIN,LOW);   
        digitalWrite(INFORMATION_INDICATOR_PIN,HIGH);
      break;
      // flash info once a second, turn off error
      case MODE_FULL_PROGRAM:
        digitalWrite(INFORMATION_INDICATOR_PIN,bAlternate);   
        digitalWrite(ERROR_INDICATOR_PIN,LOW);   
      break;
      // alternate error, turn off info
      // MODE_QUICK_PROGRAM is self contained and should never get here,
      // if it does we have an error.
      default:
      case MODE_ERROR:
        digitalWrite(INFORMATION_INDICATOR_PIN,LOW);
        digitalWrite(ERROR_INDICATOR_PIN,bAlternate);   
      break;
    }
  }
}

// simple interrupt service routine
void calcThrottle()
{
  // if the pin is high, its a rising edge of the signal pulse, so lets record its value
  if(PIND & 4)
  {
    ulThrottleStart = micros();
  }
  else
  {
    // else it must be a falling edge, so lets get the time and subtract the time of the rising edge
    // this gives use the time between the rising and falling edges i.e. the pulse duration.
    unThrottleInShared = (uint16_t)(micros() - ulThrottleStart);
    // use set the throttle flag to indicate that a new throttle signal has been received
    bUpdateFlagsShared |= THROTTLE_FLAG;
  }
}

void calcSteering()
{
  if(PIND & 8)
  {
    ulSteeringStart = micros();
  }
  else
  {
    unSteeringInShared = (uint16_t)(micros() - ulSteeringStart);
    bUpdateFlagsShared |= STEERING_FLAG;
  }
}

uint8_t readSettingsFromEEPROM()
{
  uint8_t bError = false;
 
  unSteeringMin = readChannelSetting(EEPROM_INDEX_STEERING_MIN);
  if(unSteeringMin < RC_MIN || unSteeringMin > RC_NEUTRAL)
  {
    unSteeringMin = RC_MIN;
    bError = true;
  }
  Serial.println(unSteeringMin);

  unSteeringMax = readChannelSetting(EEPROM_INDEX_STEERING_MAX);
  if(unSteeringMax > RC_MAX || unSteeringMax < RC_NEUTRAL)
  {
    unSteeringMax = RC_MAX;
    bError = true;
  }
  Serial.println(unSteeringMax);
 
  unSteeringCenter = readChannelSetting(EEPROM_INDEX_STEERING_CENTER);
  if(unSteeringCenter < unSteeringMin || unSteeringCenter > unSteeringMax)
  {
    unSteeringCenter = RC_NEUTRAL;
    bError = true;
  }
  Serial.println(unSteeringCenter);

  unThrottleMin = readChannelSetting(EEPROM_INDEX_THROTTLE_MIN);
  if(unThrottleMin < RC_MIN || unThrottleMin > RC_NEUTRAL)
  {
    unThrottleMin = RC_MIN;
    bError = true;
  }
  Serial.println(unThrottleMin);

  unThrottleMax = readChannelSetting(EEPROM_INDEX_THROTTLE_MAX);
  if(unThrottleMax > RC_MAX || unThrottleMax < RC_NEUTRAL)
  {
    unThrottleMax = RC_MAX;
    bError = true;
  }
  Serial.println(unThrottleMax);
 
  unThrottleCenter = readChannelSetting(EEPROM_INDEX_THROTTLE_CENTER);
  if(unThrottleCenter < unThrottleMin || unThrottleCenter > unThrottleMax)
  {
    unThrottleCenter = RC_NEUTRAL;
    bError = true;
  }
  Serial.println(unThrottleCenter);
 
  unRotationCenter = readChannelSetting(EEPROM_INDEX_ROTATION_CENTER);
  Serial.println(unRotationCenter);
 
  // ideally we would have 512 as the center
  if(unRotationCenter < 100 || unRotationCenter > 560)
  {
    unRotationCenter = ROTATION_CENTER;
    bError = true;
  }
 
  return (false == bError);
}

void writeSettingsToEEPROM()
{
  writeChannelSetting(EEPROM_INDEX_STEERING_MIN,unSteeringMin);
  writeChannelSetting(EEPROM_INDEX_STEERING_MAX,unSteeringMax);
  writeChannelSetting(EEPROM_INDEX_STEERING_CENTER,unSteeringCenter);
  writeChannelSetting(EEPROM_INDEX_THROTTLE_MIN,unThrottleMin);
  writeChannelSetting(EEPROM_INDEX_THROTTLE_MAX,unThrottleMax);
  writeChannelSetting(EEPROM_INDEX_THROTTLE_CENTER,unThrottleCenter);
 
  writeChannelSetting(EEPROM_INDEX_ROTATION_CENTER,unRotationCenter);
           
  Serial.println(unSteeringMin);
  Serial.println(unSteeringMax);
  Serial.println(unSteeringCenter);
  Serial.println(unThrottleMin);
  Serial.println(unThrottleMax);
  Serial.println(unThrottleCenter);
 
  Serial.println(unRotationCenter);
}


uint16_t readChannelSetting(uint8_t nStart)
{
  uint16_t unSetting = (EEPROM.read((nStart*sizeof(uint16_t))+1)<<8);
  unSetting += EEPROM.read(nStart*sizeof(uint16_t));

  return unSetting;
}

void writeChannelSetting(uint8_t nIndex,uint16_t unSetting)
{
  EEPROM.write(nIndex*sizeof(uint16_t),lowByte(unSetting));
  EEPROM.write((nIndex*sizeof(uint16_t))+1,highByte(unSetting));
}

/////////////////////////////////////////////////////////////////////////////
//
// The following function reads the analog settings.
// These adjustments are read at startup and anytime that the program button
// is pressed including QUICK_PROGRAM and FULL_PROGRAM
//
// Note the 1-1023 range for sensitivity and 1-100 range for decay 1 = 500/(50*1) per sec = 10 seconds. 100 = 500/(50*100) per sec = .1 seconds
//
/////////////////////////////////////////////////////////////////////////////
void readAnalogSettings()
{
 // dummy read to settle ADC
  analogRead(THROTTLE_SENSITIVITY_PIN);
  unThrottleSensitivity  = constrain(analogRead(THROTTLE_SENSITIVITY_PIN),1,1023); 

  // dummy read to settle ADC
  analogRead(STEERING_SENSITIVITY_PIN);
  unSteeringSensitivity = constrain(analogRead(STEERING_SENSITIVITY_PIN),1,1023);
 
  // dummy read to settle ADC
  analogRead(THROTTLE_DECAY_PIN);
  unThrottleDecay = analogRead(THROTTLE_DECAY_PIN);
  // if its at the bottom end of the range, turn it off
  if(unThrottleDecay <= 50) // instant
  {
    unThrottleDecay = 500; // = 500/1
  }
  else if(unThrottleDecay <= 100) // 2 tenths of a second
  {
    unThrottleDecay = 50; // 2 tenths = (2*(1/10))/(1/50)
  }
  else if(unThrottleDecay <= 150) // 3 tenths
  {
    unThrottleDecay = 33;
  }
  else if(unThrottleDecay <= 200) // 4 tenths
  {
    unThrottleDecay = 25;
  }
  else if(unThrottleDecay <= 250) // 5 tenths
  {
    unThrottleDecay = 20;
  }
  else if(unThrottleDecay <= 300) // 6 tenths
  {
    unThrottleDecay = 17;
  }
  else if(unThrottleDecay <= 350) // 7 tenths
  {
    unThrottleDecay = 14;
  }
  else if(unThrottleDecay <= 400) // 8 tenths
  {
    unThrottleDecay = 12;
  }
  else if(unThrottleDecay <= 500) // 9 tenths
  {
    unThrottleDecay = 11; 
  }
  else if(unThrottleDecay <= 600) // 10 tenths
  {
    unThrottleDecay = 10;
  }
  else if(unThrottleDecay <= 700) // 11 tenths
  {
    unThrottleDecay = 9;
  }
  else if(unThrottleDecay <= 800) // 12 tenths
  {
    unThrottleDecay = 8;
  }
  else if(unThrottleDecay <= 850) // 16 tenths
  {
    unThrottleDecay = 7;
  }
  else if(unThrottleDecay <= 900) // 2 seconds
  {
    unThrottleDecay = 5;
  }
  else if(unThrottleDecay <= 1000)
  {
     unThrottleDecay = 4;
  }
  else
  {
    unThrottleDecay = 1;
  }
 
  Serial.println(unThrottleDecay);
 
  // dummy read to settle ADC
  analogRead(STEERING_DECAY_PIN);
  unSteeringDecay = constrain(analogRead(STEERING_DECAY_PIN),1,500);
}