Bug Lift


Figure 1 : bug lift

Continuing the bug theme (drum roll) i present the bug lift. This is a teaching demo based around a winch, lifting and lowering a bug (mass). The bug is just for fun, the main aim of this hardware is to produce some real world signals: input signals from sensors, output signals driving actuators, to put the electronic circuits we look at in lectures into some context. The challenge is to implement a PID controller, controlling the speed of a small DC motor, such that (in theory) the speed of the bug whilst being raised, or lowered remains "constant" i.e. it takes more torque to raise the bug and less to lower, therefore, need to vary the current through the motor to ensure a smooth ride (for motors output torque is proportional to the current flowing in its winding). To be honest with the cheap and cheerful hardware used, a truly "constant" speed is a bit of a big ask. Also, one of the aims of this demo is to show noisy signals, the types of problems faced in these types of embedded systems, so having proper, accurate, high resolution sensors kind of defeats the point, but it would make life a lot simpler i.e. control systems are lot easier to implement when you have good sensor data i.e. GIGO - garbage in, garbage out. The other aim is to show different types of output drivers e.g. PWM, open collector drivers, relays etc, which are used to control the different actuators in this system. A video of the system in action is available here: LINK. At the time of filming only a prototype, so control loop needed to be improved a little, movement is a lot smoother in the final version, honest :).

Table of Contents

Motor Control
Infra-red Sensor
Analogue to Digital Conversion
Micro-switch
PCB and Hardware
PID control
Calculating Speed
Version 1 : hardware sensor interface
Version 2 : software sensor interface


Figure 2 : mass = load = bug

Motor Control

Only one actuator, the winch motor, but we do need to control its speed and direction, which does complicates things a little. To reverse the direction of the motor you need to flip the voltage across the motor i.e. swap the voltage on the terminals. This can be done in a number of different ways e.g. L298 H-bridge, as used in Roach v2 LINK, however, as this was a scrap box demo (made from left over parts) decided to go for a pair of SPDT relays as shown in figure 3.


Figure 3 : relay circuit

The two relays (LINK), K1 and K2 each have a single pole, double throw (SPDT) set of contacts. Each common pole is connected to one of the motor's terminals, such that energising the coils on each relay will reverse the voltage across the motor i.e. in figure 3: VA=4V, VB=0V, energising the relays by turning on transistor Q1 will move the switch contacts into their other position, therefore, reversing the voltage across the motor (assuming transistor Q2 is turn on). The DIR input controlling Q1 is connected to digital GPIO pin on he micro-controller, logic 0 = relays turned off, logic 1 = relays turned on. The base resistor is 330 ohms, a little low for the relays, but was needed for Q2 as the current through the motor peaks at about 500mA and the current gain of the TIP41 transistors can be quite low LINK.

To vary the speed of a D.C. motor you need to vary the voltage across the motor's winding i.e. a low voltage = low speed, high voltage = high speed, for this particular motor 1V is a slow speed and 4V is its maximum speed. Increasing the voltage above 4V will damage the contacts / winding within the motor i.e. if you exceed a components maximum current it gets hot and melts. You can control the voltage across the motor using a linear regulator, as used in the labs, shown in figure 4. Here a reference voltage is produced using signal diodes, 3 x 0.7 = 2.1V. An operational amplifier then controls the voltage across the motor by comparing the voltage across the motor's winding to the reference voltage. The transistor is configured as an emitter follower, acting as a current amplifier.


Figure 4 : linear regulator speed controller

The disadvantage of this approach is that the micro-controller (a digital component) would need to produce an analogue output i.e. the reference voltage. This can be done using a Digital to Analogue Converter (DAC), but would require more hardware. Also the drive transistor does waste power, so its not the best solution if battery powered e.g. if powered from a 12V battery with the motor running at 4V drawing 300mA (max), the power dissipated in the drive transistor would be approximately P = V*I = (12-4) * 0.3 = 2.4W, so it would get a little warm, need a heat-sink. An alternative way to vary the voltage across the motor is to use pulse width modulation (PWM). Repeated turning the motor's drive voltage on/off "very" quickly i.e. connecting the motor to +4V, then 0V, reduces the average current flowing through the coils, therefore, reducing the average voltage across the motor. Figure 5 shows a SPICE simulation of a varying duty cycle voltage (ratio of on-time to off-time) across a simple D.C. motor model, R=0.5, L=2mH.








Figure 5 : Pulse Width Modulation (PWM), 15%, 50%, 85% and ramp

As these simulations show by varying the pulse width we can vary the voltage across the motor and therefore its speed: 15% = 2V, 50% = 5V, 85% = 8V. PWM uses a fixed frequency and varies the pulse width. In theory this frequency should not matter, but in reality it does. The motor is effectively a coil which will have a associated resistance and inductance i.e. it will act like a low pass filter (LPF). If the frequency is too high there will not be time for the applied voltage to produce a current in the coil, therefore, no torque will be produced. Normally i would select the frequency to give the best range of speeds, fast enough so that the motor is not "pulsing", but not too fast so that its "hummmmmming". To generate this PWM signal we can use the built in function analogueWrite(). This is generated by the micro-controllers on-chip timers, so nice and easy a fire and forget, implemented directly in hardware. The default frequency for the timer used to control pin 11 is 490Hz, which seems to give a reasonable range of control i.e. different speeds. The test code below slowly accelerates, then de-accelerates the motor, then reverses the motor terminals and repeats.

// OUTPUTS
// -------

// Yellow : relay : D12
// Green : motor : D11

#define RELAY  12
#define MOTOR_PWM 11

// CONSTANTS
// ---------

// VARIABLES
// ---------

unsigned int count = 0;

// FUNCTIONS
// ---------

// INITIALISATION
// --------------

void setup()
{
  pinMode(RELAY, OUTPUT);
  digitalWrite(RELAY, LOW); 
}

// MAIN LOOP
// ---------

void loop()
{
  while (true)
  {
    digitalWrite(RELAY, HIGH); 
    for( int i=0; i < 255; i=i+5)
    {
      analogWrite(MOTOR_PWM, i);
      delay( 500 );  
    }
    for( int i=255; i > 0; i=i-5)
    {
      analogWrite(MOTOR_PWM, i);
      delay( 500 );  
    }
    digitalWrite(RELAY, LOW); 
    for( int i=0; i < 255; i=i+5)
    {
      analogWrite(MOTOR_PWM, i);
      delay( 500 );  
    }
    for( int i=255; i > 0; i=i-5)
    {
      analogWrite(MOTOR_PWM, i);
      delay( 500 );  
    }
  }
}

Infra-red Sensor

To control the speed of the winch you need to sense the speed of the motor. Normally you would use a nice optical rotary encoder LINK such as the one shown in figure 6, datasheet available here: LINK. This is a glass of plastic disk with fine "slits" that chop an infra-red beam (block/pass), generating a pulse each time the bean is blocked e.g. 200 - 1000 pulses per revolution. These sensors can be a little on the expensive side, but do give you a very clean (electrical noise), high resolution (movement) sensor signal.


Figure 6 : encoder disk

To keep this demo cheap and to add an extra layer of "fun" went for a DIY solution. The model motor kit used in the winch allows you to add or remove gears to achieve the desired speed/torque requirements. Therefore, it is very easy to add a reflective sensor next to the motor, as shown in figures 7 and 8, datasheet available here: LINK. This sensor shines a infra-red beam onto the first cog. When rotating more light is reflected from the flat surface, less when there is a hole. Therefore, the sensor will produced four "pulses" per cog revolution. However, as the motor's drive cog has 12 teeth and the cog used as the reflective surface has 48 teeth, this only gives 1 pulse per motor drive shaft revolution, which is not a lot of information to work with when you are trying to control the speed of the motor.



Figure 7 : drive motor


Figure 8 : infra-red reflective sensor

Electrically the sensor's circuitry is just a couple of resistors, as shown in figure 9. The LED resistor can be calculated from the datasheet. The forward voltage of the diode is 1.7V, maximum current is 12mA and the supply voltage is 5V. Therefore, the minimum value of R is (5-2.5)/12x10^-3 = 208 ohms. Its never a good idea to run components at their theoretical max i.e. to ensure a long life, therefore, went for 330 ohms (increase R, reduce I). The transistor's collector resistor is a little more tricky to calculate as it depends on the amount of reflected light i.e. the current flowing through the photo-transistor. From the datasheet we know that the max current must be limited to 25mA, assuming a VCE of 0.7V when fully turned on this gives a minimum value of R of (5-0.7)/25x10^-3 = 172 ohms. However, in normal usage this will not occur. The reflected light will cause a very small current to flow through photo-transistor, less that a miili-amp. These small changes in current can be converted into changes in voltage by considering the transistor and collector resistor as a potential divider:

Using the values of R shown the output signal shown in figure 10 was produced. Note, 2V was connected across the motor's winding, so quite a slow speed.


Figure 9 : infra-red reflective sensor circuit


Figure 10 : sensor output signal

The output signal shows a good consistent signal, peaks correspond to holes in the cog and troughs to flat surfaces. However, the minimum voltage is outside the logic threshold for a logic zero, as shown in figure 11. Therefore, if this signal is connected to a digital input no pulses would be detected i.e. it would look like a constant logic 1. For the 74LS04 NOT gate a logic 0 is defined as a voltage of less than 0.8V. Normally to get round this problem i would use an analogue comparator as shown in figure 8 on the Roach v3 robot LINK. However, for this case study i wanted to try and use the ADC to sample the sensor signal i.e. a software based solution, therefore, want to keep the hardware as simple as possible e.g. a NOT gate, to square up the signal ready for the Arduino micro-controller. To confirm the logic thresholds of this NOT gate a 2.5Vp triangular wave with a +2.5V D.C. offset i.e. a +5v to 0V triangular wave, was connected to an input of a NOT gate, the resultant output plot is shown in figure 12.


Figure 11 : 74LS04 logic threshold voltages


Figure 12 : 74LS04 logic threshold test

This shows that the logic thresholds for this gate are 0.88V and 1.12V i.e. coming from a 0V when 0.88V is reached it is considered a logic 1, coming from 5V when 1.12V is reached it is considered a logic 0. As this plot shows the output is not "square", a little rounded, this is because this NOT gate was _not_ designed to handle analogue signals such as this triangular waveform, they were designed to process square waves. In general if you are connecting analogue signals to digital inputs you would normally go through a Schmitt trigger LINK e.g. a 74HC14, datasheet available here: LINK. These gates implement the same NOT function, but have been designed to interface to this type of analogue signal and reduce noise problems associated with signals crossing logic thresholds. Running the same test using a 74HC14, the output waveform in figure 13 shows the increased separation between logic voltage thresholds (now 0.9V rather than 0.2V), reducing the chance of random noise accidentally moving the signal voltage between logical states e.g. in the case of a 7404 if the signal voltage is 0.9V, small voltage fluctuations caused by noise +/- 200mV will cause the output of the NOT gate to rapidly switch between logical states, resulting in a false input signal.


Figure 13 : 74HC14 logic threshold test

From the previous calculations to achieve the logic 0 voltage threshold required by the 7404 a current of 0.9mA will need to flow through the photo transistor. This could be achieve by increasing the brightness of the LED, the amount of light reflected back from the cog, or the value of the photo-transistor's collector resistor. In general increasing the collector resistor is probably the easiest solution, but this could increase signal noise e.g. small changes in current cause by motor interference. In the previous calculation the current flowing in the photo-transistor when in front of a flat section was 0.5mA, therefore, to produce an output signal of 0.8V the collector resistor will need to be (5-0.8)/0.5^10-3 = 8400 ohms, rounded up to 10K. However, as i had already soldered in the resistors decided to add reflective strips to increase the amount of reflected light, as shown in figure 14.




Figure 14 : reflective pads

The interface circuit is shown in figure 15, the reflective sensors plug onto connectors X1 or X2. The photo-transistor collector signal is passed through a first order low pass filter to help remove any sharp transients, before going to a 7414 NOT gate. The raw input signal and output of the LPF is shown in figure 16, the output of the NOT gate in figure 17.


Figure 15 : interface circuit


Figure 16 : sensor input signal, top - raw, bottom - filtered


Figure 17 : sensor output signal, top - raw, bottom - NOT gate

Figure 17 shows that not all the pulses were detected by the NOT gate. These errors were reduced by moving the cog with the reflective strips closer to the sensor (removed a spacer washer). The output is now reasonably stable, as shown in figure 18.


Figure 18 : sensor output signal, top - raw, bottom - NOT gate

Analogue to Digital Conversion

The main aim of this case study is to look at real world signals, these can be digital, but for embedded systems such as this are more likely to be analogue. To convert these signals into the digital domain we can use circuits as shown in figure 15, alternatively we can use analogue to digital converters (ADC). The micro-controller used in this project has a 12bit successive approximation ADC, a discussion of this type of converter can be found here: LINK. When selecting an ADC the two important things you need to identify are its resolution and conversion speed. The resolution of this ADC is 12 bits, e.g. a conversion value of 1023 = 5V, a value of 1 = 5V / 1024 = 4.9mV. Within any system there will be sources of internal and external electrical interference e.g. digital logic switching, motor contacts sparking etc, this will cause small voltage fluctuations on the power supply rails and within circuits. Therefore, always be conservative on the ADC's true resolution i.e. how many of the lower bits are swamped by these noise sources and how many truly represent the signal you are trying to measure. For this system i suspect you are probably looking at a 8 bit resolution i.e. 20mV+. For this ADC its resolution is fixed by the hardware, its conversion time is dependant on the clock speed, you also have to consider software overheads incurred in using this hardware. Reading around, sources suggested the conversion time was 100us or 10,000 samples per second (10KHz). To test this out i wrote the following code that prints the ADC value to the screen and toggles a GPIO pin when performing the conversion.

// INPUTS
// ------

#define IR_SENSOR A0

// OUTPUTS
// -------

#define TEST_PIN 13
  
// VARIABLES
// ---------

int sensorData = 0;

// FUNCTIONS
// ---------

// INITIALISATION
// --------------

void setup()
{
  Serial.begin(115200);  
  while (!Serial) {}; 
  Serial.println("ADC test"); 
  pinMode(TEST_PIN, OUTPUT);
  digitalWrite(TEST_PIN, LOW); 
}

// MAIN LOOP
// ---------

void loop()
{
  digitalWrite(TEST_PIN, HIGH);
  sensorData = ( analogRead(IR_SENSOR) );
  digitalWrite(TEST_PIN, LOW);
  
  digitalWrite(TEST_PIN, HIGH);
  Serial.println( sensorData ); 
  digitalWrite(TEST_PIN, LOW);

  delay(1);   
}

Using a scope to capture the activity on the test pin you can see it takes:






Figure 19 : software timing

Therefore, conversion is approximately 105us, so in the ball park. As the ADC is software controlled this is obviously the best-case conversion rate, as more software is added to the program the time between conversions time will increase. The next important consideration when using an ADC is the relationship between conversion time and the frequency components within the signal being captured i.e. aliasing LINK. You need to sample the signal at twice the maximum frequency component e.g. a 10KHz sine wave needs to be sampled (converted) at 20KHz i.e. the Nyquist–Shannon sampling theorem LINK. However, in reality to capture the amplitude characteristics of the signal you are looking to sample much more quickly e.g. 10 times the maximum frequency component. Fortunately, the signal from the infra-red sensor is relatively slow (30Hz - 100Hz), to evaluate the required ADC conversion rate for this signal the following code was used:

// INPUTS
// ------

// Grey : IR sensor (raw) : A0

#define IR_SENSOR A0

// OUTPUTS
// -------

#define TEST_PIN 13
#define MOTOR_PWM 11

// CONSTANTS
// ---------

#define MAX_READINGS 900
#define MAX_DELAY 500
  
// VARIABLES
// ---------

int volatile sensorDataState = 0;
int volatile sensorData[MAX_READINGS];
int volatile sensorDataCount = 0;

// FUNCTIONS
// ---------


// INITIALISATION
// --------------

void setup()
{
  Serial.begin(115200);  
  while (!Serial) {}; 
  Serial.println("ADC test"); 
  pinMode(TEST_PIN, OUTPUT);
  digitalWrite(TEST_PIN, LOW); 
}

// MAIN LOOP
// ---------

void loop()
{
  analogWrite(MOTOR_PWM, 90);
  delay(3000);
  
  while (true){
    if ( sensorDataState == 0 ){
      Serial.println( "Start" );
      sensorDataState = 1;
    }  
    else if ( sensorDataState == 1 ){

      digitalWrite(TEST_PIN, HIGH);
      sensorData[sensorDataCount] = ( analogRead(IR_SENSOR) );
      digitalWrite(TEST_PIN, LOW);
      
      delayMicroseconds( MAX_DELAY );
      sensorDataCount ++;
      
      if (sensorDataCount == MAX_READINGS){
        sensorDataState = 2; 
      }          
    }
    else if ( sensorDataState == 2 ){
      int i;
      for (i=0; i MAX_READINGS; i++){
        Serial.print( sensorData[i] ); 
        Serial.print(',');  
      }  
      sensorDataState = 3;
      analogWrite(MOTOR_PWM, 0);
      delay(1000);
    }
  }       
}

Reading around there seemed to be differing views of the amount of RAM available for variables, i should really read the datasheet, but playing around an array greater than 1000 caused the micro-controller to crash, an array of 900 worked, so went with 900 and didn't look too closely :). As this array uses the int data type, that would be an array of 1800 bytes. The constant MAX_DELAY was varied from 500us to 8ms, serial data was cut and pasted from the terminal, then loaded into a spreadsheet to produce the graphs shown in figure 20. Spreadsheet is available here: LINK.










Figure 20 : ADC data - 500us, 1ms, 2ms, 4ms and 8ms sample rate

The input sensor signal is approx 70Hz (14ms), therefore, to ensure the frequency components in the signal are captured need to sample at 140Hz, or every 7ms. This can be clearly seen from figure 20, when you sample every 8ms all information is lost. Note, this is best case i.e. a slow motor speed, as the winch speed is increased more pulses per second will be produced, therefore, the sampling rate will need to be increased. A scope plot of the sensor's output at max speed is shown in figure 21, from this we can estimate that the maximum sampling rate needed will be 200Hz, or every 5ms. This should be ok, the Arduino is clocked at 16MHz, so that gives around 80,000 cycles to capture the sensor signal, process the data and update the output controlling the motor. Note, this is the maximum practical motor speed, the absolute maximum speed gives a sensor frequency of 160Hz, but at this speed the motor makes a lot of noise / vibrations.


Figure 21 : ADC data - max speed

Micro-switch

The final sensor is a micro-switch, used to count winch drum rotations, as shown in figure 21. This switch has a roller lever arm that follows a cam, machined by my own fair hands :), which is attached to the main drive shaft. The switch produces 4 pulses per revolution. The micro-switch is plugged onto connector X3 in figure 15, using a simple 4K7 ohm pull-up resistor i.e. when the switch is closed the output signal goes to a logic 0, otherwise the signal is pulled up to a logic 1. Again, a LPF helps remove any noise produced by contact bounce LINK, before being feed into a 7414 NOT gate. The output signal (voltage across the micro-switch) without this filter can be seen in figure 22, showing the contacts "bouncing" as they are close (rapidly opening and closing). In general this noise only occurs when the switch is closed, when the switch is opened you will not see this affect i.e. when the contacts are open they are open.


Figure 22 : position sensor - microswitch




Figure 23 : microswitch contact bounce, bottom zoomed in view

The final output of the 7414 NOT gate is shown in figure 23, showing that all high frequency noise generated by the switch has been removed. Note, a side affect of all filters is that they delay the signal, in this case you can see from the plot that the output signal from the NOT gate changes approximately 1ms after the switch is closed.




Figure 24 : microswitch, top - contacts closing, bottom - contacts opening

PCB and Hardware

Knocked out a quick PCB (figure 25) for the circuit shown in figure 15. Four simple NOT gate input filters for infr-red and switches, and four open collector drivers to switch relays and control the speed of the motor, this gives some spare inputs and outputs for later modifications etc. Then using the joys of the laser cutter and the 3D printer produced the system shown in figure 26. Powered from 12V (battery), uses an off the shelf switch mode voltage regulator (buck converter) to produce the +4V for the motor. Relays are panel mounted types so bolted onto chassis.






Figure 25 : interface PCB








Figure 26 : system

PID control

Now the fun bit, what could go wrong :). The software on the Arduino implements a simple PID controller LINK, as shown in figure 27. The user sets/requests the desired speed r(t), the actual speed y(t) is subtracted from this to produce the error signal e(t). This value is then used to generate the P, I and D terms to produce the control signal u(t).

The P, I and D terms are then added together to produce the new control signal u(t), after a small delay the system will produce a new value of y(t) and the process repeats. In theory a PID controller is simple to code and tune (calculate the value of Kp, Ki and Kd), but ive always found them an absolute pain, but we shall see ....


Figure 27 : PID controller

Calculating Speed

First step in implementing this controller is to determine the motor's speed y(t). For the first version going to keep it simple and use the hardware filtered infra-red speed sensor signal. There are two methods to determine the speed of the motor:

Initially i went for counting the number of pulses over a timed interval method as i thought it would give a more stable result. To simplify the implementation used two interrupts, one counting the number of pulses, the other triggered when the desired time period had elapsed. The Arduino has two external interrupt pins: 2,3. To configure pin 3 to generate an interrupt on a logic '1' -> logic '0' transition i.e. a falling edge:

#define INT1 1    
#define INT1_PIN 3

unsigned long volatile irSensorCount = 0;

pinMode(INT1_PIN, INPUT);
digitalWrite(INT1_PIN, HIGH);  
attachInterrupt(INT1, liftMovement, FALLING);

void liftMovement()
{
  irSensorCount = irSensorCount + 1;
}

Therefore, every time a reflective strip passes the sensor the value of irSensorCount is increased by one, a nice simple, clean solution. To measure the counting time period the micro-controller has three timers:

Pin 11 is used to generate the PWM signal controlling the motor. The general delay functions use timer 0, therefore, leaves timer 1 to generate the periodic interrupted needed. The sample time interval needed is dependant on the frequency of the sensor data, connecting the winch motor to a variable power supply the relationship between pulses/second and voltage can be seen in figure 28.


Figure 28 : voltage vs frequency

At minimum speed this signal will be changing at 25Hz, i.e .a pulse every 40ms. At maximum speed this signal will be changing at 150Hz i.e. a pulse every 6.7ms. To give an accurate indication of the motor's speed you need to accumulate this count value over a "long" period of time e.g. couple of seconds, giving a count value of between 50 - 300, an active range of approximately 250. This however is not possible as the controller needs to respond quickly to changes in motor speed. Initially a control loop speed of 10Hz (100ms) was chosen i.e. the motor's speed is updated 10 times a second. Therefore, this gives a count value of between 2 - 15, an active range of approximately 13. This is a very coarse, low resolution measurement of speed, using this approach the controller will be unable to detect small changes in motor speed. When building a system its very easy just to jump in and hack, always stop and check that the solution you are implementing can in theory work.

RULE NUMBER 1 OF BUILDING HARDWARE : GUESSES & ASSUMPTIONS ARE YOUR WORST ENEMY ....

Therefore, change of plan, a move to measuring the infra-red sensor's pulse width. This should allow the motor's speed to be measured worst case every 40ms i.e. the slowest sensor frequency, or best case every 6.7ms. At the slowest speed this allows at least two sensor readings to be processed and then averaged. The resolution of these readings it determined by the clock speed of the micro-controller i.e. how quickly it can test the input pin. To evaluate this method i borrowed a bit of code from the Arduino Cookbook (slightly modified):

const int irqPin = 3;
const int irqNum = 1;
const int numberOfEntries = 8;

volatile unsigned long microSeconds;
volatile byte index = 0;
volatile unsigned long results[ numberOfEntries ];
volatile boolean start = false;

void setup()
{
  pinMode( irqPin, INPUT );
  Serial.begin( 9600 );
  attachInterrupt( irqNum, dataLogger, CHANGE );
}

void loop()
{
  if( index >= numberOfEntries )
  {
    Serial.println( "Data" );
    for( byte i=0; i< numberOfEntries; i++ )
    {
      Serial.println( results[i] );
    }
    while(1);
  }
  delay( 1000 );
}

void dataLogger()
{
  if( start )
  {
    if( index < numberOfEntries )
    {
      results[index] = micros() - microSeconds;
      index = index + 1;
    }
  }
  else
  {
    if( digitalRead( irqPin ) == 1 )
    {
      start = true;
    }
  }
  microSeconds = micros();  
}

This code measures the width of the high and low sections (in microseconds) of the infra-red sensor and then prints these to the serial terminal. A plot of this data for slow, medium and fast motor speeds is shown in figure 29. Spreadsheet is available here: LINK.


Figure 29 : infra-red sensor pulse widths

From these plots you can see the variable, but periodic nature of these pulses, undoubtedly these differences are due to variations in the widths of the reflective strips. However, these readings do look reasonably stable, taking an eyeball average:

To smooth out the variations the sum of the two values i.e. the period will be used. This should remove the differences due to variations in the reflective strips i.e. a thin strip will have a longer logic '1' and a shorter logic '0' time, a thick strip will have a longer logic '0' and a shorter logic '1' time, sum of the two should be the same. Then to remove variations due to "noise" a running average will be taken over the last 8 readings. This may introduce some delay e.g. if you had a step change in the speed value it would take 8 x 32ms = 256ms, to get a new set of speed readings. However, as we are measuring the pulse width during the 100ms control loop delay, three updated readings out of the eight will have been recorded. At faster speeds this is not a problem, as a complete set of readings will have been recorded i.e 8 x 11.5ms = 92ms. Code to test out these speed calculations is shown below, also added code to give median for a comparison:

const int irqPin = 3;
const int irqNum = 1;
const int numberOfEntries = 8;

volatile unsigned long microSeconds;
volatile byte index = 0;
volatile unsigned long results[ numberOfEntries ];
volatile unsigned long pulse[ numberOfEntries ];
volatile boolean start = false;

volatile unsigned long previous;
volatile unsigned long period;
volatile unsigned long median;
volatile unsigned int count = 0;

void setup()
{
  pinMode( irqPin, INPUT );
  Serial.begin( 115200 );
  attachInterrupt( irqNum, dataLogger, CHANGE );
}

void loop()
{
  if( index >= numberOfEntries )
  {
    previous = results[0];
    count = 0;
    period = 0;
    for( byte i=1; i pulse[j+1] )
        {
          median = pulse[j+1];
          pulse[j+1] = pulse[j];
          pulse[j] = median;
        }
      }
    }

    if( count == 4 )
    {
      median = (pulse[1] + pulse[2]) /2;
    }
    else
    {  
      median = pulse[1];
    }  
    Serial.print( ", " );
    Serial.println( median );

    delay( 250 );  
  }
}

void dataLogger()
{
  if( start )
  {
    if( index < numberOfEntries )
    {
      results[index] = micros() - microSeconds;
      index = index + 1;
    }
    else
    {
      for( byte i=1; i < numberOfEntries; i++ )
      {     
        results[i-1] = results[i];
      }     
      results[numberOfEntries-1] = micros() - microSeconds;   
    }
  }
  else
  {
    if( digitalRead( irqPin ) == 1 )
    {
      start = true;
    }
  }
  microSeconds = micros();  
}

This code fills a sliding windows of 8 values, then every 250ms looks for high-low pairs, adds these and then takes the mean and median. A plot of this data for slow, medium and fast motor speeds is shown in figure 30. Spreadsheet is available here: LINK.


Figure 30 : infra-red sensor mean and median period

From the low speed plot (top) you can clearly see a periodic variation in speed, probably due to a tight spot on the drive shaft i.e. at a particular point during the rotation you have an increase in friction (load). For the medium and fast speeds this is not so obvious, starting to loose the signal through aliasing, however, increasing the sampling rate to 100ms you can see the same pattern for the medium speed plot shown in figure 31.


Figure 31 : infra-red sensor medium speed increased sample rate

Looking at these plots not seeing a lot of difference between mean and median. As expected mean has more of a low pass filter smoothing affect so will use this signal as y(t).

Version 1 : hardware sensor interface

The aim of the speed controller is to maintain a constant motor speed during no-load and on-load operation. The speed of the winch drive shaft can be calculated knowing the number of pulses generated per motor revolution and the ratio of the gearbox used. The motor's drive cog has 12 teeth and the cog has 48 teeth i.e. reduction by 4. This is repeated four times in the gearbox, therefore, one revolution of the winches drive shaft will require 256 motor shaft revolutions. If the reflective stripes were on the motor shaft this could produce 1024 pulses per revolution, but as these are on the second cog this is value will be reduced by 4, so 1024/4 = 256 pulses per motor shaft revolution. Alternative gear ratios are shown in figure 32.


Figure 32 : gearbox

RULE NUMBER 2 OF BUILDING HARDWARE : CHECK, CHECK and CHECK AGAIN ....

To avoid future pain its always a good idea to double check your calculations. To check that 256 pulses were indeed generated during one revolution the following test code was written (all was good).

#include 

// INPUTS
// ------

// Yellow : micro-switch : D2
// Blue : IR sensor (filtered) :D3

#define INT0 0
#define INT1 1    
#define INT0_PIN 2
#define INT1_PIN 3

// OUTPUTS
// -------

// Yellow : relay : D12
// Green : motor : D11

#define RELAY  12
#define MOTOR_PWM 11

// CONSTANTS
// ---------

#define TEST 1 //0=AUTO, 1=MANUAL

// VARIABLES
// ---------
unsigned long volatile switchCount = 0;

unsigned long volatile irSensorCount = 0;
boolean volatile irSensorCountValid = false;

// FUNCTIONS
// ---------

// INITIALISATION
// --------------

void setup()
{
  Serial.begin(115200);  
  while (!Serial) {};  

  pinMode(INT0_PIN, INPUT);
  pinMode(INT1_PIN, INPUT);
  digitalWrite(INT0_PIN, HIGH);
  digitalWrite(INT1_PIN, HIGH);  
  
  attachInterrupt(INT0, liftPosition, FALLING);
  attachInterrupt(INT1, liftMovement, FALLING);

  pinMode(RELAY, OUTPUT);
  digitalWrite(RELAY, LOW); 

  if( TEST == 0 )
  {
    analogWrite(MOTOR_PWM, 70);
  }
  else
  {
    analogWrite(MOTOR_PWM, 0);
  }
}

// ISR
// ---

void liftPosition()
{
    switchCount = switchCount + 1;
}

void liftMovement()
{
  irSensorCount = irSensorCount + 1;
  
  if( TEST == 1)
  {
    Serial.println( irSensorCount ); 
  }
  else
  {  
    if( irSensorCount > 1024 )
    {
      analogWrite(MOTOR_PWM, 0);
      irSensorCountValid = true;
    }
  }
}

// MAIN LOOP
// ---------

void loop()
{
  while (true)
  {
    if (irSensorCountValid)
    {
      Serial.println( switchCount );
      Serial.println( irSensorCount );      
      while( true ); 
    }  
  } 
  delay( 500 );  
}

A winch drive shaft speed of 10 rev/minute (rpm) or 1 revolution every 6 seconds will produce 256/6 = 43 pulses per second, or approximately one pulse every 24ms. Therefore, a speed of 1 rpm will produce 4.3 pulses per second, or approximately one pulse every 234ms. In the code shown above the variable period is an unsigned long data type, representing a time in microseconds e.g. a time of 24ms will therefore be represented by the integer value 24,000. To convert a users speed request (rpm) into a pulse period we need to perform the following calculation: period = (1 / speed (rpm) ) x 234,000. If we were to implement this equation directly we would need to represent the very small and the very large i.e. floating point, which is probably not the best approach, with a little thought we can reduce the number of calculations, processing and data types used. The Arduino can represent the following data types:

Type Size Range
boolean 1 byte true / false
char 1 byte -128 to 127
unsigned char 1 byte 0 to 255
int 2 byte -32,768 to 32,767
unsigned int 2 byte 0 to 65,535
long 4 byte -2,147,483,648 to 2,147,483,647
unsigned long 4 byte 0 to 4,294,967,295
float 4 byte 1.2E-38 to 3.4E+38

We can simplify the equations by multiplying through by 234,00 i.e. period = 234,000 / speed (rpm). To represent these values we would need to use the long data type. However, with any system you need to consider the resolution of your data i.e. what bits within this 32bit number actually represent useful data and what are just noise. The table below shows a summary of the sensor data from figure 31:

Speed Max Min Diff Mean
Slow - mean 38085 33226 4859 35656
Slow - median 39788 33648 6140 36718
Medium - mean 17640 16077 1563 16859
Medium - median 18816 16324 2492 17570
Fast - mean 12160 11426 734 11793
Fast - median 12716 11530 1186 12123

If you consider the slow and fast speeds as the usable range for this system the sensor data has an active range of 35656 - 11793 = 23863. In operation we do not need to define 24K different speeds, therefore, internally we could scale these values by a factor of 10, allowing the int data type to be used, halving storage requirements and reducing processing time. As this scaling factor is not critical we can further improve processing performance by scaling by 8 rather than 10, as this can be implemented a using bit-shift i.e. each bit shift to the right performs a divide by 2, therefore, three bit shifts to the right performs a divide by 8. To measure the time taken to perform different maths functions the following code was written:

// OUTPUTS
// -------

#define TEST_PIN 13
  
// VARIABLES
// ---------

volatile int a0 = 1000;
volatile int b0 = 8;
int c0 = 0;

volatile unsigned int a1 = 1000;
volatile unsigned int b1 = 8;
unsigned int c1 = 0;

volatile float a2 = 1000;
volatile float b2 = 8;
float c2 = 0;

// FUNCTIONS
// ---------

// INITIALISATION
// --------------

void setup()
{
  pinMode(TEST_PIN, OUTPUT);
  digitalWrite(TEST_PIN, LOW); 
}

// MAIN LOOP
// ---------

void loop()
{
  digitalWrite(TEST_PIN, HIGH);
  digitalWrite(TEST_PIN, LOW);
  
  digitalWrite(TEST_PIN, HIGH);
  //c0 = a0 / b0;
  //c0 = a0 * b0;
  //c0 = a0 + b0; 
  digitalWrite(TEST_PIN, LOW);

  digitalWrite(TEST_PIN, HIGH);
  //c1 = a1 / b1;
  //c1 = a1 >> b1;  // b1=3
  //c1 = a1 * b1;
  c1 = a1 + b1; 
  digitalWrite(TEST_PIN, LOW);

  digitalWrite(TEST_PIN, HIGH);
  //c2 = a2 / b2;
  //c2 = a2 * b2;
  c2 = a2 + b2; 
  digitalWrite(TEST_PIN, LOW);

  delay(1);     
}

As expected, but playing around with this code, if you divide by a constant rather than a variable you see the same results as figure 34, i guess the compiler makes a similar optimisation. Not too bad for speed, approximately 10us for divide and 2us for multiplication, addition and subtraction. Now to pull all of this together into the final PID controller


Figure 33 : divide - signed, unsigned and float


Figure 34 : divide (unsigned shifted) - signed, unsigned and float


Figure 35 : multiplication, addition and subtraction - signed, unsigned and float

To control the speed of the

#include 

// INPUTS
// ------

// Yellow : micro-switch : D2
// Blue : IR sensor (filtered) :D3

#define INT0 0
#define INT1 1    
#define INT0_PIN 2
#define INT1_PIN 3

// OUTPUTS
// -------

// Yellow : relay : D12
// Green : motor : D11

#define RELAY  12
#define MOTOR_PWM 11

// CONSTANTS
// ---------

#define IRQPIN 3

const int irqNum = 1;
const int numberOfEntries = 8;

// VARIABLES
// ---------
unsigned long volatile switchCount = 0;

unsigned long volatile irSensorCount = 0;
boolean volatile irSensorCountValid = false;

volatile unsigned long microSeconds;
volatile byte index = 0;
volatile unsigned long results[ numberOfEntries ];
volatile boolean start = false;

volatile unsigned long previous;
volatile unsigned long period;
volatile unsigned int count = 0;







// FUNCTIONS
// ---------

// INITIALISATION
// --------------

void setup()
{
  Serial.begin(115200);  
  while (!Serial) {};  

  pinMode(INT0_PIN, INPUT);
  pinMode(INT1_PIN, INPUT);
  digitalWrite(INT0_PIN, HIGH);
  digitalWrite(INT1_PIN, HIGH);  
  
  attachInterrupt(INT0, liftPosition, FALLING);
  attachInterrupt(INT1, liftMovement, FALLING);

  pinMode(RELAY, OUTPUT);
  digitalWrite(RELAY, LOW); 

  if( TEST == 0 )
  {
    analogWrite(MOTOR_PWM, 70);
  }
  else
  {
    analogWrite(MOTOR_PWM, 0);
  }
}




void setup()
{
  pinMode( irqPin, INPUT );
  Serial.begin( 115200 );
  attachInterrupt( irqNum, dataLogger, CHANGE );
}

void loop()
{
  if( index >= numberOfEntries )
  {
    previous = results[0];
    count = 0;
    period = 0;
    for( byte i=1; i pulse[j+1] )
        {
          median = pulse[j+1];
          pulse[j+1] = pulse[j];
          pulse[j] = median;
        }
      }
    }

    if( count == 4 )
    {
      median = (pulse[1] + pulse[2]) /2;
    }
    else
    {  
      median = pulse[1];
    }  
    Serial.print( ", " );
    Serial.println( median );

    delay( 250 );  
  }
}

void dataLogger()
{
  if( start )
  {
    if( index < numberOfEntries )
    {
      results[index] = micros() - microSeconds;
      index = index + 1;
    }
    else
    {
      for( byte i=1; i < numberOfEntries; i++ )
      {     
        results[i-1] = results[i];
      }     
      results[numberOfEntries-1] = micros() - microSeconds;   
    }
  }
  else
  {
    if( digitalRead( irqPin ) == 1 )
    {
      start = true;
    }
  }
  microSeconds = micros();  
}



// OUTPUTS
// -------

// Yellow : relay : D12
// Green : motor : D11

#define RELAY  12
#define MOTOR_PWM 11

// CONSTANTS
// ---------

// VARIABLES
// ---------

unsigned int count = 0;

// FUNCTIONS
// ---------

// INITIALISATION
// --------------

void setup()
{
  pinMode(RELAY, OUTPUT);
  digitalWrite(RELAY, LOW); 
}

// MAIN LOOP
// ---------

void loop()
{
  while (true)
  {
    digitalWrite(RELAY, HIGH); 
    for( int i=0; i < 255; i=i+5)
    {
      analogWrite(MOTOR_PWM, i);
      delay( 500 );  
    }
    for( int i=255; i > 0; i=i-5)
    {
      analogWrite(MOTOR_PWM, i);
      delay( 500 );  
    }
    digitalWrite(RELAY, LOW); 
    for( int i=0; i < 255; i=i+5)
    {
      analogWrite(MOTOR_PWM, i);
      delay( 500 );  
    }
    for( int i=255; i > 0; i=i-5)
    {
      analogWrite(MOTOR_PWM, i);
      delay( 500 );  
    }
  }
}



WORK IN PROGRESS

Version 2 : software sensor interface

Creative Commons Licence

This work is licensed under a Creative Commons Attribution-NonCommercial-NoDerivatives 4.0 International License.

Contact details: email - mike.freeman@york.ac.uk, telephone - 01904 32(5473)

Back