/*=======================================================================+
|             DEPARTAMENTO MECANICA UNIVERSIDADE AVEIRO                  |
|               Projecto Humanoide (PHUA - MODULROB)                     |
|                                                                        |
| Programadores:                                                         |
|              David Manuel Costa Gameiro          N:20281               |
|              Filipe Carvalho Mostardinha         N:22085               |
|                                                                        |
| Nome Ficheiro: MasterPrinc.c                                           |
|                                                                        |
|                            SOURCE FILE                                 |
+=======================================================================*/


#include <p18f258.h>
#include <delays.h>
#include "Tipos.h"
#include "InitPic.h"
#include "InitCan.h"
#include "Can18xx8.h"
#include "MasterSec.h"
#include "MasterTrd.h"
#include "adc.h"

///////////////////////////////
//    Declaracao de Macros   //
///////////////////////////////

// Macros da Inicializacao do PIC

#define FREQ_OSC 20 //(MHz) freq do clock interno PIC
#define FREQ_ACT 10 //(KHz) freq actualizacao valores das juntas
#define BAUD 9600	// baudrate da comunicacao serie

// Macros respeitantes aos diversos sistemas escravos

#define NUM_MEMBROS 4//existem 4 membros, 2 pernas e 2 bracos

#define NUM_MCU 7 //existem 7 MCU no controlo
#define MCU_ANCA 2

/////////////////////////////////////////////////////
//			AINDA PRECISA DE ALTERACOES 		   //
/////////////////////////////////////////////////////

// Indica o ID dos MCU na posicao dos sensores de forca
// 0bqqqqwwww, (qqqq) - lado esquerdo e (wwww) - lado direito
#define MCU_PE		0b01100111 // PES(esq|dir)


//////////////////////////
//	  MACROS DA ADC     //
//////////////////////////

//0babxxxcdx, ab - ADCON0<7:6>, c - ADCON1 <7>, d - ADCON1<1>
#define ADC_1 0b01001010
#define ADC_2 0b00001001	//datasheet pag 126


/////////////////////////////////////////////////////////////////////////
// Macro CAN, configuracao envio mensagem                              //
//																	   //
// byte definido 0b1a111bcd											   //
// a - pretende-se notificacao de recepcao de msg (1) caso contrario 0 //
// b - indica qual o tipo de indentificador (1)STD (0)XTD			   //
// [c,d] - prioridade a dar a esta mensagem, ver registo TXBnCON	   //
/////////////////////////////////////////////////////////////////////////

#define CAN_TX_FLAGS 0b11000111


///////////////////////////////
//    Declaracao Variaveis   //
///////////////////////////////

//numero de contagens a serem efectuadas, a ter poder ser 
//enviada uma nova mensagem
int Timer = 8;
byte NumByte;
// estrutura com todas articulacoes do robot
// numero MCU e qual o servo PWM
PernaBraco Membros[NUM_MEMBROS];

byte HostMcu = 0x01;//Numero deste MCU

//Variaveis USART
bool FaltaByte = false;
byte MsgRecebUsart[2];

//Variaveis CAN
byte McuActual = 2;//depois do master o primeiro escravo e o dois

//Variaveis CAN e USART, variaveis comuns
matriz1 NovPosServ;	//Tronco fica pra proximas introducoes
					//[0]|[1] anca esquerda | direita
					//[2]|[3] joelho esquerdo | direito
					//[4]|[5] pe esquerdo | direito
					//bracos ainda por implementar

matriz1 ValSens;	//sensores posicao dos servos
					//usando a logica anterior

matriz2 SensPe;		//sensores forca pes


byte SensInerc[4];
////////////////////////////////////////////////////////////
//														  //
//    				PreDeclaracao de Funcoes   			  //
//														  //
////////////////////////////////////////////////////////////

void UsartIsr(void);
void CanTimerIsr(void);

#pragma code HighIsrVector = 0x08
void HighIsrVector(void)
{
_asm
	goto UsartIsr	//rotina de interrupt mais prioritaria
_endasm
}

#pragma code LowIsrVector = 0x018
void LowIsrVector(void)
{
_asm
	goto CanTimerIsr//rotina de interrupt menos prioritaria
_endasm
}


#pragma code
void main(void)
{
byte i=12;

NumByte= 0;

PORTB = PORTA = 0;//inicializa todos os pinos desligados
PORTC = 1;
McuActual = 2;

HomePosition(&NovPosServ[0][0]);//coloca a matriz com os valores iniciais
InitTab(Membros);//inicializacao de toda a estrutura referentes ao membros

//inicializacao das configuracoes do MCU
Timer = InitPic(FREQ_OSC, BAUD, FREQ_ACT);


//SetADC(ADC_1, ADC_2);

ADCON0 = 0b10000001;
ADCON1 = 0b01001001;

InitCan(HostMcu);


//Ligar a geracao Frequencia Actualizacao apenas depois de 
//todo o MCU estar configurado
CCP1CONbits.CCP1M3 = true;
CCP1CONbits.CCP1M2 = true;


while(1)
{

	//Rotina para ler os Sensores Inerciais
};
}

#pragma interrupt UsartIsr
void UsartIsr(void)
{
byte tmp=123;
//interrupt da Usart
if(PIR1bits.RCIF)
{
	NumByte++;
	if (NumByte < 3)
	{
		MsgRecebUsart[FaltaByte] = RCREG;//retira do buffer o byte
		if(FaltaByte)
		{
			FaltaByte = false;
			
			EnvCh(&tmp,1);
			AnalisaMsgUsart(Membros, MsgRecebUsart, &NovPosServ[0][0],\
							&ValSens[0][0], &SensPe[0][0], SensInerc);
	
		}
		else
			FaltaByte = true;
	}
	else 
		{tmp = RCREG;
		 NumByte = 0;}

	PIR1bits.RCIF = false;//interrupt atendido
}

}

#pragma interrupt CanTimerIsr
void CanTimerIsr(void)
{
byte MsgRecebCan[8], CompMsgCan;
unsigned long RemoteID;
bool AcabReceb;
enum CAN_RX_MSG_FLAGS FlagsMsg;

byte MsgEnvCan[3];
bool AcabEnv;

//////////////////////////////////////////////////////////////
//         Chegada de alguma mensagem atraves do CAN        //
//////////////////////////////////////////////////////////////
if(PIR3 & 0b00000011)
{
bool ErroCan;

	ErroCan = CANReceiveMessage(&RemoteID, MsgRecebCan, &CompMsgCan, &FlagsMsg);

	if(ErroCan)
		{
//		EnvCh(MsgRecebCan, CompMsgCan);
		AnalisaMsgCan(MsgRecebCan, CompMsgCan, &ValSens[0][0], &SensPe[0][0], MCU_PE);
		}
	else
		PORTCbits.RC4 = true;//ocorreu algum erro no CAN

}


//////////////////////////////////////////////////////////////
//     Taxa a que se envia novas posicoes para o Escravos   //
//////////////////////////////////////////////////////////////
if(INTCON3bits.INT1IF)
{
		INTCON3bits.INT1IE = false;
		MsgEnvCan[0] = NovPosServ[(McuActual-2)][0];//(McuActual-2)*3
		MsgEnvCan[1] = NovPosServ[(McuActual-2)][1];
		MsgEnvCan[2] = NovPosServ[(McuActual-2)][2];

		AcabEnv = CANSendMessage((unsigned int)McuActual, MsgEnvCan, 3, CAN_TX_FLAGS);
				
		if(AcabEnv)
		{
		//para quem vai a msg
		(McuActual == NUM_MCU) ? McuActual = MCU_ANCA : McuActual++;
		
/*###########################################################
#                        RETIRAR                            #
###########################################################*/
//		Delay10KTCYx(255);Delay10KTCYx(255);//retirar este comentario
		 }

	INTCON3bits.INT1IE = true;
}
}