arduino_motor_controller (with red/blue lightbar)

arduino_motor_controller_lightbar

// Arduino-based motor controller
// This was designed to work with a Spektrum AR6200 receiver, but any receiver should work
// with proper calibration and setup of available channels.

#include <Adafruit_NeoPixel.h>
const uint8_t DATA_PIN = 7;
const uint8_t N_PIXELS = 8;
Adafruit_NeoPixel  strip = Adafruit_NeoPixel(N_PIXELS, DATA_PIN, NEO_GRB + NEO_KHZ800);
uint8_t cpix = 0;
int8_t dir = 1;

const long PIXEL_DELAY = 30;
long pixel_delay = PIXEL_DELAY;
long last_loop = 0;

uint8_t GEAR_INPUT = 5;
uint8_t FLAPS_INPUT = 6;

uint8_t GEAR_OUTPUT = 7;
uint8_t FLAPS_OUTPUT = 8;

// motor input pins
const uint8_t MOTOR0_INPUT = 2;
const uint8_t MOTOR1_INPUT = 4;

// motor output pins
const uint8_t MOTOR0_FORWARD = 3;
const uint8_t MOTOR0_BACKWARD = 11;
const uint8_t MOTOR1_FORWARD = 9;
const uint8_t MOTOR1_BACKWARD = 10;

struct SpektrumChannel
{
  uint16_t min_threshold;
  uint16_t max_threshold;
  uint16_t center_min;
  uint16_t center_max;
  uint16_t upper_divisor;
  uint16_t lower_divisor;
  uint8_t pin;
  
  uint8_t forward_pin;
  uint8_t backward_pin;
  
  SpektrumChannel( uint8_t _pin )
  {
     pin = _pin;
     
     // input pin from receiver
     pinMode( pin, INPUT );
  }
  
  void setup_motor_channel( uint8_t _forward_pin, uint8_t _backward_pin, uint16_t _min, uint16_t _max, uint16_t _center_min, uint16_t _center_max )
  {
    min_threshold = _min;
    max_threshold = _max;
    center_min = _center_min;
    center_max = _center_max;
    forward_pin = _forward_pin;
    backward_pin = _backward_pin;    
    
    upper_divisor = (max_threshold - center_max);
    lower_divisor = (center_min - min_threshold);
    
     // output pins for motor driver; normalized output [0, 1]
     pinMode( forward_pin, OUTPUT );
     pinMode( backward_pin, OUTPUT );
  }
  
  float poll( int8_t & direction )
  {
    int pulse = pulseIn( pin, HIGH );
    int dt = 0;
    float normalizedValue = 0.0;
    
    Serial.println( pulse );
    direction = -1;
    
    if ( pulse > 0 )
    {
      if ( pulse > center_max )
      {
        direction = 1;
        dt = pulse - center_max;
        
        normalizedValue = ((float)dt/(float)upper_divisor);
        if ( normalizedValue > 1.0 )
        {
          normalizedValue = 1.0;
        }
      }
      else if ( pulse < center_min )
      {
        direction = 0;
        dt = center_min - pulse;
        
        normalizedValue = ((float)dt/(float)lower_divisor);
        if ( normalizedValue > 1.0 )
        {
          normalizedValue = 1.0;
        }        
      }
      else
      {
        normalizedValue = 0.0;
        direction = -1;
      }
    }
//    Serial.print( "Value: " );
//    Serial.println( normalizedValue );
    return normalizedValue;
  }

  void apply_motor_speed()
  {
    int8_t direction = 0;
    int8_t analog_out = 0;
    float normalized_value = 0.0f;
    
    normalized_value = poll( direction );
    analog_out = int8_t(255.0 * normalized_value);
    
    if ( direction == 1 )
    {
      analogWrite( forward_pin, analog_out );   
      analogWrite( backward_pin, 0 );
    }
    else if ( direction == 0 )
    {
      analogWrite( forward_pin, 0 );
      analogWrite( backward_pin, analog_out );
    }
    else
    {
      analogWrite( forward_pin, 0 );
      analogWrite( backward_pin, 0 );    
    }
  }
  
  uint8_t is_pin_high()
  {
    int pulse = pulseIn( pin, HIGH );
    return (pulse > 1900);
  }
};


SpektrumChannel chan0( MOTOR0_INPUT );
SpektrumChannel chan1( MOTOR1_INPUT );
SpektrumChannel chan2( GEAR_INPUT );
SpektrumChannel chan3( FLAPS_INPUT );

void run_scanner_bar()
{
    for( uint8_t i = 0; i < N_PIXELS; ++i )
    {
      uint32_t color = strip.Color( 0, 0, 0 );
      
      if ( cpix == i )
      {
        color = strip.Color( 255, 0, 0 );
      }
      else if ( cpix == i + (dir*1) )
      {
        color = strip.Color( 128, 0, 0 );
      }
      else if ( cpix == i + (dir*2) )
      {
        color = strip.Color( 64, 0, 0 );
      }
      else if ( cpix == i + (dir*3) )
      {
        color = strip.Color( 32, 0, 0 );
      }
      
      strip.setPixelColor( i, color );
    }
    strip.show();
    
    cpix += dir;
    
    if (cpix == N_PIXELS-1 )
    {
      dir = -dir;
    }
    else if ( cpix == 0 )
    {
      dir = -dir;
    }  
}

void run_police()
{
    uint8_t num_flashes = 3;
    long current = millis();
    long delta = (current - last_loop);
    last_loop = current;
    pixel_delay -= delta;
    
    if ( pixel_delay > 0 )
    {
       return; 
    }
    
    pixel_delay = PIXEL_DELAY;
    
    uint32_t red = strip.Color( 255, 0, 0 );
    uint32_t blue = strip.Color( 0, 0, 255 );
    uint32_t black = strip.Color( 0, 0, 0 );
    for( uint8_t i = 0; i < N_PIXELS; ++i )
    {
      uint32_t color = black;
      if ( (cpix % 2) == 0 ) // every even iteration
      {
        if ( dir > 0 )
        {
          if ( i < 4 )
          {
            color = red;
          }
          else
          {
            color = black;
          }
        }
        else
        {
          if ( i < 4 )
          {
            color = black;
          }
          else
          {
            color = blue;
          }
        }
      }
      
      strip.setPixelColor( i, color );
    }
  
  
  strip.show();

  
  if ( cpix == ((num_flashes * 2)-1) )
  {
    dir = -dir;
    cpix = 0;
    return;
  }
  
  cpix++;
}

void disable_leds()
{
  for( uint8_t i = 0; i < N_PIXELS; ++i )
  {
    strip.setPixelColor( i, strip.Color(0,0,0) );
  }
  strip.show();
  
  // reset these vars
  dir = 1;
  cpix = 0;
}  

void setup()
{
  // set timer1 to default value
  TCCR1B = TCCR1B & (0b11111000 | 0x03);

  // set timer2 to default value
  TCCR2B = TCCR2B & (0b11111000 | 0x04);
  
  // these two will be motor channels
  chan0.setup_motor_channel( MOTOR0_FORWARD, MOTOR0_BACKWARD, 1108, 1875, 1490, 1515 );
  chan1.setup_motor_channel( MOTOR1_FORWARD, MOTOR1_BACKWARD, 1108, 1875, 1490, 1515 );
  
  //pinMode( GEAR_OUTPUT, OUTPUT );
  //pinMode( FLAPS_OUTPUT, OUTPUT );
  
  //digitalWrite( GEAR_OUTPUT, LOW );
  //digitalWrite( FLAPS_OUTPUT, LOW );
  
  strip.begin();
  disable_leds();
}

void loop()
{ 
  chan0.apply_motor_speed();
  chan1.apply_motor_speed();
  
//  if ( chan2.is_pin_high() )
//  {
//    digitalWrite( GEAR_OUTPUT, HIGH );
//  }
//  else
//  {
//    digitalWrite( GEAR_OUTPUT, LOW );
//  }
  
  if ( chan3.is_pin_high() )
  {
    digitalWrite( FLAPS_OUTPUT, HIGH );
  }
  else
  {
    digitalWrite( FLAPS_OUTPUT, LOW );
  }  

  if ( chan2.is_pin_high() )
  {
//    run_scanner_bar();
      run_police();
  }
  else
  {
    disable_leds();
  }
}