/*
 * libkhepctrl : A library to control Khepera II robots 
 * Copyright (C) 2007 Guillaume Libersat <glibersat@sigill.org>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 *   (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA
 */


#include <stdio.h>

#include "raw.h"
#include "oneway_cmds.h"

/**********
 * oneway_cmds.c
 *
 * This is where every command which doesn't need
 * data reply goes (i.e. "void" commands)
 *
 * These are only "native" functions which directly
 * maps a native command of the khepera protocol.
 *
 * Functions are sorted by Native Command Character.
 *
 **********/

/**
 * "A" : Configure the speed controller (cf Manual)
 */
khep_cmd_ret_code_t khep_cmd_config_speedctrl(khep_hand_t hand,
					      unsigned int proportionnal,
					      unsigned int integral,
					      unsigned int derivative)
{
  char cmd[KHEP_CMD_MAX];

  snprintf(cmd, KHEP_CMD_MAX,
	   "A,%d,%d,%d",
	   proportionnal,
	   integral,
	   derivative);

  return khep_cmd_raw(hand, cmd).result;
}

/**
 * "C" : Make the Khepera reach a given position
 */
khep_cmd_ret_code_t khep_cmd_reachpos(khep_hand_t hand,
				      unsigned int pos_left,
				      unsigned int pos_right)
{
  char cmd[KHEP_CMD_MAX];

  snprintf(cmd, KHEP_CMD_MAX,
	   "C,%u,%u",
	   pos_left,
	   pos_right);

  return khep_cmd_raw(hand, cmd).result;
}

/**
 * "D" : Set speed of motors
 */
khep_cmd_ret_code_t khep_cmd_setspeed(khep_hand_t hand,
				      int speed_motor_left,
				      int speed_motor_right)
{
  char cmd[KHEP_CMD_MAX];

  snprintf(cmd, KHEP_CMD_MAX,
	   "D,%d,%d",
	   speed_motor_left,
	   speed_motor_right);

  return khep_cmd_raw(hand, cmd).result;
}
				 
/**
 * "F" : Configure the position regulator (cf Manual)
 */
khep_cmd_ret_code_t khep_cmd_config_posctrl(khep_hand_t hand,
					    unsigned int proportionnal,
					    unsigned int integral,
					    unsigned int derivative)
{
  char cmd[KHEP_CMD_MAX];

  snprintf(cmd, KHEP_CMD_MAX,
	   "F,%d,%d,%d",
	   proportionnal,
	   integral,
	   derivative);

  return khep_cmd_raw(hand, cmd).result;
}

/**
 * "G" : Set value of the position counter
 */
khep_cmd_ret_code_t khep_cmd_setposcounter(khep_hand_t hand,
					   int position_motor_left,
					   int position_motor_right)
{
  char cmd[KHEP_CMD_MAX];

  snprintf(cmd, KHEP_CMD_MAX,
	   "G,%d,%d",
	   position_motor_left,
	   position_motor_right);

  return khep_cmd_raw(hand, cmd).result;
}

/**
 * "J" : Configure the speed profiler controller (cf Manual)
 */
khep_cmd_ret_code_t khep_cmd_config_speedprofiler(khep_hand_t hand,
						  unsigned int max_speed_left,
						  unsigned int acc_left,
						  unsigned int max_speed_right,
						  unsigned int acc_right)
{
  char cmd[KHEP_CMD_MAX];

  snprintf(cmd, KHEP_CMD_MAX,
	   "J,%u,%u,%u,%u",
	   max_speed_left,
	   acc_left,
	   max_speed_right,
	   acc_right);

  return khep_cmd_raw(hand, cmd).result;
}

/**
 * "L" : Change led status
 */
khep_cmd_ret_code_t khep_cmd_setled(khep_hand_t hand,
				    led_t selected,
				    led_status_t status)
{
  char cmd[KHEP_CMD_MAX];

  snprintf(cmd, KHEP_CMD_MAX,
	   "L,%u,%u",
	   selected,
	   status);

  return khep_cmd_raw(hand, cmd).result;
}

/**
 * "P" : Set PWM (see Manual, p.6)
 */
khep_cmd_ret_code_t khep_cmd_setpwm(khep_hand_t hand,
				    short int pwm_motor_left,
				    short int pwm_motor_right)
{
  char cmd[KHEP_CMD_MAX];

  snprintf(cmd, KHEP_CMD_MAX,
	   "P,%d,%d",
	   pwm_motor_left,
	   pwm_motor_right);

  return khep_cmd_raw(hand, cmd).result;
}