/*=======================================================================+
|             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"

///////////////////////////////
//    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)


/////////////////////////////////////////////////////////////////////////
// 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;

// 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


////////////////////////////////////////////////////////////
//														  //
//    				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)
{

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

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);

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)
{

//interrupt da Usart
if(PIR1bits.RCIF)
{
	MsgRecebUsart[FaltaByte] = RCREG;//retira do buffer o byte
	if(FaltaByte)
	{
		FaltaByte = false;

		AnalisaMsgUsart(Membros, MsgRecebUsart, &NovPosServ[0][0],\
						&ValSens[0][0], &SensPe[0][0]);

	}
	else
		FaltaByte = true;

	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++;
		
		 }

	INTCON3bits.INT1IE = true;
}
}