/**********************************************************
 * motor.c
 *
 * DESCRIPTION
 *  Provides extended functions for querying the state of 
 *  the motors. See below for an explanation of the byte 
 *  address and bits where the motor status is stored.
 *
 * CREATED
 *  August 3rd, 2005
 *
 * EXTERNAL DEPENDANCIES
 *  none
 *
 * Copyright (C) 2005 Vincent Filby
 *
 * 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.
 *
 */


/* EXPLANATION OF THE MOTOR CONTROL BYTE
 *
 * The byte that is usedto turn the motors on and establish
 * direction is located at 0x0e.
 *
 *   [ ][ ][ ][ ][ ][ ][ ][ ]
 *    |  |  |  |  |  |  |  |
 *    |  |  |  |  |  |  |  +- Direction for motor 0
 *    |  |  |  |  |  |  +---- Direction for motor 1
 *    |  |  |  |  |  +------- Direction for motor 2
 *    |  |  |  |  +---------- Direction for motor 3
 *    |  |  |  +------------- On/Off for Motor 0
 *    |  |  +---------------- On/Off for Motor 1
 *    |  +------------------- On/Off for Motor 2
 *    +---------------------- On/Off for Motor 3
 *
 * For more examples or information on motor control see
 * 'MOTOR PRIMITIVES' in lib_hb.c (around line 98)
 *
 */


/*************************************
 * get_motor_status( int motor )
 *
 * DESCRIPTION
 * 	Queries the motor controller and returns the
 * 	status of the motor in question
 *
 * PARAMETERS
 *  motor - The number of the motor port (0-3)
 *          that you want to find the status
 *          of
 *
 * RETURNS
 *   1 - Motor is on and going forward
 *   0 - Motor is off
 *  -1 - Motor is on and going backwards
 *
 */
int get_motor_status( int motor ) {
	
	/* Check to see if the motor is off */
	if( ((peek( 0x0e ) >> (4 + motor)) & 0x1) == 0 )
		return 0;

	/* Determine motor dircetion */
	if( ((peek( 0x0e ) >> motor) & 0x1) == 1 )
		return -1;
	else
		return 1;

} /* get_motor_status() */


/*************************************
 * get_motor_speed( int motor )
 *
 * DESCRIPTION
 *  Returns the speed of 'motor'
 *
 * PARAMTERS
 *  motor - The number of the motor port (0-3)
 *          that you want to find the status
 *          of
 *
 * RETURNS
 *  The speed of the given motor as a value
 *  between 0 and 255.
 *
 * ADDITIONAL NOTES
 *  The Handyboard by default has only 7 graduations
 *  of power control even though you can set the
 *  speed with a range 0 to 100.  The 7 speeds are
 *  represented by the following bit patterns.
 *
 *          0b00000000
 *          0b00010001
 *          0b01001001
 *          0b01010101
 *          0b01010111
 *          0b01110111
 *          0b01111111
 *          0b11111111
 *
 */
int get_motor_speed( int motor ) {
	return peek( 0x22 + motor );
} /* get_motor_speed() */