File content as of revision 0:b20808175db0:

#include "LamborSteer.h"
#include "mbed.h"

LamborSteer::LamborSteer(PinName _cw_pin, PinName _ccw_pin, PinName _enablerPin, PinName _sensor_pin, int _offset)
: cw_pin(_cw_pin), ccw_pin(_ccw_pin), enabler_pin(_enablerPin), sensor_pin(_sensor_pin)
{
  if (s_index == MAX_SERVOS)
  {
    return;
  }
  
  index = s_index;
  
  _m_servos[s_index++] = this;

  tolerance = 15;
  
  /* New */
  offset = _offset;
  active = true;
  
  cw_pin = 0;
  ccw_pin = 0;
}

// timer interrupt routine
void LamborSteer::timer_int_routine()         
{
  for (int i = 0; i < s_index; i++)
  {
    if (_m_servos[i]->active)
    {
      _m_servos[i]->update();
    }
  }
}


void LamborSteer::detach()
{
  stop();
  
  active = false;
}

void LamborSteer::write(int degrees)
{
  // map degrees to potentiometer value
  int pot_value = map(degrees, MIN_ANGLE, MAX_ANGLE, MIN_POT_VALUE, MAX_POT_VALUE);  

  write_pot_value(pot_value);
}

void LamborSteer::update()
{
  int current_position = get_position();

  // Guard servo's min and max bounds
  if (current_position < MIN_POT_VALUE)
  {
    write(MIN_ANGLE);
  }

  if (current_position > MAX_POT_VALUE)
  {
    write(MAX_ANGLE);
  }

  // Gap between curent and wanted positions
  int gap = abs(current_position - wanted_position);

  // If gap is within tolerance then stop
  if (gap < tolerance)
  {
    stop();

    return;
  }

  // set speed according to gap remaining 
  speed = 100;    //125

  if (gap > 50)
  {
    speed = 100;  //165
  }
  if (gap > 75)
  {
    speed = 100;  //205
  }
  if (gap > 100)
  {
    speed = 100;  //255
  }

  // Determine direction
  if (current_position < wanted_position)
  {
    run_cw();
  }

  if (current_position > wanted_position)
  {
    run_ccw();
  }
}

servo_status LamborSteer::read()
{
  // Get current postion, wanted position and speed
  servo_status current_status;
  
  int current_angle = map(get_position(), MIN_POT_VALUE, MAX_POT_VALUE, MIN_ANGLE, MAX_ANGLE);
  int wanted_angle = map(wanted_position, MIN_POT_VALUE, MAX_POT_VALUE, MIN_ANGLE, MAX_ANGLE);

  current_status.current_angle = current_angle;
  current_status.wanted_angle = wanted_angle;
  current_status.speed = speed;

  return current_status;
}

/*************** Private stuff **************/

void LamborSteer::write_pot_value(int value)
{
  // Constrain value to servo's min and max bounds
  //int constrained_value = constrain(value, MIN_POT_VALUE + tolerance, MAX_POT_VALUE - tolerance);
  if(value > MAX_POT_VALUE - tolerance)
  {
    constrained_value = MAX_POT_VALUE - tolerance;
  }
  else if(value < MIN_POT_VALUE + tolerance)
  {
    constrained_value = MIN_POT_VALUE + tolerance;  
  }
  else
  {
    constrained_value = value;   
  }
  // set destination
  wanted_position = constrained_value;
}

// Run motor clockwise at current speed
void LamborSteer::run_cw()
{
  cw_pin = 1;
  ccw_pin = 0;

  enabler_pin.write(speed / 255.0f);
}

// Run motor counter clockwise at current speed
void LamborSteer::run_ccw()
{
  cw_pin = 0;
  ccw_pin = 1;

  enabler_pin.write(speed / 255.0f);
}

// Stop the motor, set speed to 0
void LamborSteer::stop()
{
  cw_pin = 0;
  ccw_pin = 0;
  
  enabler_pin.write(0);
}

// Get current offset potentiometer position
int LamborSteer::get_position()
{
  return (sensor_pin.read()*1023) + offset;  // Read the analog input value (value from 0.0 to 1.0)
                                             // And convert to 0 - 1023
}

// Mapping
long LamborSteer::map(long x, long in_min, long in_max, long out_min, long out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}