 #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

          motor_drive_present[i] += MOTOR_RAMP_STEP;    
        }
      }
	  
	  analogWrite(front_motor_en[i], motor_drive_present[i]);
	  analogWrite(rear_motor_en[i], motor_drive_present[i]);
    }
}

void set_motor_states (unsigned int command)
{
    switch(command)
    {
      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;
    }	
}

unsigned int set_motor_outputs (unsigned int mode)
{
  unsigned int target_out;
	
  switch(mode)
  {
	// drive motors: forward: a = high, b = low; reverse: a = low, b = high; off: both low
	case MOTOR_OFF:
	  digitalWrite(front_motor_a[i], LOW);
	  digitalWrite(front_motor_b[i], LOW);
	  digitalWrite(rear_motor_a[i], LOW);
	  digitalWrite(rear_motor_b[i], LOW);
	  //analogWrite(front_motor_en[i], 0);
	  //analogWrite(rear_motor_en[i], 0);
	  //motor_drive_present[i] = 0;
	  target_out = 0;
	break;

	case MOTOR_FORWARD_FULL:
	  digitalWrite(front_motor_a[i], HIGH);
	  digitalWrite(front_motor_b[i], LOW);
	  digitalWrite(rear_motor_a[i], HIGH);
	  digitalWrite(rear_motor_b[i], LOW);
	  //analogWrite(front_motor_en[i], FULL_SPEED);
	  //analogWrite(rear_motor_en[i], FULL_SPEED);
	  //analogWrite(front_motor_en[i], motor_drive_present[i]);
	  //analogWrite(rear_motor_en[i], motor_drive_present[i]);
	  target_out = FULL_SPEED;
	break;

	case MOTOR_FORWARD_TURN:
	  digitalWrite(front_motor_a[i], HIGH);
	  digitalWrite(front_motor_b[i], LOW);
	  digitalWrite(rear_motor_a[i], HIGH);
	  digitalWrite(rear_motor_b[i], LOW);
	  //analogWrite(front_motor_en[i], TURN_SPEED);
	  //analogWrite(rear_motor_en[i], TURN_SPEED);
	  target_out = TURN_SPEED;
	break;

	case MOTOR_REVERSE_FULL:
	  digitalWrite(front_motor_a[i], LOW);
	  digitalWrite(front_motor_b[i], HIGH);
	  digitalWrite(rear_motor_a[i], LOW);
	  digitalWrite(rear_motor_b[i], HIGH);
	  //analogWrite(front_motor_en[i], FULL_SPEED);
	  //analogWrite(rear_motor_en[i], FULL_SPEED);
	  //analogWrite(front_motor_en[i], motor_drive_present[i]);
	  //analogWrite(rear_motor_en[i], motor_drive_present[i]);          
	  target_out = FULL_SPEED;
	break;

	case MOTOR_REVERSE_TURN:
	  digitalWrite(front_motor_a[i], LOW);
	  digitalWrite(front_motor_b[i], HIGH);
	  digitalWrite(rear_motor_a[i], LOW);
	  digitalWrite(rear_motor_b[i], HIGH);
	  //analogWrite(front_motor_en[i], TURN_SPEED);
	  //analogWrite(rear_motor_en[i], TURN_SPEED);
	  target_out = TURN_SPEED;
	break;

	default:
	  digitalWrite(front_motor_a[i], LOW);
	  digitalWrite(front_motor_b[i], LOW);
	  digitalWrite(rear_motor_a[i], LOW);
	  digitalWrite(rear_motor_b[i], LOW);
	  //analogWrite(front_motor_en[i], 0);
	  //analogWrite(rear_motor_en[i], 0);
	  target_out = 0;
	break;
  }	
  
  return target_out;
}

//
