/*
 *  B12.c  (c) 1994 by Sam R. Thangiah (thangiah@samuel.cpsc.sru.edu)
 *  This program may be freely copied for educational
 *  and research purposes.  All other rights reserved.
 *
 *  file:	b12base.a68
 *
 *  purpose:	This program is the C Language code that supports the
 *              functions called from the main.c file. The
 *              funtionality of the procedures are explained in the paper
 *              "A C Language Interface for the B12 Mobile Robot", in 
 *              Proceedings of the American Society for Engineering Education
 *              Conference, Edmonton, Canada, 1994.
 */



#include "define.h"

void Delay();

void Initialize_B12()
/* initialize the b12 mobile robot */
{
	void Find_Rotate_Index();
	void Set_Rotate_Accel();
	void Set_Rotate_Velo();

	Find_Rotate_Index();   /* set the robot to the initial starting position*/	
	Set_Rotate_Accel(1);   /* set accel to 1 rev. per second sq. */
	Set_Rotate_Velo(1);    /* set vel to 1 rev. per second sq. */

}


void Wait_TBase_Stop()		
{
	void AWait_TBase_Stop();
	void Delay();
	extern b12_value; 
	
	/* wait until the robot has stopped moving */
	b12_value = 10;
	Delay(1); 
	while (b12_value != 1)
	{
		Delay(1);
		AWait_TBase_Stop();
		if (b12_value == 0) return;
	}	
} 

void Wait_RBase_Stop()		/* wait until base has stopped rotating */
{
	void AWait_RBase_Stop();
	void Delay(); 
	extern b12_value; 
	
	/* wait until the robot has stopped rotating */
	b12_value = 10;
	Delay(1); 
	while (b12_value != 1)
	{
		Delay(1);
		AWait_RBase_Stop();
		if (b12_value == 0) return;
	}	
}



/*****************************************************************************
						Find_Rotate_Index()
This procedure is used to find the rotaion index.  The rotation index is a 
sensor that aligns the robot to an initial position with the wheels parallel
to each other for perpendicular movement.  The rotated position of the robot
can be queryed using the SR (status report) command.
****************************************************************************/

void Find_Rotate_Index()
{
	void AFind_Rotate_Index();

	AFind_Rotate_Index();

}

void Set_Rotate_Accel(num)
int num;
/* set the rotate acceleration */
{
	void ASet_Rotate_Accel(); 
	extern user_value;

	if (num > MaxRotAccel)
		num = MaxRotAccel;
	if (num < MinRotAccel)
		num = MinRotAccel;

	/* a hex value  of 20 (dec. 32) results in 1 rev./ second */
     user_value = num * 32;  
	 ASet_Rotate_Accel(); 

}

void Set_Rotate_Velo(num)
int num;
/* set the velocity  */
{
	void ASet_Rotate_Velo(); 
	extern user_value;
	
	if (num > MaxRotVelo)
		num = MaxRotVelo;
	if (num < MinRotVelo)
		num = MinRotVelo;

	/* a hex value  of 20 (dec. 32) results in 1 rev./ second */
    user_value = num * 32;  
    ASet_Rotate_Velo(); 
}



void Rotate_Const_Clock()
/* this procedure rotates the robot at a constant velocity and acceleration
  in the clockwise direction  */
{
	void ARotate_Const_Clock();
	ARotate_Const_Clock();
}

void Rotate_Const_AntiClock()
/* this procedure rotates the robot at a constant velocity and acceleration
  in the anti-clockwise direction */
{
	void ARotate_Const_AntiClock();
	ARotate_Const_AntiClock();
}

void Move_Const_Forward()
/* this procedure moves the robot at a constant velocity and accelration */
{
	void AMove_Const_Forward();
	AMove_Const_Forward(); 
}

void Move_Const_Backward()
/* this procedure moves the robot at a constant velocity and accelration */
{
	void AMove_Const_Backward();
	AMove_Const_Backward(); 
}

void Kill_B12()
/* this procedure kill both the motors in the b12 */
{
	void AKill_B12();
	AKill_B12();

}

void Rotate_Clock(num)
int num;
/* Rotate Relative Position Positive (32 bit parameter) */
{
    void ARotate_RP_Positive();
    void Wait_RBase_Stop();
	extern user_value;
	int seconds;

		if (num > MaxAngle)
			num = MaxAngle;
		if (num < MinAngle)
			num = MinAngle;


		  user_value = num * 3;  
		  /* user_value = num * 328.3; */  
		  ARotate_RP_Positive();

		  Wait_RBase_Stop();
}

void Rotate_Anti_Clock(num)
int num;
/* Rotate Relative Position Negative (32 bit parameter) - These
commands cause the B12 to rotate to a new position given as an
offset of its present position using the current acceleration and
velocity values.  The parameter units are encoder counts. */
{
	void ARotate_RP_Negative();
	extern user_value;
	int seconds;
 
		if (num > MaxAngle)
			num = MaxAngle;
		else if (num < MinAngle)
			num = MinAngle;
	  
        user_value = num * 3;
		ARotate_RP_Negative();

    	Wait_RBase_Stop();
	
}


void Move_Forward(num)
int num;
/* Translate Relative Positive (32 bit parameter) - SEE Translate
Relative Negative. */
{
	 void ATranslate_RP_Positive();
    void Wait_TBase_Stop();
	 extern user_value;
     int seconds;

        /*num *= (int) 268.267;   conversion to decimal (1cm =268) */
		user_value = num * 268;
		ATranslate_RP_Positive();
		
		Wait_TBase_Stop();     
}

void Move_Back(num)
int num;
/* Translate Relative Negative (32 bit parameter) - These commands
cause the the B12 to translate to a new position given as an offset 
of its present position.  The parameter units are  the encoder counts. 
*/
{
	extern user_value;
    void Wait_TBase_Stop();
	void ATranslate_RP_Negative();
	int seconds;
         /* num *= (int) 268.267;   conversion to decimal (1cm = 268) */
		user_value = num * 268;
		ATranslate_RP_Negative();

		/* keep the 68000 busy till the movement is done */
		seconds= num/5;
		Delay(seconds);
	
		Wait_TBase_Stop();     
     
}

void Ping_Sonars()
{
	extern short Sonar[];
	void APing_Sonars();
	int seconds;
	int i;
	
	/* read in the sonar values */
	APing_Sonars();			/* get the sonar values into the Sonar array */

    /* Sonars[0] = Sonar[0];
    Sonars[1] = Sonar[1];
    Sonars[2] = Sonar[2];
    Sonars[3] = Sonar[3];
    Sonars[4] = Sonar[4];
    Sonars[5] = Sonar[5];
    Sonars[6] = Sonar[6];
    Sonars[7] = Sonar[7];
    Sonars[8] = Sonar[8];
    Sonars[9] = Sonar[9];
    Sonars[10] = Sonar[10];
    Sonars[11] = Sonar[11]; */
    

	/* delay the sonar */
	seconds=1;
	Delay(seconds);
}

void Delay(sec)
int sec;
{
	int i,j,k;
	for(k=0; k < sec; k++)
		for(i=0; i < 1000; i++)
			for (j=0; j < 500; j++)
		{
			i=i;
		}
}