Saturday, June 1, 2013

RC Arduino Traction Control - The Traction Alarm - Draft



In order to control traction we first need to detect when traction has failed. This post presents a hardware and software solution for efficiently detecting when traction is lost.

One of the challenges with detecting a loss of traction is that there is a lot happening. The RCArduino test vehicle generates two interrupts for each revolution of each wheel. With the four wheels rotating as much as 80 times per second, thats 2 * 4 * 80 = 640 interrupts per second. If we add two or three RC Channels we quickly approach 1000 interrupts per second.

It might sound like a lot to process, 1000 interrupts in a second, but the 16Mhz processor in your Arduino is able to get through 16,000 operations in each one thousandth of a second.



Bench Testing

 
Road Testing
More lights = Less traction. You can see the car light up on acceleration and the lights go one by one as traction is recovered.


If the processor is so fast, why do we need to focus on efficiency ?

The problem is that there will be times when all of our interrupts will occur at the same time. The Arduino is only able to process one interrupt at a time and so a queue will be formed. The interrupts within the queue are processed according to a fixed priority, not the time at which they occured.

The interrupts we are interested in are listed below according to the priority with which they are processed

INT0 - External Interrupt 0 - used to read RC Channels or wheel rotations
INT1 - External Interrupt 1- used to read RC Channels or wheel rotations
PCINT0 - Pin change interrupts - used to read RC Channels or wheel rotations
PCINT1 - Pin change interrupts - used to read RC Channels or wheel rotations
PCINT2 - Pin change interrupts - used to read RC Channels or wheel rotations
TIMER1COMPA - Used by the servo library to generate output pulses

If each type of interrupt takes 10 microseconds to process, we could have the signals we want to send to our ESC or steering servo (TIMER1COMPA in the table above) disrupted by upto 50 microseconds which can represent as much as a 10 percent error in the throttle and steering signals.



You can find a full list of the interrupt vectors and the order in which they are processed in the ATMega328 Datasheet under the second '11.4 Interrupt Vectors in ATmega328P' in the table 'Table 11-6. Reset and Interrupt Vectors in ATmega328'

You can find more background on clashing/queuing  interrupts in this post introducing the RCArduinoFastLib which includes an explanation of the diagram below -

Error introduced when interrupts collide

Reducing interrupt clashes in Arduino RC Projects -
http://rcarduino.blogspot.ae/2012/11/how-to-read-rc-channels-rcarduinofastlib.html


Pin Change Interrupts Vs Port Change Interrupts

The Arduino UNO, Leonardo, Mini and Micro have only two external interrupts, so how will we read four wheel sensors and two or more RC Channels ?

In the past the RCArduinoBlog has featured the pinchangeinterrupt library, this uses a capability of the ATMega chip in your Arduino to attach an interrupt to any of the 19 pins. The pinchangeint library adds higher level code to this low level capability in order to provide very similar functionality to the two external interrupts.

More here - http://rcarduino.blogspot.com/2012/03/need-more-interrupts-to-read-more.html

This convenience comes at a high cost in terms of performance but fortunately there is a simple trick we can use to access more interrupts without the performance hit of the pinchangeint library.

Introducing Port Change Interrupts  - A Simple Trick

If we limit ourselves to using just one pin from each of the Arduino ports we can eliminate all of the processing overhead normally required to determine which pin has caused the interrupt. This approach provides us with a total of five highly efficient interrupts, enough for four wheel sensors and a PPM Stream containing our RC Channel information.

Ok, but whats a port ?


The ATMega microprocessor in your Arduino has its pins arranged into ports, the following diagram shows how the Arduino software maps digital and analogue pins onto the ATMega 328 microprocessor ports. Similar diagrams are available for the ATMega 32u4 and ATMega 2560 used in the Leonardo and Mega.


The diagrams all show the pin function assigned by Arduino in red accompanied by black text which gives the ATMega port, pin number and any additional functions supported by the pin.

If we look at the Arduino pin labelled digital pin 2 we can see that nearest the pin, this is also labelled PD2, this tells us that the pin is pin number 2 on PORT D.

The ATMega 328 has three ports -

   PORTB (digital pin 8 to digital pin 13)
   PORTC (analog pin 0 to analog pin 5)
   PORTD (digital pin 0 to digital pin 7)

To create a port change interrupt we must select a single pin from a port. If we try to use more than one pin on a single port we will need additional code to determine which pin has changed, this will quickly use up our performance advantage.

As an example of the cost of having multiple pin change interrupts on a single port, the pinchangeint library is around 5 times less efficient than an external interrupt.

If we limit ourselves to this one pin per port approach we can have three additional interrupts with no lose in performance.

A Quick Introduction To Direct Port Manipulation and Registers



Have you ever seen code that looks like the following -

PORTB ^= (1<<5);

It might look like low level assembly but it isn't, its accessing the chip registers directly and there are two reasons why we would want to do this -

1) Performance - direct access is very fast, the code sample above could be 10 times faster than calling the equivalent digitalWrite(13,!digitalRead(13));


2) Configuration - Registers set up and control each of the chips modules, sometimes we will need to access a module which is not set up by default.


Registers can be thought of as a set of related switches which can be turned on and off to control some part of the chips behavior. Once you know the registers used by the modules your interested in, its as simple as programming the push button interface of a kitchen appliance.

Lets walk through the set up we need for the pin change interrupts used in the traction control project.

Step One - Turning on pin change interrupts
Pin change interrupts are turned on through the PCICR (Pin Change Interrupt Control Register). Keep in mind that this register is just a collection of on/off switches, in this register there are only three switches named PCIE2, PCIE1 and PCIE0. These three switches turn the pin change interrupt module on or off for PORTD, PORTC and PORTB respectively -

   PORTB (digital pin 8 to digital pin 13)
   PORTC (analog pin 0 to analog pin 5)
   PORTD (digital pin 0 to digital pin 7)


As our project will require pin change interrupts to be enabled on all three ports, we will need to turn the three switches on by setting the corresponding bit in the register to 1, it looks like this -

PCICR = (1<<PCIE2)|(1<<PCIE1)|(1<<PCIE0);

Each useable bit in every register is assigned a name in the datasheet, when we use the bit name in our code, it evaluates to the bit number in the register, not the bit value. To convert the bit number to the bit value we use the following notation

bit value = (1 << bit number)

We can combine values using the bitwise or operator '|' and thats how we arrived at the code above (and below) -

PCICR = (1<<PCIE2)|(1<<PCIE1)|(1<<PCIE0); // turn on PCIE2, PCIE1 and PCIE0 in the PCICR register


Step Two - Choosing our interrupt pins and linking them to a pin change interrupt 

We have discussed that there is a major performance benefit in enabling just one pin change interrupt on each port and this is easily done through setting the corresponding but in a mask register however before we do this we should return to the pin mapping diagram and take a closer look at the secondary functions of each pin.

The text between the brackets indicates the additional functions supported by each pin, for example analog input 0 has ADC0 and PCINT8 (analog to digital converter 0 and pin change interrupt 8).

We have two goals in selecting the pins -

1) Choose the pins so that we only enable one from each of the ports
2) Choose pins where we do not expect to use the secondary function later in our project - I might want to use SPI for data logging for example so must avoid these pins.

For now I have selected the following pins to enable our port change interrupts - 

  digital pin 4 (PCINT20,XCK,T0) // PD4, pin 4 on PORT D
  digital pin 8 (PCINT0,CLKO/ICP1) // PB0, pin 0 on PORT B
  analog input 0(ADC0,PCINT8) // PC0, pin 0 on PORT C
Now that we have our pins, we can use the mask registers associated with each port to attach the interrupt.
PCMSK0 = (1 << PCINT0 ); // enable pin change interrupts on PCINT0 contained in register PCMSK0
PCMSK1 = (1 << PCINT8); // enabled pin change interrupts on PCINT8 contained in register PCMSK1
PCMSK2 = (1 << PCINT20); // enabled pin change interrupts on PCINT20 contained in register PCMSK2



All of this information, including the chip modules, the registers associated with each module and the individual switches within the registers can be found in the datasheet. In this case we are dealing with PCICR and the PCMSKn registers found in sections 13.2.5 to 13.2.8 of the ATMega328 Datasheet.

http://www.arduino.cc/en/Reference/PortManipulation



The code -

#define IDLE_PERIOD 200

#define FLAG_FRONT_LEFT 1
#define FLAG_REAR_LEFT 2
#define FLAG_REAR_RIGHT 4
#define FLAG_FRONT_RIGHT 8


volatile uint16_t unPeriodFR;
volatile uint16_t unPeriodFL;
volatile uint16_t unPeriodRR;
volatile uint16_t unPeriodRL;

volatile uint8_t sFlags;

#define GREEN_INDICATOR 9
#define YELLOW_INDICATOR 10
#define RED_INDICATOR 11

void setup() {               
  pinMode(3,INPUT);
  pinMode(4,INPUT);
  pinMode(8,INPUT);
  pinMode(A0,INPUT);
  pinMode(6,OUTPUT);

  pinMode(GREEN_INDICATOR,OUTPUT);
  pinMode(YELLOW_INDICATOR,OUTPUT);
  pinMode(RED_INDICATOR,OUTPUT);

  // enable the external interrupt
  EIMSK = 2;  //1 - enable external interrupt 1
  EICRA = 12; //3 - enable interrupt on rising edge only
 
  // enable the pin change interrupts
  PCICR = _BV(PCIE0)|_BV(PCIE1)|_BV(PCIE2);
  PCMSK0 = 1;//PORT B
  PCMSK1 = 1;//PORT C
  PCMSK2 = 16;//PORT D

  TCCR1A = 0;
  TCCR1B=0x05;//Prescaler= 1024
 
  Serial.begin(9600); 
}

void loop() {
  static uint16_t unLocalPeriodFR; 
  static uint16_t unLocalPeriodFL;
  static uint16_t unLocalPeriodRR;
  static uint16_t unLocalPeriodRL;
 
  static uint32_t ulLastUpdate;
  static uint8_t sLocalFlags;     
  delay(100);

  // Take a temporary copy of the periods 
  uint8_t sreg = SREG;
  cli();

  unLocalPeriodFR = unPeriodFR;
  unLocalPeriodFL = unPeriodFL;
  unLocalPeriodRR = unPeriodFR;
  unLocalPeriodRL = unPeriodFL;
 
  sLocalFlags = sFlags & (FLAG_FRONT_RIGHT | FLAG_FRONT_LEFT);
  sFlags = 0;
 
  SREG = sreg;
 
  if(sLocalFlags)
  {
   ulLastUpdate = millis();
  
   uint16_t unTop = unLocalPeriodFR;
   
   if(unLocalPeriodFL > unTop)
   {
    unTop = unLocalPeriodFL;
   }
   if(unLocalPeriodRR > unTop)
   {
    unTop = unLocalPeriodRR;
   }
   if(unLocalPeriodRL > unTop)
   {
    unTop = unLocalPeriodRL;
   }
     
    uint16_t unBottom = unLocalPeriodFR;
    if(unLocalPeriodFL < unBottom)
    {
      unBottom = unLocalPeriodFL;
    }
    if(unLocalPeriodRR < unBottom)
    {
      unBottom = unLocalPeriodRR;
    }
    if(unLocalPeriodRL < unBottom)
    {
      unBottom = unLocalPeriodRL;
    }
     
    Serial.print(unTop);
    Serial.print(" ");
    Serial.print(unBottom);
    Serial.print(" ");
    Serial.println(unTop>>3);
   
    // if the difference is greater than 1/8th of top
    uint16_t unDifference = (unTop - unBottom);
    if(unDifference > (unTop >> 3))
    {
      tone(6,69);
    }
    else
    {
      noTone(6);
    }
   
    digitalWrite(GREEN_INDICATOR,unDifference > (unTop >> 3));  // difference > 1/8th of top
    digitalWrite(YELLOW_INDICATOR,unDifference > (unTop >> 2)); // difference > 1/4 of top
    digitalWrite(RED_INDICATOR,unDifference > (unTop >> 1));    // difference > 1/2 of top
  }
 
  if((millis() - ulLastUpdate) > IDLE_PERIOD)
  {
   noTone(6);
   digitalWrite(GREEN_INDICATOR,LOW);  // difference > 1/8th of top
   digitalWrite(YELLOW_INDICATOR,LOW); // difference > 1/4 of top
   digitalWrite(RED_INDICATOR,LOW);    // difference > 1/2 of top
  }
}

ISR(INT0_vect)
{

}

// WHEEL  ATMEGA pin, ports and masks
// FL     5           PORTD3    8 (also INT1)
// FR     6           PORTD4    16
// RR     8           PORTB0    1
// RL     A0          PORTC0    1

// WHEEL  ATMEGA pin, ports and masks
// FL     5           PORTD3    8 (also INT1)
ISR(INT1_vect)
{
  static uint16_t unLastTimer;
 
  if(PIND&8)
  {
    // debounce
    if((TCNT1-unLastTimer) > 100)
    {
      unPeriodFL = TCNT1-unLastTimer;
      // not strictly accurate, TCNT1 will have advanced between the previous line and this line,
      // however we are interested in the difference between our measurements which is not effected
      // by this constant error in each measurement unLastTimer = TCNT1;
      unLastTimer =TCNT1;
      sFlags |= FLAG_FRONT_LEFT;
    }
  }
}

// WHEEL  ATMEGA pin, ports and masks
// FR     6           PORTD4    16
ISR(PCINT2_vect)
{
  static uint16_t unLastTimer;
  if(PIND & 16)
  {
    // debounce
    if((TCNT1-unLastTimer) > 100)
    {
     unPeriodRL = TCNT1-unLastTimer;
     unLastTimer = TCNT1;
     sFlags |= FLAG_REAR_LEFT;
    }
  }  
}

// WHEEL  ATMEGA pin, ports and masks
// RR     8           PORTB0    1
ISR(PCINT0_vect)
{
  static uint16_t unLastTimer;
  if(PINB & 1)
  {
    // debounce
    if((TCNT1-unLastTimer) > 100)
    {
       unPeriodRR = TCNT1-unLastTimer;
      // not strictly accurate, TCNT1 will have advanced between the previous line and this line,
      // however we are interested in the difference between our measurements which is not effected
      // by this constant error in each measurement.
      unLastTimer = TCNT1;
      sFlags |= FLAG_REAR_RIGHT;
    }
  }
}

// WHEEL  ATMEGA pin, ports and masks
// RL     A0          PORTC0    1
ISR(PCINT1_vect)
{
  static uint16_t unLastTimer;
  if(PINC & 1)
  {
    // debounce
    if((TCNT1-unLastTimer) > 100)
    {
 
      unPeriodFR = TCNT1-unLastTimer;
      // not strictly accurate, TCNT1 will have advanced between the previous line and this line,
      // however we are interested in the difference between our measurements which is not effected
      // by this constant error in each measurement.
      unLastTimer = TCNT1;
      sFlags |= FLAG_FRONT_RIGHT;
    }
  }
}


Come back next time for track testing the control algorithms

Duane B (RCArduino)

No comments:

Post a Comment