Xilinx FPGA with AVRILOS

/***************************************************************************
 Project:		AVRILOS
 Title:			Robotic Mid Level Control
 Author:		Ilias Alexopoulos
 Version:		2.00
 Last updated:	16-May-2011
 Target:		ATMEGA164p
 File:			RoboLayer.c

* Support E-mail:
* avrilos@ilialex.gr
* 
* license: See license.txt on root directory (CDDL)
*

* DESCRIPTION
* Medium Level Control For Robotics
***************************************************************************/
#include "../includes/types.h"
#include "../includes/settings.h"
//#include "../periphint/Uart.h"
//#include "../Utils/typeconv.h"
#include "../periphint/Timer1.h"
#include "../periphint/Motor.h"
#include "../periphint/adc.h"

#include "fpgassi.h"

#include "robolayer.h"

//#include "../periphext/sst.h"
//#include "../periphext/lpc.h"
//#include "../debug/debugger.h"

#include <stdio.h>


#define c_SERVOLIMITUP 1024
#define c_SERVOLIMITDN 0
#define c_SERVOLIMITZR 300

extern INT8U v_ServoLock;
extern INT8U v_ServoUnLock;

#define c_SERVOZERO 	128


static INT8 v_engine;
static INT8U buf_rcservo[8];

void f_InitRoboLayer(void)
{

	v_engine = 0;
	
	buf_rcservo[0] = c_SERVOZERO;
	buf_rcservo[1] = c_SERVOZERO;
	buf_rcservo[2] = c_SERVOZERO;
	buf_rcservo[3] = c_SERVOZERO;
	buf_rcservo[4] = c_SERVOZERO;
	buf_rcservo[5] = c_SERVOZERO;
	buf_rcservo[6] = c_SERVOZERO;
	buf_rcservo[7] = c_SERVOZERO;
	
	f_MOTOREnable(0);
	f_MOTOREnable(1);
	
	f_MOTORSet(0, 0);
	f_MOTORSet(1, 0);

}

void f_RL_MotorInc(INT8 v_step)
{
	v_engine += v_step;
}

void f_RL_EngineEnable(void)
{
    f_MOTOREnable(0);
	f_MOTOREnable(1);
}


void f_RL_EngineDisable(void)
{
    f_MOTORDisable(0);
	f_MOTORDisable(1);
}

void f_RL_EngineStop(void)
{
	f_MOTORDIR(0, stop);
	f_MOTORDIR(1, stop);
}

void f_RL_EngineFwd(void)
{
	f_MOTORDIR(0, fwd);
	f_MOTORDIR(1, fwd);
}

void f_RL_EngineBwd(void)
{
	f_MOTORDIR(0, bwd);
	f_MOTORDIR(1, bwd);
}

void f_RL_EnginePower(INT8 v_power)
{
	INT8U v_pwm;
	if (v_power < 0) v_pwm = (INT8U) (-v_power);
	else v_pwm = (INT8U) v_power;
	
	f_MOTORSet(0, v_pwm);
	f_MOTORSet(1, v_pwm);


}
	
void f_RL_DoMotion(void)
{
	if ( (v_engine < 10) && (v_engine > -10) ) f_RL_EngineStop();
	else if (v_engine > 10) f_RL_EngineFwd();
	else if (v_engine < -10) f_RL_EngineBwd();
	
	f_RL_EnginePower(v_engine);	

}


void f_RL_ServoExec(void)
{
	INT8U v_idx;
	INT8U v_fpgaservo;
	
	for(v_idx=0; v_idx < 8; v_idx++)
	{
		v_fpgaservo = buf_rcservo[v_idx];
		f_FPGAWr(v_idx+1, v_fpgaservo);
	}

}

void f_RL_RCServoInc(INT8U v_servo, INT8 v_step)
{
	buf_rcservo[v_servo] += v_step;
	if ( (v_step < 0) && (buf_rcservo[v_servo] < 7) ) buf_rcservo[v_servo] = 7;
	if ( (v_step > 0) && (buf_rcservo[v_servo] > 247) ) buf_rcservo[v_servo] = 247;
}


void f_RL_RCServoCtrl(INT8U v_servo, INT8 v_angle_deg)
{
	INT8U v_fpgaservo;
	
	if ( (v_angle_deg < 45) && (v_angle_deg > -45) )
	{
		v_fpgaservo = (200 * (45-v_angle_deg) )/90 + 20;
		buf_rcservo[v_servo] = v_fpgaservo;
	}

}

INT8U f_RL_Distance(void)
{
	INT8U v_temp;
	
	v_temp = (INT8U) (v_Chan[0] >> 2);
	return v_temp;

}