ArduinoPulser

From arrizza.org wiki

Jump to navigation Jump to search
Previous ⇦ ArduinoInterrupts Arduino ⇫ Up ArduinoAccuratePulser ⇨ Next

Overview

To control a stepper motor, a series of pulses is sent to it. The goal of this project is to generate a stream of pulses from a GPIO pin at an accurate rate. So when I attach that GPIO pin to a Stepper Motor controller, the series of pulses will turn the stepper at a very accurate rate. For now the rate is controlled by simple commands from putty or from a ruby script.

There is a lower bound to how slowly we can pulse the stepper. See ArduinoAccuratePulser for a much better solution. Eventually, there may be a LCD panel that can control the pulse stream see ArduinoLcdTest and ArduinoTouch

This project also shows one way to do unit testing on an Arduino Sketch. It uses Google Test to run some test cases. See Tools and other Setup#Googletest to set it up.

Git repository

https://bitbucket.org/arrizza/arduinopulser

git clone git@bitbucket.org:arrizza/arduinopulser.git

Setup

The setup sets up the Serial Port for communication see ArduinoSerialInput and ArduinoSerialOutput.

It then initializes the Pulser and the Parser.

Pulser

The Pulser is used to perform the pulsing of the GPIO pin at a regular interval.

Pulser Init

The pulser_init function:

  • initializes GPIO pin 10
  • initializes all statistics to 0s
  • sets up Timer2 to run every 2ms, and sets up a separate prescaler to cause the overall timer interval to be 100ms.

Pulser Go!

When the Pulser gets a request to Go! from the Parser:

  • it temporarily disables interrupts
  • makes sure the GPIO pin is off
  • sets up a short array of slots to spread the pulses evenly across multiple ticks.
  • re-enables interrupts

The Pulser uses an array to spread the requested number of pulses across a longer time period. The current code uses an array with only 5 slots in it. Each slot is visited when the ISR interrupt occurs, in this case every 100ms.

A final version of this would use a larger number, say 1000 slots (or longer) in the array and each slot visited every 100ms. This would spread the total number of requested pulses across 100ms * 1000 = 100 seconds.

Right now the per-slot pulse calculation is simple: the total number of pulses divided by the number of slots in the pulse array. This works OK if the total number of pulses is evenly divisible by the number of slots. For example if the number of slots is 5 and the requested number of pulses is 10, then the number of pulses to do per slot is 2.

It doesn't work very well for other values. For example if the requested number of pulses is 9, then 9 / 5 is 1 in integer arithmetic. (I have to use integer arithmetic since I can't to a partial pulse!) So in this case, I end up only pulsing 1 time per slot and with 5 slots, that's only 5 times instead of the 9 that was requested. See ArduinoAccuratePulser and Low_Rate_Peristaltic_Pump#Pulse_Distribution for a solution.

Pulser Stop!

When the Pulser gets a request to Stop! from the Parser:

  • it temporarily disables interrupts
  • makes sure the GPIO pin is off
  • saves the current number of pulses to last
  • saves the current number of ticks to last
  • clears the pulse array - sets all slots to 0
  • re-enables interrupts

Pulser ISR

The ISR is triggered on every Timer2 interrupt. Currently that's set to 100ms.

The ISR routine does the following:

  • uses the prescaler to determine if it should pulse or not. If not, it simply returns
  • increments the number of ticks. In this case, each tick count means 100ms has elapsed.
  • it uses the value in the current slot to pulses the GPIO pin that many times. In other words, it is possible to achieve higher motor speeds by pulsing the pin and therefore the stepper, more times every tick.
  • Each pulses is 10uS
  • It increments the slot index to the next slot. If it is past the end of the array, it rolls back to the zeroth slot.

On an oscilloscope, this sequence shows up as a sequence of very fast pulses followed by a period of dead space.

ArduinoPulser- g10.jpg

If you give a Go! command with more pulses the width of the very fast pulses sequence gets wider.

ArduinoPulser- g100.jpg

Note: that the pulse width is 10uS. This may or may not be long enough for a particular stepper motor.

Also, each pulse immediately follows the previous pulse. The stepper motor may require a short reset after each pulse.

Parser

The Parser is used to parse the incoming commands from the ruby script (see auto_test.rb). The parser can accept the following commands:

  • g<n> <cr> : Go! start pulsing using n pulses in the pulse array
  • s <cr> : Stop! stop pulsing and capture the pulse count and elapsed time
  • n <cr> : Numbers! return statistics at the current moment

Parser Go!

Get the total number of pulses across all slots of the pulse array. <n> is in decimal format.

Example: g100<cr> will spread 100 pulses evenly through all slots in the pulse array.

It returns the per slot value and the total number of slots on success.

 g100
 ACK 20 5

Note: currently there is no bounds checking. You could enter "g1234567890" and it would accept it without errors or warnings. This number could, for example, overflow the variables used to convert it to an internal Arduino integer variable. Or it could overflow the maximum value an array slot can hold.

Some odd-ball things are checked. For example "g-10" will give you a warning about the "-".

More complex scenarios can cause a domino effect of errors. In "g-10" causes a warning about "-", but then the Parser resets itself and since "1" is not a valid command, it gives you another warning and resets itself. see #Notes for an alternative method.

Parser Stop!

Stops the pulsing by clearing the pulse array

Example:

 s
 ACK

Parser Numbers!

Returns statistics about the current pulsing

Example:

   n
   ACK 10 0 234 0<lf>

The statistics are:

  • Current number of pulses since Go! This can be 0 if Stop! was sent.
  • Current number of timer ticks since Go! This can be 0 if Stop! was sent.
  • The number of pulses when Stop! was last sent
  • The number of timer ticks when Stop! was last sent
  • The tick time in milliseconds. This is the ISR interrupt timeout. Currently it's set at 100ms.

Main Loop

The pulsing is done in the ISR, so main() simple looks at the serial port for anything to do.

Unit Tests

Run It

The unit tests are in a separate directory called "ut" and have their own Makefile.

cd ut
make

# to clean:
make clean

Typical Output

Passing

Here's what it looks like when everything is passing. The very bottom line says "PASSED".

$ make
mkdir -p build
g++ -I mock_arduino -fno-exceptions -pthread -c ut.cpp -o build/ut.o
g++ -pthread build/ut.o build/mock_arduino.o -lgtest -lgtest_main -o build/ut
build/ut
Running main() from gtest_main.cc
[==========] Running 9 tests from 6 test cases.
[----------] Global test environment set-up.
[----------] 1 test from pulser_info
[ RUN      ] pulser_info.init
[       OK ] pulser_info.init (0 ms)
[----------] 1 test from pulser_info (0 ms total)

[----------] 2 tests from stop
[ RUN      ] stop.happy_path
[       OK ] stop.happy_path (0 ms)
[ RUN      ] stop.bad_format
[       OK ] stop.bad_format (0 ms)
[----------] 2 tests from stop (1 ms total)

[----------] 1 test from isr
[ RUN      ] isr.happy_path
[       OK ] isr.happy_path (0 ms)
[----------] 1 test from isr (0 ms total)

[----------] 2 tests from stats
[ RUN      ] stats.happy_path
[       OK ] stats.happy_path (0 ms)
[ RUN      ] stats.bad_format
[       OK ] stats.bad_format (0 ms)
[----------] 2 tests from stats (0 ms total)

[----------] 2 tests from go
[ RUN      ] go.happy_path
[       OK ] go.happy_path (0 ms)
[ RUN      ] go.bad_format
[       OK ] go.bad_format (0 ms)
[----------] 2 tests from go (0 ms total)

[----------] 1 test from parser
[ RUN      ] parser.bad_command
[       OK ] parser.bad_command (0 ms)
[----------] 1 test from parser (0 ms total)

[----------] Global test environment tear-down
[==========] 9 tests from 6 test cases ran. (1 ms total)
[  PASSED  ] 9 tests.

Failing

I modified an expected result so it would fail. The bottom line says "1 FAILED TEST" and the Makefile warns about a bad return code "make: *** [run] Error 1"

$ make
mkdir -p build
g++ -I mock_arduino -fno-exceptions -pthread -c ut.cpp -o build/ut.o
g++ -pthread build/ut.o build/mock_arduino.o -lgtest -lgtest_main -o build/ut
build/ut
Running main() from gtest_main.cc
[==========] Running 9 tests from 6 test cases.
[----------] Global test environment set-up.
[----------] 1 test from pulser_info
[ RUN      ] pulser_info.init
ut.cpp:22: Failure
Value of: pulser_info.pin_num
  Actual: 10
Expected: 120
[  FAILED  ] pulser_info.init (0 ms)
[----------] 1 test from pulser_info (0 ms total)

[----------] 2 tests from stop
[ RUN      ] stop.happy_path
[       OK ] stop.happy_path (0 ms)
[ RUN      ] stop.bad_format
[       OK ] stop.bad_format (0 ms)
[----------] 2 tests from stop (0 ms total)

[----------] 1 test from isr
[ RUN      ] isr.happy_path
[       OK ] isr.happy_path (0 ms)
[----------] 1 test from isr (0 ms total)

[----------] 2 tests from stats
[ RUN      ] stats.happy_path
[       OK ] stats.happy_path (0 ms)
[ RUN      ] stats.bad_format
[       OK ] stats.bad_format (0 ms)
[----------] 2 tests from stats (0 ms total)

[----------] 2 tests from go
[ RUN      ] go.happy_path
[       OK ] go.happy_path (0 ms)
[ RUN      ] go.bad_format
[       OK ] go.bad_format (0 ms)
[----------] 2 tests from go (0 ms total)

[----------] 1 test from parser
[ RUN      ] parser.bad_command
[       OK ] parser.bad_command (0 ms)
[----------] 1 test from parser (0 ms total)

[----------] Global test environment tear-down
[==========] 9 tests from 6 test cases ran. (2 ms total)
[  PASSED  ] 8 tests.
[  FAILED  ] 1 test, listed below:
[  FAILED  ] pulser_info.init

 1 FAILED TEST
Makefile:24: recipe for target 'run' failed
make: *** [run] Error 1

In this example, I changed this line:

EXPECT_EQ(10, pulser_info.pin_num);

to

EXPECT_EQ(120, pulser_info.pin_num);

The resulting output shows that the Expected value should have been "120" but actually it was 10.

The failure occurred in ut.cpp in line 22 which is the line above.

[ RUN      ] pulser_info.init
ut.cpp:22: Failure
Value of: pulser_info.pin_num
  Actual: 10
Expected: 120

A Typical Test Case (TC)

To "run" the Arduino sketch, I needed to simulate or "mock" the Arduino library calls. For example, the Serial.print() calls. These are specified in Arduino.h and mock_arduino.h.

In this sketch's case, I also needed to mock out some interrupt and gpio related calls, see avr/interrupt.h and wiring_private.h for them.

Note that I included the ArduinoPulser.ino" file. This gives the the TCs access to all privately scoped variables in the file (e.g. "static int x" is visible to the TC)

#include "gtest/gtest.h"

#include "mock_arduino.h"
#include "../ArduinoPulser.ino"

This tests the Stop! behavior for a "happy path", i.e. the normal, expected behavior when all goes well. The happy path usually tests the majority of execution time. Subsequent TCs should test for odd-ball, edge-condition, failures, recoveries, etc. not-happy-path. Generally speaking, most code is used for these conditions, but most execution time is spent in the happy path. Sort of like real life :)

I called mock_init() to reset the state of all of the mocked variables. For example it keeps track of the number of times interrupts were cleared and restored, so that test cases can check if they were called the correct number of times. These need to be reset in each test case so previous test cases don't interfere with the current results.

setup() calls the sketch's setup. Again this ensures that the current state of the simulated system is reset to a known initial state.

// --------------------
TEST(stop, happy_path)
  {
  mock_init();
  setup();

This next section simulates some pulses and some ticks. This is just to get non-zero values in place. To simplify the TC, I didn't call the ISR to simulate running, see TEST(isr, happy_path) for that.

  // sim running for a while
  pulser_info.curr_num_pulses = 100;
  pulser_info.curr_num_ticks = 150;


This section simulates receiving a Stop! command: "s" followed by a carriage return.

It then checks that the correct serial output is generated. In this case, a simple "ACK" followed by a linefeed. See Arduino.h and mock_arduino.cpp for the various mocked Serial calls:

  • int MockSerial::available()
  • void MockSerial::print(const char* s)
  • void MockSerial::print(unsigned int v, int format)
  • etc.
  EXPECT_EQ(PARSER_START, parser_info.state);
  parser_handle_ch('s');
  EXPECT_EQ(PARSER_STOP, parser_info.state);
  parser_handle_ch(0x0D);

  EXPECT_STREQ("ACK\x0A", mock_values.serial_out);

This checks if the pulses and ticks are saved in the "last_xx" variables and that the current variables "curr_xx" are set to 0.

  // confirm stop was done
  EXPECT_EQ(100, pulser_info.last_num_pulses);
  EXPECT_EQ(150, pulser_info.last_num_ticks);
  EXPECT_EQ(0, pulser_info.curr_num_pulses);
  EXPECT_EQ(0, pulser_info.curr_num_ticks);

This checks the pulse array slots are all 0.

  for (int i = 0; i < PULSER_ARRAY_WIDTH; ++i)
    {
    EXPECT_EQ(0, pulser_info.pulses[i]);
    }

And finally it checks the set and clear interrupt were called the right number of times.

  // the set/clear interrupts were called twice
  // once in the pulser init and once by stop
  EXPECT_EQ(2, mock_values.sei_called_count);
  EXPECT_EQ(2, mock_values.cli_called_count);
  }

Arduino Mocks

mock_arduino.h

Some of the code in mock_arduino.h came from existing code in Arduino.h and other AVR code.

#define __COMPILING_AVR_LIBC__
#define F_CPU 16000000L
#define ARDUINO 10005
#define ARDUINO_
#define ARDUINO_ARCH_AVR

#ifdef __AVR_ATmega2560__
  #undef __AVR_ATmega2560__
  #define __AVR_ATmega2560__
#endif

#ifdef __THROW
  #undef __THROW
  #define __THROW
#endif

#include "Arduino.h"

I put most of the mock values in a struct to group them together. This is very "C"-like, a more C++ way would be to put them in a class.

struct _mock_values

 {
   int  cbi_called_count;
   int  sbi_called_count;
   int  sei_called_count;
   int  cli_called_count;
   char serial_out[2048];
 };

typedef struct _mock_values mock_values_t; extern mock_values_t mock_values;

void mock_init(); void mock_clear_serial_out();

This section sets up some #defines to replicate what is done in the existing Arduino and AVR libraries. These are most related to some AVR registers used for setting up the Timer2.

#define _BV(v) v

#define CS20 0b001
#define CS21 0b010
#define CS22 0b100
extern int TCCR2B;

extern int OCR2A;

#define WGM21 0b001
extern int TCCR2A;

#define OCIE2A 0b001;
extern int TIMSK2;

Arduino.h

Arduino.h mocks the Serial port. These are only the calls I used in the ut.cpp, there are probably many other functions in Arduino's Serial class that needs to be mocked out. These are sufficient for this project, for now.

#define DEC 10

class MockSerial
  {
  public:
    MockSerial();
    void begin(int num);
    char read();
    void print(const char* s);
    void print(const char c);
    void print(unsigned int, int);
    void println();
    void println(const char* s);
    void println(int, int);
    int available();
  };

extern MockSerial Serial;

These are a couple of support functions used in the pin setup and in the ISR

#define OUTPUT 0
void pinMode(int pin, int mode);

void delayMicroseconds(int delay);

wiring_private.h

The wiring_private.h just contains references to setting and clearing a bit in a port register

extern void sbi(int port, int bit);
extern void cbi(int port, int bit);

avr/interrupt.h

The avr/interrupt.h holds routines for interrupts and various support definitions.

The #define for the ISR translates the the call in ArduinoPulser.ino to a unique function name.

#define ISR(name) void isr_##name(void)

For example this translates `ISR(TIMER2_COMPA_vect)` to `void isr_TIMER2_COMPA_vect()` so it can be called in a TC like this

## in ut.cpp around line 6 or so ##
// --------------------
// simulate running the ISR one tick
void run_isr_one_tick()
  {
  for (int i = 0; i < PULSER_PRESCALER; ++i)
    {
    isr_TIMER2_COMPA_vect();
    }
  }

Back in avr/interrupt.h, there are a few mocks related to the Timer2 register values and setting/clearing interrupts.

#define PORTB 0
#define PORTB4 4

extern void sei(void);
extern void cli(void);

mock_arduino.cpp

This file provides most of the code for simulating various functions and for providing symbols for the linker to be happy.

These provide variables to for the ArduinoPulser code to work on. In this project, I don't care that much if they are set correctly, but since they are not static, a TC could check them.

int TCCR2B = 0;
int OCR2A  = 0;
int TCCR2A = 0;
int TIMSK2 = 0;

mock_init() is used to reset the overall state to a known configuration.

mock_values_t mock_values;

void mock_init()
  {
  mock_values.cbi_called_count = 0;
  mock_values.sbi_called_count = 0;
  mock_values.cli_called_count = 0;
  mock_values.sei_called_count = 0;
  mock_clear_serial_out();
  }

The next section mocks out the Serial I/O. In this case I do care about the content, so these functions simulate actual input and output functions.

The serial output is just a buffer in memory that the TC can check to see if the output is what it should be.

The mock_clear_serial_out() function resets the buffer to "" (empty string).

void mock_clear_serial_out()
  {
  memset(mock_values.serial_out, 0x00, sizeof(mock_values.serial_out));
  }

This line declares a variable as Serial so all of the .ino calls will reference this one variable.

MockSerial Serial;

Typical Serial calls. A print(s) simply takes the input string and concatenates it to the end of the buffer. The print(int, int) is a little more complex in that it uses sprintf() to fill a local buffer with the correct format and then just tacks that onto the end of the serial buffer. println() variants just tack on an 0x0A linefeed character to the end of the buffer.

Note there is no buffer length checking in any of these! It is possible that a TC goes amok and goes past the end of the serial buffer (currently 2048 bytes, see mock_arduino.h)

<... snip ...>
void MockSerial::print(const char* s)
  {
  strcat(mock_values.serial_out, s);
  }
<... snip ...>
void MockSerial::print(unsigned int v, int format)
  {
  char buf[30];
  if (format == DEC)
    {
    sprintf(buf, "%d", v);
    }
  else
    {
    sprintf(buf, "%X", v);
    }
  strcat(mock_values.serial_out, buf);
  }

void MockSerial::println()
  {
  strcat(mock_values.serial_out, "\x0A");
  }

We're optimists. There is always a character available to read. In this project, this is sufficient since I can call parser_handle_ch(ch) in a TC to input whatever character I need.

In other projects, there may have to be a Serial input buffer. In that case, available() could check a pointer or index into that buffer and check return true/false if there's anything in there.

int MockSerial::available()
  {
  return 1;
  }

These two functions just simulate setting/clearing a pin. In the TCs, I only care if the the pin is set and then cleared the same number of times, so the current mock behavior is sufficient.

Other projects may want to also keep track of the current state of the pin and give mock_xx functions to check the pin is in the correct state.

void cbi(int port, int bit)
  {
  mock_values.cbi_called_count++;
  }

void sbi(int port, int bit)
  {
  mock_values.sbi_called_count++;
  }

These two functions enable/disable interrupts. In the TCs, I only care if the the interrupts are enabled/disabled the correct number of times, other projects may want to add more functionality to check if the interrupts are in the correct state.

void sei(void)
  {
  mock_values.sei_called_count++;
  }

void cli(void)
  {
  mock_values.cli_called_count++;
  }

This is another typical example of a function that doesn't require a simulation. If you look in mock_arduino.cpp most of the functions are like this. These are required only to keep the linker happy.

In a TC, it is not usually required to run in "real time" so an actual delay would never occur. However in other projects, it may be possible that the requested delay could be checked or that the total delay over many calls is important. In those cases, there would be additional functionality and additional functions to keep track of those things.

void delayMicroseconds(int delay)
  {

  }

Notes

A simpler parsing scheme is to:

  • get a character from the serial port, if any
  • if it's not a <cr>, save it into a buffer otherwise parse the buffer in it's entirety

In this way, it is possible to throw out the entire buffer on an invalid command etc.

On the other hand this technique has problems too:

  • how big should the buffer be? Have to add checks for buffer overflow.
  • When do you tell the caller there was an buffer overflow? What if there is no incoming <cr>? Simply sending out a string, while a stream of incoming characters is coming in may not work since the caller may not be listening at that time.
  • it's not as nice when using putty since the characters are "ignored" up until enter is pressed. It works better from ruby though.
Personal tools