Friday, July 27, 2012

Lap Timer Build Along Part One

Creative Commons License
Based on a work at rcarduino.blogspot.com

Update - I have three big ideas for a version 2.0 of the lap timer system, they build on the capabilities of version 1.0, extending its functionality to a completely new area and adding what I hope will be a really cool user interface feature.

Before I begin work on version 2.0 I am completing the Version 1.0 Build Along - Starting right now.

What do we get at the end of Part One -

 - Record and review race and practice sessions of upto 500 Laps
 - Scroll through summaries of each session to view best and average lap times and the number of session laps
 - Scroll through individual laps within any session
 - A bonus feature for part 1 only - The "Fake A Lap" button
 - The "Fake A Lap" button allows you to test your build before we introduce the automatic lap capture features in  - parts two and three.
 - At the end of part one you will have a fully function manual lap timer - your very own self built multi lap, multi session stop watch !

Build this in Part One today (infra red lap capture in part 2 and build your own transponder in part 3)


Older posts covering the functionality, design and a demonstration using an RC Car can be found here -

Menu and RC Car Demonstration -
Features - 
Interface Design - 

Older videos -
Timing an RC Car fitted with the transponder we will build in part three.
A sample of the session and lap review menus
video

video




Everything in version 1.0 will be used as the basis for version 2.0. So lets get started -

Personal Lap Timer V1.0 Part 1
Goal - Build the user interface

Requirements - If you can grab a set of parts that looks like this, you can build part one of the lap timer in one hour.
- An Arduino UNO or compatible (Arduino UNO recommended for beginners)
- An LCD Display
- A 10K Potentiometer
- Hook up wire, breadboard or strip board for connections.
- Four push buttons
- Four 10K pull up resistors for use with the push buttons
- If you want to add the 'Fake A Lap' button you will require one extra button, this button is not required in later stages.







About LCD Displays
There are several libraries for driving LCD Displays with Arduino including one which is installed by default as part of the Arduino application. This is the library we will use, it provides an easy to use interface for driving 'character' LCD displays. These are LCD Displays that already know how to show text and common characters. To keep things simple, the lap timer uses a very common 16*2 character display giving us two lines of sixteen characters to display our race and setup information.

If you do not have an LCD, most electronics suppliers will have them in stock. The approximate cost is 10 dollars or 7 Pounds, if you buy the LCD from a project orientated supplier they will tend to include a 10K potentiometer used to adjust the contrast and some headers for soldering.

LCDs are well known and commonly used components within the micro controller community, for two very good introductions see the links copied below -

Oomlout - A UK Based supplier with downloadable PDF tutorials (scroll to the end of the page) -

Adafruit - A US Based supplier with online tutorials -

Lap Timer Build Along Part One
Step One - Power Connections

1) Connect the red positive power wire from the Arduino 5V Pin to the red positive rail of the breadboard
2) Connect the black ground wire from the Arduino GND Pin to the black (mine is blue) ground rail of the breadboard
3) Check that there are no breaks in the breadboard power rail, mine has a break which I am connecting across using the short green and red wires in the picture below.



I am using a double size breadboard, but if you only have access to smaller breadboards its not going to be a problem. See the two small red and green wires in the picture ? They are there to connect the two halves of this double breadboard together, you can do the same with two smaller breadboards to make a larger prototyping area. 

Step Two - Add The Buttons and Pull Up Resistors

1) Add your four push buttons, make sure to place them the correct way, in general push button pins are in a rectangular layout, the pins are connected to each other in the 'long direction'. The push buttons work across the short side, connecting and disconnect the short side as the button is pushed and released. In the picture the short side is pointing up.
2) Add the 10K pull up resistors, here you can see that I am connecting the top right pin on each button to the red power rail through a 10K pull up resistor. When the buttons is pressed - as mentioned in 1) above, it will connect to the pin closest to it, this is what we want so in the next step we will be connecting the closest (top right) to ground. Note that the top right pin is permanently connected inside the button to the bottom right pin, this is a common mistake to make - the button is permanently on if you position it the wrong way.


Step Three - Add The LCD

1) If your LCD Has headers installed you can plug it straight into your bread board at this point, if not solder in the headers and then plug it in. If you do not have headers, you can solder jumper wires to the LCD and connect it this way. I often use cut off sections of old printer ribbon cables for these types of connections.


Step Four - Add Ground Connections to the buttons
1) In step two we placed our four buttons (menu up,down,ok and cancel) and connected them through a 10K pull up resistor to the red positive rail of the breadboard. Now we want to connect the other side of the button to the black ground rail of the breadboard. Throughout the build along I am using red wire for positive connections and green wire for ground connections, it will make your own build easier if you follow a similar convention.

To understand why we are connecting the buttons this way, read any of the many Arduino Button Tutorials.





Step Five - Add The LCD Power Connections
1) We need to provide power to the LCD and the LCD Back light, to do this on most Arduino compatible LCDs we need to connect the outer two LCD connections to the ground rail and the connections immediately inside each ground connection to the red positive power rail. For alternative explanations of the same wiring refer to the LCD Links provided earlier in the post. There is one additional connection we need to make which is to connect the read write pin (R/W) to ground, we are only ever writing to the LCD so can hold this low. The end result whichever instructions you find easiest to follow should match the picture.


Step Six - Add The LCD Contrast Adjustment

As mentioned in the LCD Section at the start of the post, if you buy your Arduino compatible LCD from a project orientated supplier they will generally include a header for breadboard connections and a 10K potentiometer to adjust the contrast. If you didn't get a potentiometer with you display, any 10K potentiometer will do

1) Connect the left pin of your potentiometer to the red power rail and the right pin to the ground rail of the breadboard.
2) Connect the center pin of your potentiometer to third pin from the left of the LCD - assuming your LCD Orientation matches the picture. I have used orange wire for this connection in the picture.

This connection allows us to adjust the contrast of the LCD, very useful in different light conditions.





Step Seven - Connect the LCD To your Arduino

1) Use jumper wires to connect the LCD Pins to your Arduino, on the Arduino side I am using digital pins 7,8,9,10,11 and 12.
2) On the LCD, these pins are connected as follows -

All LCD Pins are numbered counting from left to right starting with 1.


Arduino PIN LCD PIN
Digital Pin 12 Register Select (Pin 4)
Digital Pin 11 PIN Clock/Enable (Pin 6)
Digital Pin 10 Bit 4 (Pin 11)
Digital Pin 9 Bit 5 (Pin 12)
Digital Pin 8 Bit 6 (Pin 13)
Digital Pin 7 Bit 7 (Pin 14)


The camera has fisheyed the picture slightly so it looks as if the connections on the left are one or two pins further left than they are. Follow the connections in the table and you will be fine.

Step Eight - Progress Check - Hello World

At this point you should be able to run any of the LCD Examples from the Arduino application. A good starting point is to check the connections with the 'helloworld' sketch. To do this -

1) Start the Arduino Application
2) In the File menu, select - Examples/LiquidCrystal/HelloWorld
3) You should now be looking at the example sketch 'HelloWorld' which is part of the Arduino download.
4) Find the line in the sketch that initialises the LCD Object, it should look something like the following -

// initialize the library with the numbers of the interface pins
LiquidCrystal lcd(12, 11, 5, 4, 3, 2);


This initialises an object named lcd of the class LiquidCrystal. As part of the initialisation our program is telling the object which pins to use. We want to use different pins, so we need to replace the line above with the following -


// Initialise the LCD using 12,11,10,9,8,7
//
LiquidCrystal lcd(12, 11, 5, 4, 3, 2); // Original PINs


LiquidCrystal lcd(12,11,10,9,8,7);

Now upload the sketch and if you have connected everything correctly you should see Hello World and the time displayed on you LCD. If not, double check the connections and try again.

Step Nine - Connect Buttons and Finish

1) Connect the four buttons by connecting a jumper wire between the pull up resistor and the top left pin of each button.
2) The buttons should be connected to the Arduino as follows -

Arduino PIN Lap Timer Button (buttons from left to right)
Digital Pin 6 Menu Up (Left Most Button)
Digital Pin 5 Menu Down
Digital Pin 4 Ok
Digital Pin 3 Cancel (Right Most Button


Step Ten - Upload

1) At this point we have completed Part One of the Lap Timer Build Along and can upload the Part One sketch to our build - I originally intended to include some test data so that at the end of Part One you would be able to use the system to scroll through some sessions and review the best, average and individual lap times within a session but then I thought - why not just add one more button ?

Bonus Step - The Big Red "Fake A Lap" Button


The lap timer uses infra red beacons or an infrared transponder to detect laps, we will get to detecting these  in step two and building one in step three.

If we add one more button we can use the system as a manual lap timer right now on day one of the build.

1) Add the 'Fake A Lap' button using the same approach as the other four buttons - 10K pull up resistor etc.
2) Connect the button to the Arduino by adding a jumper from the button to digital pin 2. We are using interrupts to detect laps and so it must be digital pin 2.





The Code -

There are three files lapTimerBuildAlongPart1.pde, LapTimes.cpp and LapTimes.h

If it looks like a lot of code, it isn't, its mostly comments to explain what the code does.

LapTimerBuildAlongPart1.pde -

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

#include <avr/pgmspace.h>
#include <EEPROM.h>
#include <LiquidCrystal.h>
#include "laptimes.h"

//*******************************************************************************************
// USER INTERFACE DEFINITIONS
//*******************************************************************************************
// initialise the liquidCrystal Library

// Initialise the LCD using 12,11,10,9,8,7
LiquidCrystal lcd(12,11,10,9,8,7);

// PINs for user interface buttons - use any
#define KEY_OK_PIN     6
#define KEY_CANCEL_PIN     5
#define KEY_UP_PIN     4
#define KEY_DOWN_PIN     3

// bit flags used in key functions getKeys, waitForKeyPress, waitForKeyRelease
#define KEY_NONE     0
#define KEY_OK         1
#define KEY_CANCEL     2
#define KEY_UP         4
#define KEY_DOWN     8

#define KEYPRESS_ANY B11111111

// display width + 1, used by getRamString to copy a PROG_MEM string into ram
#define DISPLAY_ROW_BUFFER_LENGTH 17

//*******************************************************************************************
// Lap Capture definitions
//*******************************************************************************************
#define LAP_CAPTURE_LED 13
#define BUZZER_PIN A0

// minimum and maximum duration of qualifying IR Pulse
#define MIN_PULSE_DURATION 200
#define MAX_PULSE_DURATION 500

// start and end of pulse
uint32_t ulStartPulse;
uint32_t ulEndPulse;
volatile uint32_t ulPulseDuration;

// flags to manage access and pulse edges
volatile uint8_t bIRPulseFlags;
//
volatile uint32_t ulNewLapStartTime;

#define IR_PULSE_START_SET 1
#define IR_PULSE_END_SET 2

//*****************************************************************
// Global Instance of CLapTimes class
//*****************************************************************
CLapTimes gLapTimes(new CEEPROMLapStore());

//////////////////////////////////////////////////////////////////////////////////
//
// doShowSessionSummaries
//
// implements the show session summary menu
// allows the user to scroll up and down through summaries of the recorded sessions
//
//////////////////////////////////////////////////////////////////////////////////
void setup()
{
 Serial.begin(9600);
 Serial.println("In Setup");
  
 lcd.begin(16, 2);
 lcd.print("Lap Timer");
 lcd.setCursor(0,1);
 lcd.print("Version 0.9 Beta");

 delay(3000);

 pinMode(KEY_OK_PIN,INPUT);
 pinMode(KEY_CANCEL_PIN,INPUT);
 pinMode(KEY_UP_PIN,INPUT);
 pinMode(KEY_DOWN_PIN,INPUT);

 pinMode(LAP_CAPTURE_LED,OUTPUT);
 pinMode(BUZZER_PIN,OUTPUT);

 digitalWrite(LAP_CAPTURE_LED,LOW);
 digitalWrite(BUZZER_PIN,LOW);

 showTotals(); 
 
 Serial.println("Out Setup");
}

//////////////////////////////////////////////////////////////////////////////////
//
// base loop, implements root of menu system
//
// allows the user to scroll up and down through summaries of the recorded sessions
//
//////////////////////////////////////////////////////////////////////////////////
void loop()

  // lets keep control of the loop
  while(true)
  {
   // wait for a key command to tell us what to do
   Serial.println("Beginning Loop");
   switch(waitForKeyPress(KEYPRESS_ANY))
   {
    // start recording
    case KEY_OK:
     doRecord();
     break;
    // delete all sessions
    case KEY_CANCEL:
     doConfirmDeleteSessions();
     break;
    // scroll through recorded session summaries
    case KEY_UP:
    case KEY_DOWN:
     doShowSessionSummaries();
     break;
   }

   showTotals();
   
   waitForKeyRelease();
  }
}

//////////////////////////////////////////////////////////////////////////////////
//
// doRecord
//
// start recording new sessions, update screen every second
// check for new laps
// record new laps
// show lap time for a few seconds at the end of a lap
// update and show new best lap if its a new session best
//
//////////////////////////////////////////////////////////////////////////////////
void doRecord()
{
  lap_handle_t currentLapHandle = gLapTimes.createNewSession();
 
  uint32_t ulOldLapStartTime = millis(); 
  lap_time_t bestLapTime = 0XFFFF;
 
  uint32_t ulLastTimeRefresh = millis();
  char *pStringTimeBuffer = NULL;
 
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print(getRamString(PSTR("Recording")));

  attachInterrupt(0,captureLap,CHANGE);
 
  while((getKeys() != KEY_CANCEL) && (currentLapHandle != INVALID_LAP_HANDLE))
  {
    Serial.println(ulPulseDuration);   
    //////////////////////////////////////////////////////////////////////////////////////////////////////
    // Check for new laps captured
    //////////////////////////////////////////////////////////////////////////////////////////////////////
    if((IR_PULSE_END_SET|IR_PULSE_START_SET) == bIRPulseFlags)
    {
      uint32_t ulLastLapDuration = ulNewLapStartTime - ulOldLapStartTime;
      ulOldLapStartTime = ulNewLapStartTime;
     
      lap_time_t lapTime = CLapTimes::convertMillisToLapTime(ulLastLapDuration);

      gLapTimes.addLapTime(currentLapHandle,lapTime);       
      currentLapHandle = gLapTimes.moveNext(currentLapHandle);       

      // new best lap     
      if(lapTime < bestLapTime)
      {
        bestLapTime = lapTime;
      }
     
      lcd.clear();
      lcd.print(getRamString(PSTR("Best Lap ")));
      lcd.print(CLapTimes::formatTime(bestLapTime,true));
     
      lcd.setCursor(0,1);
      lcd.print(getRamString(PSTR("Last Lap")));
      // use this to show lap time
      lcd.print(CLapTimes::formatTime(lapTime,true));
      // or this to show delta time
      //lcd.print(CLapTimes::formatTime(lapTime-bestLapTime,true));
     
      digitalWrite(LAP_CAPTURE_LED,HIGH);
      digitalWrite(BUZZER_PIN,HIGH);
      delay(400);
     
      if(lapTime == bestLapTime)
      {
        digitalWrite(LAP_CAPTURE_LED,LOW);
        digitalWrite(BUZZER_PIN,LOW);
        delay(200);
        digitalWrite(LAP_CAPTURE_LED,HIGH);
        digitalWrite(BUZZER_PIN,HIGH);
        delay(400);
      }
     
      digitalWrite(LAP_CAPTURE_LED,LOW);
      digitalWrite(BUZZER_PIN,LOW);
                     
      // dont look for another lap for 2 seconds
      delay(2000); 
     
      // give ownership of the shared variables back to the ISR
      bIRPulseFlags = 0;
    }
       
    //////////////////////////////////////////////////////////////////////////////////////////////////////
    // Update screen with current lap time
    //////////////////////////////////////////////////////////////////////////////////////////////////////
    uint32_t ulCurrentLapTime = millis();
    if((ulCurrentLapTime - ulLastTimeRefresh) > 1000)
    {
      ulLastTimeRefresh = ulCurrentLapTime;
     
      lcd.clear();
     
      if(bestLapTime != 0XFFFF)
      {
       lcd.print(getRamString(PSTR("Best Lap ")));
       lcd.print(CLapTimes::formatTime(bestLapTime,true));
      }
      else
      {
       lcd.print(getRamString(PSTR("Recording")));
      }
     
      pStringTimeBuffer = CLapTimes::formatTime(CLapTimes::convertMillisToLapTime(ulCurrentLapTime - ulOldLapStartTime),false);
      if(pStringTimeBuffer != NULL)
      {
        lcd.setCursor(0,1);
        lcd.print(pStringTimeBuffer);
      }
      else
      {
        // If we do not complete a lap for 9m59s display an idle message until a key is pressed
        lcd.setCursor(0,1);
        lcd.print(getRamString(PSTR("Idle")));
        waitForKeyPress(KEYPRESS_ANY);
        ulOldLapStartTime = millis();
      }
    }
  }
 
  if(currentLapHandle == INVALID_LAP_HANDLE)
  {
    lcd.setCursor(0,1);
    lcd.print(getRamString(PSTR("Memory Full!")));
  }
}

//////////////////////////////////////////////////////////////////////////////////
//
// doConfirmDeleteSessions
//
// Delete all sessions - if we are using storage for the first time we may need
// to call this function to initialise the storage to a known value. The user
// can access this function by pressing cancel on the root menu. This will bring
// up a confirmation message asking the user to press ok to delete all laps.
//
//////////////////////////////////////////////////////////////////////////////////
void doConfirmDeleteSessions()
{
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print(getRamString(PSTR("OK to Reset")));
  lcd.setCursor(0,1);
  lcd.print(getRamString(PSTR("Cancel to go Back")));

  // we pressed cancel to get here - so lets wait for cancel to be released before we look for more input
  waitForKeyRelease();
 
  if(KEY_OK == waitForKeyPress(KEY_OK|KEY_CANCEL))
  {
   gLapTimes.clearAll();
  }
}

//////////////////////////////////////////////////////////////////////////////////
//
// doShowSessionSummaries
//
// implements the show session summary menu
// allows the user to scroll up and down through summaries of the recorded sessions
// user can press ok to enter the session and scroll through the session laps
//
//////////////////////////////////////////////////////////////////////////////////
void doShowSessionSummaries()
{
 boolean bFinished = false;
 uint8_t nSession = 0;

 do
 {
  lap_handle_t lapHandle = 0;
  uint16_t nSessionAverage = 0;
  uint16_t nSessionBest = 0;
  uint16_t nSessionLapCount = 0;

  Serial.println(nSession);
  
  lapHandle = gLapTimes.getSessionHandle(nSession); 
 
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print(getRamString(PSTR("SNo:")));
  lcd.print(nSession);
 
  // if theres no laps for this session or its the first session but it doesnt contain any laps
  if(lapHandle == INVALID_LAP_HANDLE || (lapHandle == 0 && gLapTimes.getLapTime(lapHandle)==0))
  {
    lcd.setCursor(0,1);
    lcd.print(getRamString(PSTR("Empty Session")));
  }
  else
  {
    Serial.println(lapHandle);
   
    gLapTimes.getSessionSummary(lapHandle,nSessionAverage,nSessionBest,nSessionLapCount);
   
    lcd.print(getRamString(PSTR(" Laps:")));
    lcd.print(nSessionLapCount);
    lcd.setCursor(0,1);
// Best Lap Time
    lcd.print(CLapTimes::formatTime(nSessionBest,true));
// Average Lap Time
    lcd.print(" ");
    lcd.print(CLapTimes::formatTime(nSessionAverage,true));
  }

  waitForKeyRelease();

  switch(waitForKeyPress(KEYPRESS_ANY))
  {
   case KEY_UP:
    nSession++;
    break;
   case KEY_DOWN:
    nSession--;
    break;
   case KEY_CANCEL:
    bFinished = true;
    break;
   case KEY_OK:
    if(nSessionLapCount != 0)
    { 
      doLapScroll(gLapTimes.getSessionHandle(nSession));
    }
    break;
  }
 }while(!bFinished);
}

//////////////////////////////////////////////////////////////////////////////////
//
// showTotals shows the number of sessions, laps and laps left
// as the root of the menu
//
//////////////////////////////////////////////////////////////////////////////////
void showTotals()
{
 Serial.println(getRamString(PSTR("Entering showTotals")));

 uint16_t nSessions = 0;
 uint16_t nLapsRecorded = 0;
 uint16_t nLapsRemaining = 0;

 gLapTimes.getTotals(nSessions,nLapsRecorded,nLapsRemaining);

 lcd.clear();
 lcd.print(getRamString(PSTR("Sessions=")));lcd.print(nSessions);
 lcd.setCursor(0, 1);
 lcd.print(getRamString(PSTR("Laps=")));lcd.print(nLapsRecorded);
 lcd.print(getRamString(PSTR("Left=")));lcd.print(nLapsRemaining);
 
 Serial.println(getRamString(PSTR("Leaving showSummaryData")));
}

//////////////////////////////////////////////////////////////////////////////////
//
// doLapScroll
//
// scroll through the laps within a session, startLapHandle points to the start
//
//////////////////////////////////////////////////////////////////////////////////
void doLapScroll(lap_handle_t startLapHandle)
{
 boolean bFinished = false;
 lap_handle_t currentLapHandle = startLapHandle;
 lap_handle_t tmpLap = currentLapHandle;
 uint8_t nLapNumber = 0;

 do
 {
   lcd.clear();
   lcd.setCursor(0,0);
  
   if(tmpLap == INVALID_LAP_HANDLE)
   {
     lcd.print(getRamString(PSTR("No More Laps")));
     delay(2000);
     lcd.clear();
   }

   lcd.print(getRamString(PSTR("Lap No.")));
   lcd.print(nLapNumber);
   lcd.setCursor(0,1);
  
   if(currentLapHandle != INVALID_LAP_HANDLE)
   {
     char *pTime = CLapTimes::formatTime(gLapTimes.getLapTime(currentLapHandle),true);
     lcd.setCursor(0,1);
     lcd.print(pTime); 
   }

   waitForKeyRelease();
  
   uint8_t sKey = waitForKeyPress(KEYPRESS_ANY);
   switch(sKey)
   {
     case KEY_DOWN:
     case KEY_UP:
      (sKey == KEY_UP) ? tmpLap = gLapTimes.moveNext(currentLapHandle) : tmpLap = gLapTimes.movePrevious(currentLapHandle);
      if(tmpLap != INVALID_LAP_HANDLE)
      {
        if(gLapTimes.getLapTime(tmpLap) != EMPTY_LAP_TIME)
        {
          currentLapHandle = tmpLap;
          (sKey == KEY_UP) ? nLapNumber++ : nLapNumber--;
        }
        else
        {
          tmpLap = INVALID_LAP_HANDLE;
        }
      }
      break;
     case KEY_OK:
      tmpLap = currentLapHandle;
      break;
     case KEY_CANCEL:
      bFinished = true;
      break;
   }
 }
 while(!bFinished);
}

//////////////////////////////////////////////////////////////////////////////////
//
// Key related helpers
//
// getKeys - pole keys
// waitForKeyPress - block waiting for keys based on a mask
// waitForKeyRelease - block waiting until no kets are pressed
//
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
//
// getKeys
//
// read the inputs and create a bit mask based on the buttons pressed
// this does not block, need to review whether we should make this block, in most
// cases we loop waiting for a key, sometimes we also loop waiting for no key
// could put both options here with an input parameter.
//
//////////////////////////////////////////////////////////////////////////////////
short getKeys()
{
 // Use bit flags for keys, we may have a future use for
 // combined key presses

 short sKeys = KEY_NONE;
 if(digitalRead(KEY_UP_PIN) == LOW)
 {
  sKeys |= KEY_UP;
 }
 if(digitalRead(KEY_DOWN_PIN) == LOW)
 {
  sKeys |= KEY_DOWN;
 }
 if(digitalRead(KEY_OK_PIN) == LOW)
 {
  sKeys |= KEY_OK;
 }
 if(digitalRead(KEY_CANCEL_PIN) == LOW)
 {
  sKeys |= KEY_CANCEL;
 }

 return sKeys;
}

//////////////////////////////////////////////////////////////////////////////////
//
// waitForKeyRelease
//
// we can enter a function while the activating key is still pressed, in the new
// context the key can have a different purpose, so lets wait until it is released
// before reading it as pressed in the new context
//
//////////////////////////////////////////////////////////////////////////////////
void waitForKeyRelease()
{
  do
  {
    // do nothing
  }
  while(getKeys() != KEY_NONE);

  // debounce
  delay(20);
}

//////////////////////////////////////////////////////////////////////////////////
//
// waitForKeyPress
//
// convenience function, loop doing nothing until one of the sKeyMask keys is
// pressed
//
//////////////////////////////////////////////////////////////////////////////////
uint8_t waitForKeyPress(uint8_t sKeyMask)
{
  uint8_t sKey = KEY_NONE;
 
  do
  {
    sKey = getKeys() & sKeyMask;   
  }
  while(sKey == KEY_NONE);
 
  digitalWrite(BUZZER_PIN,HIGH);
  delay(20);
  digitalWrite(BUZZER_PIN,LOW);
 
  return sKey;
}

// A helper that copies a string from program memory into a buffer in sram
// we need this because our code can only use strings held in sram
// this fetches strings that are stored in program memory as and when
// we need them.
char * getRamString(PGM_P pString)
{
  // NEED TO ADD A CHECK HERE TO ENSURE pString < DISPLAY_ROW_LENGTH
 
  static char pBuffer[DISPLAY_ROW_BUFFER_LENGTH];
 
  return strcpy_P(pBuffer,pString);
}

//////////////////////////////////////////////////////////////////////////////////
//
// captureLap
//
// In part 1 we will fake a lap if we detect INT0 being pulled low
// In part 2 we will add a simple IR Detector which you can test with
// a TV Remote
// In part three we will add a more complex IR Detector which will allow
// us to detect a transponder and ignore any other IR Signal
//////////////////////////////////////////////////////////////////////////////////
void captureLap()
{
  uint8_t bLapCaptureState = digitalRead(2);
  digitalWrite(LAP_CAPTURE_LED,bLapCaptureState);
  if(bLapCaptureState == LOW)
  {
     bIRPulseFlags = (IR_PULSE_END_SET|IR_PULSE_START_SET);
     ulNewLapStartTime = millis();
  }
  else
  {
    bIRPulseFlags = 0;
  }
}


 // LapTimes.cpp

#include "arduino.h"
#include "laptimes.h"
#include <../../../../libraries/EEPROM/EEPROM.h>

// Does what it says, gets a lap time from EEPROM - does not do any validation
lap_time_t CEEPROMLapStore::getLapTime(lap_handle_t lapHandle)
{
  lap_time_t lapTime = (EEPROM.read((lapHandle*sizeof(uint16_t))+1)<<8);
  lapTime += EEPROM.read(lapHandle*sizeof(uint16_t));
 
  return lapTime;
}

// Does what it says, sets a lap time in EEPROM - does not do any validation
void CEEPROMLapStore::setLapTime(lap_handle_t lapHandle,lap_time_t lapTime)
{
  EEPROM.write(lapHandle*sizeof(uint16_t),lowByte(lapTime));
  EEPROM.write((lapHandle*sizeof(uint16_t))+1,highByte(lapTime));
}

// Initialise the lap store to EMPTY_LAP_TIME through out
// this is an important function, we find empty space by looking
// for one EMPTY_LAP_TIME that defines the end of a session followed
// immediatley by another EMPTY_LAP_TIME, this show that there
// are not sessions following the previous session in which case
// we are free to create a new session.
// We cannot be sure what SD, Memory or EEPROM will contain on the first run
// and so it is important we have this option to initialise the storage to
// a known value.
void CEEPROMLapStore::clearAll()
{
  for(uint16_t unIndex = 0;unIndex < (getMaxLaps()*sizeof(lap_time_t));unIndex++)
  {
    EEPROM.write(unIndex,EMPTY_LAP_TIME);
  }
}

// Return the maximum number of laps for this storage media (or device ATMega8,328,1240 etc)
uint16_t CEEPROMLapStore::getMaxLaps()
{
  return EEPROM_LAP_STORE_MAX_LAPS;
}


//*******************************************************************************************
// CLapTimes
//
// A lot of the work in this class is simply finding the start and end of sessions, and
// finding space to start a new session.
//
// With more memory I would have used headers to do a lot of the work inside CLapTimes
// A file system could also have done a lot of the work.
//
// It isn't pretty and could be refactored but it works.
//
//*******************************************************************************************

// Initialise CLapTimes which whichever class we want to provide the actual lap storage
// all lap storage is through the ILapStore interface and so we can use any class
// that implements this interface. Only CEEPROMLapStore is provided in this release,
// others may follow
CLapTimes::CLapTimes(ILapStore *pLapStore)
{
  m_pLapStore = pLapStore;
}
 
// The end of a session is marked by an empty lap (0)
// to create a new session, we first look at the very first lap, if its invalid, there are no sessions
// and we can start a new one from position 0.
// if there is a valid lap at position 0 we need to scan for two consecutive invalid laps. A single invalid lap indicates
// the end of an existing session, if this is followed by anything other than an invalid lap, it is the beginning
// of a new session, if its followed by an invalid lap then we have found the end of the existing sessions and
// can use the second invalid lap handle as the start of our new session.
lap_handle_t CLapTimes::createNewSession()
{
  lap_handle_t newSessionLapHandle = 0;
  lap_handle_t currentLapHandle = 0;
 
  // if the first lap is a valid lap - we need to scan through the recorded laps
  // and sessions to find two consecutive invalid laps - the first we leave in place to
  // mark the end of the existing sessions, the second is a free space for us to create
  // a new session.
  if(m_pLapStore->getLapTime(newSessionLapHandle) != EMPTY_LAP_TIME)
  {
    // assume the worst - there is no space left
    newSessionLapHandle = INVALID_LAP_HANDLE;
   
    // loop until we have a valid lap handle or we reach the end of the lap store
    while(newSessionLapHandle == INVALID_LAP_HANDLE && currentLapHandle < m_pLapStore->getMaxLaps())
    {
     // loop until we reach the end of the lap store or we find an empty lap time
     while(currentLapHandle < m_pLapStore->getMaxLaps() && (m_pLapStore->getLapTime(currentLapHandle) != EMPTY_LAP_TIME))
     {
       currentLapHandle++;
     };
    
     // we found an invalid lap, so check the the next lap handle is less than the end of the lap store
     // and that the content of the next lap is an empty lap meaning it is free for us to use
     if(((currentLapHandle+1)<m_pLapStore->getMaxLaps()) && (m_pLapStore->getLapTime(++currentLapHandle) == EMPTY_LAP_TIME))
     {
       // Yay ! we got two consecutive empty laps so lets set the firstLapHandle so we can start our new session.
       newSessionLapHandle = currentLapHandle;
     }
  }
}

return newSessionLapHandle;
}

void CLapTimes::setLapTime(lap_handle_t lapHandle,lap_time_t lapTime)
{
  m_pLapStore->setLapTime(lapHandle,lapTime);
}

lap_time_t CLapTimes::getLapTime(lap_handle_t lapHandle)
{
  return  m_pLapStore->getLapTime(lapHandle);
}

// scan through all of the recorded laps, total the number of sessions, total the number of laps
// recorded and return and indicative number of remaining laps - its indicative becuase
// each session requires on end of session marker so 10 sessions of 5 laps takes 60 laps
// (10 * 5 laps + 10 end of session markers) one session of 5 sessions of 10 laps takes 55 laps
// (5 * 10 + 5 end of session markers)
void CLapTimes::getTotals(uint16_t &nSessions,uint16_t &nLapsRecorded,uint16_t &nLapsRemaining)
{
  lap_handle_t lapHandle = 0;
  nSessions = 0;
  nLapsRecorded = 0;
  nLapsRemaining = 0;
  
  while(lapHandle < m_pLapStore->getMaxLaps() && (m_pLapStore->getLapTime(lapHandle) != EMPTY_LAP_TIME))
  {
    // we have a session so count it
    nSessions++;
    // and count the laps within the session
    while(lapHandle < m_pLapStore->getMaxLaps() && (m_pLapStore->getLapTime(lapHandle++) != EMPTY_LAP_TIME))
    {
      nLapsRecorded++;
    }
  }
 
  nLapsRemaining = m_pLapStore->getMaxLaps() - nLapsRecorded;
}

void CLapTimes::clearAll()
{
  m_pLapStore->clearAll();
}

// This is similar to get totals but works within a session only, returns the average of all laps in the session,
// the best lap and the total number of laps  
lap_handle_t CLapTimes::getSessionSummary(lap_handle_t lapHandle,uint16_t &nSessionAverage,uint16_t &nSessionBest,uint16_t &nSessionLapCount)
{
  nSessionAverage = 0;
  nSessionBest = 0xFFFF;
  nSessionLapCount = 0;
 
  lap_time_t nLapTime = 0;
  uint32_t nTotalTime = 0;

  while((INVALID_LAP_HANDLE != (nLapTime = m_pLapStore->getLapTime(lapHandle))) && (nLapTime != EMPTY_LAP_TIME))
  {
    nTotalTime += nLapTime;
   
    if(nLapTime < nSessionBest)
    {
      nSessionBest = nLapTime;
    }
   
    nSessionLapCount++;
   
    lapHandle++;
  }

  nSessionAverage = nTotalTime/nSessionLapCount;
 
  return nLapTime;
}

lap_handle_t CLapTimes::addLapTime(lap_handle_t lapHandle,lap_time_t lapTime)
{
  if(lapHandle < m_pLapStore->getMaxLaps())
  {
    m_pLapStore->setLapTime(lapHandle,lapTime);
  }
  else
  {
    lapHandle = INVALID_LAP_HANDLE;
  }

return lapHandle;
}
  
lap_handle_t CLapTimes::moveNext(lap_handle_t lapHandle)
{
  if(lapHandle < m_pLapStore->getMaxLaps())
  {
    lapHandle++;
  }
  else
  {
    lapHandle = INVALID_LAP_HANDLE;
  }
  
  return lapHandle;
}
  
lap_handle_t CLapTimes::movePrevious(lap_handle_t lapHandle)
{
  if(lapHandle >= 1)
  {
    lapHandle--;
  }
  else
  {
    lapHandle = INVALID_LAP_HANDLE;
  }

  return lapHandle;
}

// given a session number, find it the start of the session and return a handle to it  
lap_handle_t CLapTimes::getSessionHandle(uint8_t nSession)
{
  lap_handle_t currentLapHandle = 0;
  uint8_t nCurrentSession = 0;
  uint16_t nLapTime = 0;
  
  while(nCurrentSession != nSession && currentLapHandle < m_pLapStore->getMaxLaps())
  {
    // loop until we read the max laps or we find and empty lap
    do
    {
      currentLapHandle++; 
    }
    while((currentLapHandle) < m_pLapStore->getMaxLaps() && m_pLapStore->getLapTime(currentLapHandle) != EMPTY_LAP_TIME);
    
    nCurrentSession++;
    
    if(currentLapHandle < m_pLapStore->getMaxLaps())
    {
      // move next to step over the 0 terminator for the previous session   
      currentLapHandle++;
      
      // if the first lap of the session is empty there is no session
      // so return invalid lap to indicate no session found.
      if(getLapTime(currentLapHandle) == EMPTY_LAP_TIME)
      {
        currentLapHandle = INVALID_LAP_HANDLE;
      }
    }
    else
    {
      currentLapHandle = INVALID_LAP_HANDLE;
    }
  }
  
  return currentLapHandle;
}

// 10 minutes is 600 seconds or 600,000 milli seconds, this is too big to fit into a uint32_t
// so we divide by 10 to convert the value into a lap_time_t which contains the lap time in 100's
// of seconds.
lap_time_t CLapTimes::convertMillisToLapTime(uint32_t ulTime)
{
  return ulTime/10;
}

// turn a lap_time_t into a time string formatted as - m:ss:dd
// bPrecision turns 100's on or off
char* CLapTimes::formatTime(lap_time_t time,unsigned char bPrecision)
{
  char *pResult = NULL;

  lap_time_t nSeconds = time/100;
  lap_time_t nMinutes = nSeconds/60;
  lap_time_t nHundredths = 0;

  if(nMinutes <= 9)
  {
    if(bPrecision)
    {    
      nHundredths = time - (nSeconds*100);
    }

    nSeconds -= (nMinutes * 60);  

    m_pTimeStringBuffer[7] = 0;
    m_pTimeStringBuffer[6] = (nHundredths%10)+'0';
    m_pTimeStringBuffer[5] = (nHundredths/10)+'0';
    m_pTimeStringBuffer[4] = '.';
    m_pTimeStringBuffer[3] = (nSeconds%10)+'0';
    m_pTimeStringBuffer[2] = (nSeconds/10)+'0';
    m_pTimeStringBuffer[1] = ':';
    m_pTimeStringBuffer[0] = nMinutes + '0';   
   
    pResult = m_pTimeStringBuffer;
  }
 
  return pResult;
}
 
char CLapTimes::m_pTimeStringBuffer[9];/*m:ss:dd - dd represents hundredths of a second */









// LapTimes.h

// If we assume that lap data will always be set to 0
// Session ends will always be 0
// we only ever return invalid handle

//*******************************************************************************************
// Lap storage and retreival definitions
//*******************************************************************************************
#define EMPTY_LAP_TIME 0
#define INVALID_LAP_HANDLE 0XFFFF

typedef uint16_t lap_handle_t;
typedef uint16_t lap_time_t;

//*******************************************************************************************
// ILapStore
//
// Defines a pure virtual class (C++ Terminology) or interface (Java Terminology)
// It simply defines the functions that can be used to get, set and clear laps
// in a lap store.
//
// The following lap store is provided
// 1) CEEPromLapStore - this one stores laps in the EEPROM
// If you wanted to add SD Card Storage you could define a new class CSDCardLapStore
//
//*******************************************************************************************
class ILapStore
{
public:
  virtual lap_time_t getLapTime(lap_handle_t lapHandle) = 0;
  virtual void setLapTime(lap_handle_t lapHandle,lap_time_t lapTime) = 0;
  virtual void clearAll() = 0;
  virtual uint16_t getMaxLaps() = 0;
};

//*******************************************************************************************
// CEEPROMLapStore
//
// Store laps in memory -
//
// For - simple, easy, its just using an array in memory
// Against - lose all laps if power is lost or Arduino is reset
//
//*******************************************************************************************
#define EEPROM_LAP_STORE_MAX_LAPS 500

class CEEPROMLapStore : public ILapStore
{
public:
 virtual lap_time_t getLapTime(lap_handle_t lapHandle);
 virtual void setLapTime(lap_handle_t lapHandle,lap_time_t lapTime);
 virtual void clearAll();
 virtual uint16_t getMaxLaps();
protected:
 lap_time_t m_LapTimes[EEPROM_LAP_STORE_MAX_LAPS];
};

//*******************************************************************************************
// CLapTimes
//
// A lot of the work in this class is simply finding the start and end of sessions, and
// finding space to start a new session.
//
// With more memory I would have used headers to do a lot of the work inside CLapTimes
// A file system could also have done a lot of the work.
//
// It isn't pretty and could be refactored but it works.
//
//*******************************************************************************************
class CLapTimes
{
public:
  CLapTimes(ILapStore *pLapStore);
  lap_handle_t createNewSession();
  void setLapTime(lap_handle_t lapHandle,lap_time_t lapTime);
  lap_time_t getLapTime(lap_handle_t lapHandle);
  void getTotals(uint16_t &nSessions,uint16_t &nLapsRecorded,uint16_t &nLapsRemaining);
  void clearAll();
  lap_handle_t getSessionSummary(lap_handle_t lapHandle,uint16_t &nSessionAverage,uint16_t &nSessionBest,uint16_t &nSessionLapCount);
  lap_handle_t addLapTime(lap_handle_t lapHandle,lap_time_t lapTime);
  lap_handle_t moveNext(lap_handle_t lapHandle);
  lap_handle_t movePrevious(lap_handle_t lapHandle);
  lap_handle_t getSessionHandle(uint8_t nSession);
  static lap_time_t convertMillisToLapTime(uint32_t ulTime);
  static char* formatTime(lap_time_t time,unsigned char bPrecision);
protected:
  ILapStore *m_pLapStore;
public:
  static char m_pTimeStringBuffer[9];/*m:ss:dd - dd represents hundredths of a second */
};
 



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);
}