arduino controller (wip)

arduino_motor_controller.ino

// 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.


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 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 );
}

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 );
  }  

  //normalizedValue = headlights.Poll( direction );
//  int pulse = pulseIn( aux1, HIGH );
//  Serial.println( pulse );
  
//  if ( pulse > 1900 )
//  {
//    analogWrite( commandModePin, 255 );
//  }
//  else
//  {
//    analogWrite( commandModePin, 0 );
//  }
}