 #define MOTOR_FORWARD_TURN          33
 #define MOTOR_REVERSE_FULL          44
 #define MOTOR_REVERSE_TURN          55
-#define MOTOR_RAMP_STEP             25
+
+#define MOTOR_RAMP_STEP             5
+#define LOOP_DELAY_MS				50
 #define MOTOR_RAMP_THHRESHOLD       50
 
 #define BUTTON_MASK                 0x10

      digitalWrite(13, LOW);
    }
    
    switch(xbee_command & JOYSTICK_MASK)
    {
      case XBEE_COMMAND_STOP:
        // stop position: all off
        motor_state[LEFT_MOTORS] = MOTOR_OFF;
        motor_state[RIGHT_MOTORS] = MOTOR_OFF;
        Serial.println("xbee command stop");
      break;

      case XBEE_COMMAND_FORWARD:
        // forward position: all forward full speed
        motor_state[LEFT_MOTORS] = MOTOR_FORWARD_FULL;
        motor_state[RIGHT_MOTORS] = MOTOR_FORWARD_FULL;
        Serial.println("xbee command forward");
      break;

      case XBEE_COMMAND_FORWARD_LEFT:
        // forward left: left motors forward turn speed, right motors forward full speed
        motor_state[LEFT_MOTORS] = MOTOR_FORWARD_TURN;
        motor_state[RIGHT_MOTORS] = MOTOR_FORWARD_FULL;
        Serial.println("xbee command forward left");
      break;
      
      case XBEE_COMMAND_FORWARD_RIGHT:
        // forward right: left motors forward full speed, right motors forward turn speed
        motor_state[LEFT_MOTORS] = MOTOR_FORWARD_FULL;
        motor_state[RIGHT_MOTORS] = MOTOR_FORWARD_TURN;
        Serial.println("xbee command forward right");
      break;
      
      case XBEE_COMMAND_REVERSE:
        // reverse position: all reverse full speed
        motor_state[LEFT_MOTORS] = MOTOR_REVERSE_FULL;
        motor_state[RIGHT_MOTORS] = MOTOR_REVERSE_FULL;
        Serial.println("xbee command reverse");
      break;
      
      case XBEE_COMMAND_REVERSE_LEFT:
        // reverse left: left motors reverse turn speed, right motors reverse full speed
        motor_state[LEFT_MOTORS] = MOTOR_REVERSE_TURN;
        motor_state[RIGHT_MOTORS] = MOTOR_REVERSE_FULL;
        Serial.println("xbee command reverse left");
      break;
      
      case XBEE_COMMAND_REVERSE_RIGHT:
        // reverse right: left motors reverse full speed, right motors reverse turn speed
        motor_state[LEFT_MOTORS] = MOTOR_REVERSE_FULL;
        motor_state[RIGHT_MOTORS] = MOTOR_REVERSE_TURN;
        Serial.println("xbee command reverse right");
      break;
      
      case XBEE_COMMAND_TURN_LEFT:
        // turn left position: left motors reverse full speed, right motors forward full speed
        motor_state[LEFT_MOTORS] = MOTOR_REVERSE_FULL;
        motor_state[RIGHT_MOTORS] = MOTOR_FORWARD_FULL;
        Serial.println("xbee command turn left");
      break;
      
      case XBEE_COMMAND_TURN_RIGHT:
        // turn right position: left motors forward full speed, right motors reverse full speed
        motor_state[LEFT_MOTORS] = MOTOR_FORWARD_FULL;
        motor_state[RIGHT_MOTORS] = MOTOR_REVERSE_FULL;
        Serial.println("xbee command turn right");
      break;

      default:
        // default is stop: all off
        motor_state[LEFT_MOTORS] = MOTOR_OFF;
        motor_state[RIGHT_MOTORS] = MOTOR_OFF;
        Serial.println("Xbee command error");
      break;
    }

    // read motor states and drive motors    
    for(i = 0; i < 2; i++)
    {
      if (abs(motor_drive_present[i] - motor_drive_target[i]) < MOTOR_RAMP_THHRESHOLD)
      {
        // if close to target, set to target
        motor_drive_present[i] = motor_drive_target[i];
      }
      else

unsigned int rear_motor_en[2] = {9, 6};

unsigned int motor_state[2];
unsigned int motor_drive_target[2];
unsigned int motor_drive_present[2];

void set_motor_states (unsigned int command);
unsigned int set_motor_outputs (unsigned int mode);

void setup() {
  // put your setup code here, to run once:
