/*=======================================================================+
|             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 "Tipos.h"
#include "MasterSec.h"
#include "InitPic.h"
#include "InitCan.h"

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

#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

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

// indica quais os mcu dos pes, 0baaaabbbb
#define MCU_PE 0b01100111 //aaaa - pe esquerdo, bbbb - pe direito


#define NUM_MCU 7

/////////////////////////////////////////////////////////////////////////
// Macro CAN                                                           //
//																	   //
// byte definido 0b1a111bcd											   //
// a - pretende-se notifica‡„o de recep‡ao 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 0b10000111


///////////////////////////////
//  Declaracao de variaveis  //
///////////////////////////////

byte Timer;//numero de contagens a efectuar pelo timer

PernaBraco Membros[NUM_MEMBROS];

byte HostMCU = 0x01;// MCU master

//variaveis Usart
bool FaltaByte = false;
byte MsgRecebUsart[2];

//variaveis Can
byte McuActual = 2;

matriz1 NovPosServ;//Tronco possivel nova introducao*/
					//[0] anca esquerda
					//[1] anca direita
					//[2] joelho esquerdo
					//[3] joelho direito
					//[4] p‚ esquerdo
					//[5] p‚ direito
					//bracos ainda ¤ implementados

matriz1 ValSens;// sensores de posi‡„o dos motores

matriz2 SensPe;//sensores de for‡a pes


///////////////////////////////
//   Declaracao de funcoes   //
///////////////////////////////

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



#pragma code IsrHigh=0x08
void IsrHigh(void)
{
UsartIsr(); //salta para a rotina de interrupt da porta serie
}

#pragma code IsrLow=0x018
void IsrLow(void)
{
CanTimerIsr();
}

#pragma code
void main(void)
{
byte tmp;

PORTC=PORTB=PORTA=0;

HomePosition(&NovPosServ);//coloca tdos as juntas na homeposition
InitTab(Membros);//valores de incializacao da estrutura dos Membros

InitPic(FREQ_OSC, BAUD, FREQ_ACT);
InitCan();

/*apenas agora o timer pode funcionar para realizar as actualiza‡oes
dos valores dos diversos sensores, e posicionamento dos servos */

Timer = TMR0L;

T0CONbits.TMR0ON = true;//liga-se o timer0




PIE3 |= 0b00011111;

while(1);
}


#pragma interrupt UsartIsr
void UsartIsr(void)
{
byte tmp;

if (PIR1bits.RCIF)
{
	MsgRecebUsart[FaltaByte]=RCREG;
	
	if (FaltaByte)//ja chegou o ultimo byte
	{
		FaltaByte = false;

		AnalisaMsgUsart(Membros, MsgRecebUsart, &NovPosServ,
					 &ValSens, &SensPe);
	}
	else
		FaltaByte = true;


	PIR1bits.RCIF = 0;
}
}

/*--------------------------------------------------------------------+
|                    Interrupt ou do Timer0 ou do  Can                |
+--------------------------------------------------------------------*/

#pragma interrupt CanTimerIsr
void CanTimerIsr(void)
{

byte MsgRecebCan[8], CompMsgCan, a;
byte MsgEnvCan[3];
unsigned long RemoteID;
bool AcabReceb, AcabEnv;
enum CAN_RX_MSG_FLAGS tmp;

////////////////////////////////////////////////////////////
//    Taxa de reenvio de novas posi‡oes para os servos    //
////////////////////////////////////////////////////////////
if(INTCONbits.TMR0IF)
	{

	if(CANIsTxReady())// Existem buffers vazios
	{	//variavel indicando pra quem vai a mensagem

		if(McuActual == NUM_MCU)
			McuActual = 2;
		else
			McuActual++;


		MsgEnvCan[0] = NovPosServ[McuActual][0];
		MsgEnvCan[1] = NovPosServ[McuActual][1];
		MsgEnvCan[2] = NovPosServ[McuActual][2];
		AcabEnv = !CANSendMessage(McuActual, MsgEnvCan, 3, CAN_TX_FLAGS);

		EnvCh(&Timer, 1);
		EnvCh(&McuActual, 1);
	
	}
	
	if(PIR3bits.ERRIF || PIR3bits.IRXIF)
	{
		a = 45;
		EnvCh(&a, 1);
		PIR3bits.ERRIF=false;
		PIR3bits.IRXIF=false;
	}

	TMR0L = Timer;

	INTCONbits.TMR0IF = false;
	}

if(PIR3 & 0b00011100)
{
	a = 89;
	EnvCh(&a, 1);
}

////////////////////////////////////////////////////////////
//          Chegada de alguma mensagem pelo CAN           //
////////////////////////////////////////////////////////////
if(PIR3 & 0b00000011)//chegou algum dado pelo Can
	{
	//retira a mensagem recebida
	if(CANReceiveMessage(&RemoteID, MsgRecebCan, &CompMsgCan, &tmp))
		AnalisaMsgCan((byte)RemoteID, MsgRecebCan, CompMsgCan,
					 &ValSens, &SensPe, MCU_PE);
	else
		PORTAbits.RA1 = true;// ocorreu algum erro com o Can

	a = 45;

	EnvCh(&a, 1);

	PIR3 &= 0b11111100;//desactiva os interrupts chegada msg
	};


}