Wednesday, August 11, 2010

Engineering: A simple Verilog project

For this project, we have a motorized wheelchair with individually motorized left and right wheels. There are quadrature-encoded sensors on each wheel which detect 2000 increments per revolution - so each increment corresponds to 0.18 degrees of rotation. When properly inflated, the wheel radius is about 9 inches, so each increment corresponds to a distance of 0.028 inches.

The operator will control the chair using a joystick: forward to go forward, back to go backwards, left to turn left, and right to turn right. The servo control system needs to know how fast we're moving and how much we're turning so that it can make actual motion follow the operator's joystick commands. This data needs to be refreshed every 100 milliseconds.

All files for this project can be downloaded from HERE.

First we'll create the Quad2Delta module to convert the quadrature-encoded wheel position signals to increment/decrement signals which will then be fed into delta-position counters.

Quad2Delta.v
//------------------------------------------------------------------------------
//--Convert quadrature signals to position change values------------------------
//              |
//              |
//   Quadrant 2 | Quadrant 1

//    Q_State=2 | Q_State=1
//              |                             _______         _______
//        AB=01 | AB=00              B: _____/       \_______/       \___
//--------------------------            _         _______         _______
//        AB=11 |AB=10               A:  \_______/       \_______/
//              |
//    Q_State=3 | Q_State=4   Quadrant:    1   2   3   4   1   2   3   4
//   Quadrant 3 | Quadrant 4    The position increments or decrements with
//              |              each crossing from quadrant to quadrant.
//              |

module Quad2Delta (CLK, RESET, A, B, Change[15:0], SpeedError, CkWise) ;

  input A, B, CLK, RESET ;
//  A and B are the quadrant sensors.
//  CLK should be faster than the max speed we can move from quadrant to
//   quadrant.

  output [15:0] Change ;
  output SpeedError, CkWise ;
//  Change is how much the position changed since previous rising edge
//   of CLK. It can range from -2 to +2.
//  SpeedError tells us we moved so fast we went through more than
//   1 quadrant in a single CLK period.
//  CkWise tells us we're moving clockwise (counting up).

  reg SpeedError, CkWise ;
  reg [2:0] Q_State, Next_Q_State ;
  reg [1:0] AB ;
  reg [15:0] Change ;

//  On each falling edge of CLK, we read quadrant sensors A and B into
//   register AB[1:0].
//
always @ (negedge CLK)
  begin  AB[1] <= A ;  AB[0] <= B ;  end

//  Now we'll make a state machine to keep track of how we move from quadrant
//   to quadrant.
//
//  On each falling edge of CLK, we make the state machine go to the
//   next state unless RESET is asserted.
//  During RESET, we initialize the state machine, error status, and
//   position change value.
//
always @ (negedge CLK or posedge RESET)
  if (RESET)
    begin
      Q_State <= 0 ; Next_Q_State <= 0 ;
      SpeedError <= 0 ; Change <= 0 ;
    end
  else
    Q_State <= Next_Q_State ;

//  In the state machine, we can be in any of five states denoted by Q_State.
//  Q_State is zero if we don't know what quadrant we're in. Otherwise Q_State
//   is the number for the quadrant we last found ourselves in corresponding
//   to quadrant sensors A and B.
//
//  On each rising edge of CLK, we will ...
//  . Compute what the next Q_State should be based on quadrant sensors
//    A and B ( AB[1:0] ).
//  . If we move to a new quadrant, indicate how far we moved since
//    previous rising edge of CLK. This is Change.
//  . Note whether we're moving through quadrants in a clockwise direction
//    (count up) or counterclockwise direction (count down).
//  . If we jumped through 2 quadrants since the previous rising edge of CLK,
//    indicate so in Change, and set the SpeedError flag.
//
always @ (posedge CLK)
  begin  // State machine for quadrature decoder.
    if (!RESET)
      begin
        case(Q_State) // Q_State is the number for the quadrant we
                      //  last found ourselves in. Q_State is zero if
                      //  we don't know what quadrant we're in.
          0:   // At Q_State=0, Figure out which quadrant we're in
               //  and clear the position change register (Change).
            begin
              case(AB[1:0])
                2'b00:  Next_Q_State <= 1 ;
                2'b01:  Next_Q_State <= 2 ;
                2'b10:  Next_Q_State <= 4 ;
                2'b11:  Next_Q_State <= 3 ;
              endcase
              Change <= 0 ;
            end
          1:   // Check if we moved out of quadrant 1.
            case(AB[1:0])
              2'b00:    // Did not move - still same quadrant.
                begin
                  Change <= 0 ;
                  SpeedError <= 0 ;
                  Next_Q_State <= 1 ;
                end
              2'b01:    // Moved counterclockwise.
                begin
                  CkWise <= 0;
                  Change <= -1 ;
                  SpeedError <= 0 ;
                  Next_Q_State <= 2 ;
                end
              2'b10:    // Moved clockwise.
                begin
                  CkWise <= 1;
                  Change <= +1 ;
                  SpeedError <= 0 ;
                  Next_Q_State <= 4 ;
                end
              2'b11:    // Moved too fast to keep up.
                begin
                  if (CkWise) Change <= +2 ;
                  else        Change <= -2 ;
                  SpeedError <= 1 ;
                  Next_Q_State <= 3 ;
                end
            endcase
          2:   // Check if we moved out of quadrant 2.
            case(AB[1:0])
              2'b00:    // Moved clockwise.
                begin
                  CkWise <= 1;
                  Change <= +1 ;
                  SpeedError <= 0 ;
                  Next_Q_State <= 1 ;
                end
              2'b01:    // Did not move - still same quadrant.
                begin
                  Change <= 0 ;
                  SpeedError <= 0 ;
                  Next_Q_State <= 2 ;
                end
              2'b10:    // Moved too fast to keep up.
                begin
                  if (CkWise) Change <= +2 ;
                  else        Change <= -2 ;
                  SpeedError <= 1 ;
                  Next_Q_State <= 4 ;
                end
              2'b11:    // Moved counterclockwise.
                begin
                  CkWise <= 0;
                  Change <= -1 ;
                  SpeedError <= 0 ;
                  Next_Q_State <= 3 ;
                end
            endcase
          3:   // Check if we moved out of quadrant 3.
            case(AB[1:0])
              2'b00:    // Moved too fast to keep up.
                begin
                  if (CkWise) Change <= +2 ;
                  else        Change <= -2 ;
                  SpeedError <= 1 ;
                  Next_Q_State <= 1 ;
                end
              2'b01:    // Moved clockwise.
                begin
                  CkWise <= 1;
                  Change <= +1 ;
                  SpeedError <= 0 ;
                  Next_Q_State <= 2 ;
                end
              2'b10:    // Moved counterclockwise.
                begin
                  CkWise <= 0;
                  Change <= -1 ;
                  SpeedError <= 0 ;
                  Next_Q_State <= 4 ;
                end
              2'b11:    // Did not move - still same quadrant.
                begin
                  Change <= 0 ;
                  SpeedError <= 0 ;
                  Next_Q_State <= 3 ;
                end
            endcase
          4:  // Check if we moved out of quadrant 4.
            case(AB[1:0])
              2'b00:    // Moved counterclockwise.
                begin
                  CkWise <= 0;
                  Change <= -1 ;
                  SpeedError <= 0 ;
                  Next_Q_State <= 1 ;
                end
              2'b01:    // Moved too fast to keep up.
                begin
                  if (CkWise) Change <= +2 ;
                  else        Change <= -2 ;
                  SpeedError <= 1 ;
                  Next_Q_State <= 2 ;
                end
              2'b10:    // Did not move - still same quadrant.
                begin
                  Change <= 0 ;
                  SpeedError <= 0 ;
                  Next_Q_State <= 4 ;
                end
              2'b11:    // Moved clockwise.
                begin
                  CkWise <= 1;
                  Change <= +1 ;
                  SpeedError <= 0 ;
                  Next_Q_State <= 3 ;
                end
            endcase
          default:  Next_Q_State <= 0 ;
        endcase
      end
  end

endmodule
//------------------------------------------------------------------------------

In the WC_Monitor module, we'll invoke the Quad2Delta module twice - once for each wheel, left and right.

With the Quad2Delta module giving us increment/decrement signals for each wheel, we can feed these signals into accumulators. The accumulated counts over each 100 millisecond period will tell us the speed.

The speed going forward is the sum of the speed of both left plus right wheels. The rate at which we are turning corresponds to the difference between the speed of the left and right wheels.

Speed values will be updated every 25 milliseconds with the sum of the 4 preceding 25 millisecond periods giving us the average speed over the preceding 100 milliseconds. Updating every 25 milliseconds (instead of every 100 milliseconds) allows us to have less delay between measured speed and reported speed.

WCMon.v
//------------------------------------------------------------------------------
//--Convert quadrature signals to velocity and turn-rate values-----------------

module WC_Monitor (Clk_100kHz, RESET, LA, LB, RA, RB, Velocity[15:0],
                   TurnRate[15:0], DataOutStrobe, TooFast) ;

  input Clk_100kHz, RESET, LA, LB, RA, RB ;
//  Clk_100kHz: At 100 kHz, the wheelchair would have to go 159 mph before
//   there would be more than 1 quadrature count per clock cycle.
//  RESET is asserted when the system first powers on.
//  LA, LB are the quadrature-encoded sensors for the left wheel.
//  RA, RB are the quadrature-encoded sensors for the right wheel.

  output [15:0] Velocity ;
  output [15:0] TurnRate ;
  output DataOutStrobe ;
//  Velocity is the sum of how many encoder counts were detected for both left
//   and right wheels over the previous 100 mSec period.
//  TurnRate is the difference of how many encoder counts were detected for
//   left minus right wheels over the previous 100 mSec period.
//  The Velocity and TurnRate registers are updated every 25 milliseconds to
//   contain the sum of the four previous 25 mSec blocks of time.
//  DataOutStrobe tells us when to read Velocity and TurnRate registers.

  output TooFast ;
  wire TooFast, LSpeedError, RSpeedError ;
//  TooFast is a warning indicator that tells us one of the wheels exceeded the
//   maximum speed to keep from having more than 1 quadrature count per clock
//   cycle. We should never be  close to this speed under normal operation.
assign TooFast = LSpeedError || RSpeedError ;

  reg [15:0] Velocity, Vel_Accumulator ;
  reg [15:0] Velocity1, Velocity2, Velocity3, Velocity4 ;
  reg [15:0] TurnRate, TR_Accumulator ;
  reg [15:0] TurnRate1, TurnRate2, TurnRate3, TurnRate4 ;
  reg [14:0] Counter_25mSec ;
  reg DataOutStrobe ;

  wire [15:0] LChange, RChange ;

//------------------------------------------------------------------------------
//--Convert quadrature signals to position change values for both wheels--------
  Quad2Delta LQ2D(Clk_100kHz, RESET, LA, LB,
                  LChange[15:0], LSpeedError, LCkWise) ;
  Quad2Delta RQ2D(Clk_100kHz, RESET, RA, RB,
                  RChange[15:0], RSpeedError, RCkWise) ;

//------------------------------------------------------------------------------
//  Make a counter that cycles every 25 mSecs.
always @ (posedge Clk_100kHz or posedge RESET)
  if (RESET)
    Counter_25mSec <= 0 ;
  else
    if (Counter_25mSec == 2499)
      Counter_25mSec <= 0 ;
    else
      Counter_25mSec <= Counter_25mSec + 1 ;

//  Use this counter to coordinate everything that needs to be done every
//   25 milliseconds.
always @ (negedge Clk_100kHz or posedge RESET)
  if (RESET)
    begin
      Velocity1 <= 0 ; Velocity2 <= 0 ; Velocity3 <= 0 ; Velocity4 <= 0 ;
      Vel_Accumulator <= 0 ; Velocity <= 0 ;
      TurnRate1 <= 0 ; TurnRate2 <= 0 ; TurnRate3 <= 0 ; TurnRate4 <= 0 ;
      TR_Accumulator <= 0 ; TurnRate <= 0 ;
      DataOutStrobe <= 0 ;
    end
  else
    case(Counter_25mSec)
            0:  begin
                  // Save values of previous four velocity accumulators.
                  // Together, they give us the last 100 mSec worth of change.
                  Velocity4 <= Velocity3 ;
                  Velocity3 <= Velocity2 ;
                  Velocity2 <= Velocity1 ;
                  Velocity1 <= Vel_Accumulator ;
                  // Start accumulating velocity counts for this 25mSec period.
                  Vel_Accumulator <= LChange + RChange ;
                  // Save values of previous four turn rate accumulators.
                  // Together, they give us the last 100 mSec worth of change.
                  TurnRate4 <= TurnRate3 ;
                  TurnRate3 <= TurnRate2 ;
                  TurnRate2 <= TurnRate1 ;
                  TurnRate1 <= TR_Accumulator ;
                  // Start accumulating turn rate counts for this 25mSec period.
                  TR_Accumulator <= LChange - RChange ;
                  // Take away DataOutStrobe because we are about to update
                  //  the Velocity and TurnRate registers.
                  DataOutStrobe <= 0 ;
                end
            1:  begin
                  // Update the Velocity and TurnRate registers.
                  Velocity <= Velocity1 + Velocity2 + Velocity3 + Velocity4 ;
                  TurnRate <= TurnRate1 + TurnRate2 + TurnRate3 + TurnRate4 ;
                  // Continue accumulating counts for this 25mSec period.
                  Vel_Accumulator <= Vel_Accumulator + LChange + RChange ;
                  TR_Accumulator <= TR_Accumulator + LChange - RChange ;
                end
            2:  begin
                  // Assert DataOutStrobe now that we've updated the Velocity
                  //  and TurnRate registers.
                  DataOutStrobe <= 1 ;
                  // Continue accumulating counts for this 25mSec period.
                  Vel_Accumulator <= Vel_Accumulator + LChange + RChange ;
                  TR_Accumulator <= TR_Accumulator + LChange - RChange ;
                end
      default:  begin
                  // Continue accumulating counts over the rest of the
                  //  25mSec period.
                  Vel_Accumulator <= Vel_Accumulator + LChange + RChange ;
                  TR_Accumulator <= TR_Accumulator + LChange - RChange ;
                end
    endcase
endmodule
//------------------------------------------------------------------------------

For testing, we need to simulate the quadrature signals for the left and right wheels. (These are LAB[1:0] and RAB[1:0] in WCMon_Test-2.v) At the end of the test code, there are state machines for doing this. LPeriod and RPeriod control the simulated time period between quadrature steps.

LAccel and RAccel control acceleration by making LPeriod and RPeriod shrink or expand exponentially over time.

LDirCkWise and RDirCkWise control whether we're going clockwise or counter-clockwise. LStop and RStop cause the wheels to stop moving.

WCMon_Test-2.v
//------------------------------------------------------------------------------
// Verilog test bench and stimulus for WC_Monitor.

`timescale 1us/100ns
module top;
  reg Clk_100kHz, RESET ;
  reg [1:0] LAB, RAB ;

  wire [15:0] Velocity, TurnRate ;
  wire DataOutStrobe, TooFast ;

  time LPeriod, RPeriod ;

  integer  LAccel, RAccel ;
  integer  LDirCkWise, LStop, RDirCkWise, RStop ;

//------------------------------------------------------------------------------
//  Instance for WC_Monitor :
  WC_Monitor TestWCM (Clk_100kHz, RESET, LAB[1], LAB[0], RAB[1], RAB[0],
                      Velocity, TurnRate, DataOutStrobe, TooFast) ;

//------------------------------------------------------------------------------
//  Stimulus
initial
  begin
    RESET = 0 ;
    Clk_100kHz = 0;
    LAB[1:0] = 2'b00 ; RAB[1:0] = 2'b00 ;
    LPeriod = 1000 ; LAccel = 0 ; LDirCkWise = 1 ; LStop = 1 ;
    RPeriod = 1000 ; RAccel = 0 ; RDirCkWise = 1 ; RStop = 1 ;

// Toggle RESET.
    #1  RESET = 1 ; #21  RESET = 0 ;
// Move forward at 14 inches per second.
    #78 LStop = 0 ; RStop = 0 ;
        LPeriod = 2000 ; RPeriod = 2000 ;
// Stop after going 0.3 second.
    #300000  LStop = 1 ; RStop = 1 ;
// Turn clockwise.
    #200000  LStop = 0 ; RStop = 0 ; RDirCkWise = 0 ;
// Stop after turning 0.3 second.
    #300000  LStop = 1 ; RStop = 1 ;
// Move backward at 14 inches per second.
    #200000  LStop = 0 ; RStop = 0 ;
             LPeriod = 2000 ; RPeriod = 2000 ;
             LDirCkWise = 0 ; RDirCkWise = 0 ;
// Decelerate.
    #200000  LAccel = -3 ; RAccel = -3 ;
// Stop after going 0.3 second.
    #300000  LStop = 1 ; RStop = 1 ;
// Move forward at 14 inches per second.
    #200000  LAccel = 0 ; RAccel = 0 ;
             LStop = 0 ; RStop = 0 ;
             LPeriod = 2000 ; RPeriod = 2000 ;
             LDirCkWise = 1 ; RDirCkWise = 1 ;
// Decelerate.
    #200000  LAccel = -4 ; RAccel = -4 ;
// Stop after going 0.3 second.
    #300000  LStop = 1 ; RStop = 1 ;
// The End.
    #200000 $finish ;
  end

// Make Clk_100kHz.
initial forever #5 Clk_100kHz = ~Clk_100kHz ;

// LAccel and RAccel control acceleration by making LPeriod and RPeriod
//  shrink or expand exponentially over time.
always #1000
  begin
    if (!LStop)  LPeriod = LPeriod * (1000 - LAccel) / 1000 ;
    if (!RStop)  RPeriod = RPeriod * (1000 - RAccel) / 1000 ;
  end

always #LPeriod
  begin  // State machine for driving left wheel quadrature signals.
    case(LAB[1:0])
      2'b00:    // We are moving from quadrant 1 ...
        if (!LStop) begin
          if (LDirCkWise) LAB[1:0] = 2'b10 ; // to quadrant 4.
          else            LAB[1:0] = 2'b01 ; // to quadrant 2.
        end
      2'b01:    // We are moving from quadrant 2 ...
        if (!LStop) begin
          if (LDirCkWise) LAB[1:0] = 2'b00 ; // to quadrant 1.
          else            LAB[1:0] = 2'b11 ; // to quadrant 3.
        end
      2'b10:    // We are moving from quadrant 4 ...
        if (!LStop) begin
          if (LDirCkWise) LAB[1:0] = 2'b11 ; // to quadrant 3.
          else            LAB[1:0] = 2'b00 ; // to quadrant 1.
        end
      2'b11:    // We are moving from quadrant 3 ...
        if (!LStop) begin
          if (LDirCkWise) LAB[1:0] = 2'b01 ; // to quadrant 2.
          else            LAB[1:0] = 2'b10 ; // to quadrant 4.
        end
    endcase
  end

always #RPeriod
  begin  // State machine for driving right wheel quadrature signals.
    case(RAB[1:0])
      2'b00:    // We are moving from quadrant 1 ...
        if (!RStop) begin
          if (RDirCkWise) RAB[1:0] = 2'b10 ; // to quadrant 4.
          else            RAB[1:0] = 2'b01 ; // to quadrant 2.
        end
      2'b01:    // We are moving from quadrant 2 ...
        if (!RStop) begin
          if (RDirCkWise) RAB[1:0] = 2'b00 ; // to quadrant 1.
          else            RAB[1:0] = 2'b11 ; // to quadrant 3.
        end
      2'b10:    // We are moving from quadrant 4 ...
        if (!RStop) begin
          if (RDirCkWise) RAB[1:0] = 2'b11 ; // to quadrant 3.
          else            RAB[1:0] = 2'b00 ; // to quadrant 1.
        end
      2'b11:    // We are moving from quadrant 3 ...
        if (!RStop) begin
          if (RDirCkWise) RAB[1:0] = 2'b01 ; // to quadrant 2.
          else            RAB[1:0] = 2'b10 ; // to quadrant 4.
        end
    endcase
  end

endmodule
//------------------------------------------------------------------------------

This code was simulated using the SILOS Simulation Environment Demo Version 99.102.

All files for this project can be downloaded from HERE.

WCMon-Theory.txt
Quad2Delta.v
WCMon.v
WCMon_Test-2.v
WCMon_Test-2.spj