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


#include <p18f258.h>
#include "Tipos.h"
#include "InitPic.h"
#include "InitCan.h"
#include "Usart.h"
#include "Can18xx8.h"

/*------------------------------------------------------------------+
|                           Declaracao de Macros                    |
+------------------------------------------------------------------*/

#define FREQ_RES 80 //(KHz) frequencias entre 300-50 KHz
#define FREQ_OSC 20 //(MHz) frequencia do oscilador

//////////////////////////
//    BaudRate Usart    //
//////////////////////////

#define BAUD 9600

/////////////////////////
// Controlo dos Servos //
/////////////////////////

#define PINO_SERVO1 PORTBbits.RB7
#define PINO_SERVO2 PORTBbits.RB6
#define PINO_SERVO3 PORTBbits.RB5

//pwm da posicao correspondendo a 0º
#define MIN_CONTROL_SERV 0.8 //ms
//pwm da posicao correspondendo a 180º
#define MAX_CONTROL_SERV 2.0 //ms

#define MAX_ANG 170.0	//angulo maximo a ser executado pelo servo
#define MIN_ANG 5.0	//angulo minimo a ser executado pelo servo

const int MIN_PWM = 78;//(4*(int)MIN_CONTROL_SERV*(int)FREQ_RES);//78;
const int MAX_PWM = 160;//(4*MAX_CONTROL_SERV*FREQ_RES);//160;

/////////////////////////
//   Comunicacao CAN   //
/////////////////////////


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

//pino indicando erro no CAN

#define ERRO_CAN PORTBbits.RB4


/*------------------------------------------------------------------+
|                        Declaracao de Variaveis                    |
+------------------------------------------------------------------*/

byte pwm1, pwm2, pwm3, MaxCont, contador = 0;

//Vector dos diversos sensores ligados ao multiplexer
byte ValSens[7] = {10,10,10,12,34,56,89};
bool PrimeiraVez = true;

float Relacao = 0.0;

byte HostMcu;
byte MasterID;//O identificador do Master

//Rotina de interrupt mais priorit ria
void ServoIsr(void);
//Rotina de interrupt menos priorit ria
void ComIsr(void);
//Rotina para montar e envia a msg
void EnviaValSens(byte, byte, byte *);

//-----------------------------------------------------------
// Rotina de Interrupt mais prioritaria
#pragma code InterruptVectorhigh = 0x08
void InterruptVectorHigh (void)
{
	ServoIsr(); //jump to interrupt routine
}

// Rotina de Interrupt de menor prioridade
#pragma code InterruptVectorlow = 0x18
void InterruptVectorlow(void)
{
	ComIsr(); //Salta para a rotina menos importante
}

#pragma code


void main(void)
{

pwm1 = pwm2 = pwm3 = MaxCont = MIN_PWM;

MasterID = 0x01;

Relacao = ((float)(MAX_PWM - MIN_PWM) / MAX_ANG);

PORTA = 0;
PORTB = 0;
PORTC = 0;

PINO_SERVO3 = PINO_SERVO2 = PINO_SERVO1 = 1;

InitPic(FREQ_RES, FREQ_OSC, BAUD);


while(1)
{
	//Rotina respons vel pela leitura dos sensores
	//colocar aqui essa rotina
};
}


/*-----------------------------------------------------------------+
|    void ServoIsr(void)                                           |
|                                                                  |
| Entradas: Nenhuma                                                |
|                                                                  |
| Saidas: Nenhum                                                   |
|                                                                  |
| Objectivo: gerar a onda de controlo do servo, verificando se     |
|       se deve parar a geracao da onda para cada determinado pino |
+-----------------------------------------------------------------*/

#pragma interrupt ServoIsr
void ServoIsr(void)
{

byte tmp;

//referencia … frequencia de resolu‡„o, o qual acontecer  mais vezes
if(INTCON3bits.INT1IF)
{
	contador++;
	if ( contador >= pwm1)// PWM do primeiro motor
        PINO_SERVO1 = false;
   
	if (contador >= pwm2)// PWM do segundo motor
		PINO_SERVO2 = false;

	if (contador >= pwm3)// PWM do terceiro motor
		PINO_SERVO3 = false;

	if (contador >= MaxCont)
	{
	CCP1CONbits.CCP1M3 = false;//Desligar a geracao pwm
	CCP1CONbits.CCP1M2 = false;

	INTCON3bits.INT1IE = false;

	contador = 0;
	};

    INTCON3bits.INT1IF = false;
};

//frequencia de controlo servo
if (INTCONbits.INT0IF)
  {

    PINO_SERVO1 = true;
    PINO_SERVO2 = true;
    PINO_SERVO3 = true;

	/*ligamos a leitura do interrupt externo associado . resolucao 
	minima a obter pra carcterizar o controlo servo*/
	INTCON3bits.INT1IE = true;

	CCP1CONbits.CCP1M3 = true;//Ligar a geracao pwm
	CCP1CONbits.CCP1M2 = true;

    INTCONbits.INT0IF = false;
  };
}



/*-----------------------------------------------------------------+
|    void ComIsr(void)                                             |
|                                                                  |
| Entradas: Nenhuma                                                |
|                                                                  |
| Saidas: Nenhum                                                   |
|                                                                  |
| Objectivo: Analisar a mensagem que chega pelo Can                |
+-----------------------------------------------------------------*/
#pragma interrupt ComIsr
void ComIsr(void)
{
bool acabou;
byte Msg[4], CompMsg;
unsigned long ID;
enum CAN_RX_MSG_FLAGS Avisos;

//--------------------------------------------------------------
//Actualizar nome do MCU
if(PIR1bits.RCIF)
{
	if(PrimeiraVez)
	{
	HostMcu = RCREG;
	//Inicializa o CAN, sabendo agora o numero do PIC
	InitCan(HostMcu);
	

	EnvCh(&HostMcu, 1);	
	PrimeiraVez = false;
	}
};

//--------------------------------------------------------------
//chegou informacao a alguns dos buffers
if(PIR3 & 0b00000011)
{

	acabou = CANReceiveMessage(&ID, Msg, &CompMsg, &Avisos);
	if (acabou)
	{
		//Actualizacao valores pwm
		pwm1 = MIN_PWM + Relacao*max(min(Msg[0],MAX_ANG),MIN_ANG);
		
		pwm2 = MIN_PWM + Relacao*max(min(Msg[1],MAX_ANG),MIN_ANG);

		pwm3 = MIN_PWM + Relacao*max(min(Msg[2],MAX_ANG),MIN_ANG);

		MaxCont = max(pwm1,max(pwm2,pwm3));//numero maxima de contagens

		EnvCh(Msg, CompMsg);//ordem executada
	//EnvCh(&MasterID, 1);
		EnviaValSens(MasterID, HostMcu, ValSens);
	}
	else
		ERRO_CAN = true;//ocorreu um erro no bus

	switch (Avisos & CAN_RX_FILTER_BITS)
	{
	case CAN_RX_OVERFLOW:
		{
		ERRO_CAN = true;
		COMSTAT &= 0b00111111;//retirar o overflow
		};
	case CAN_RX_INVALID_MSG:
		{
		ERRO_CAN = true;
		PIR3bits.IRXIF = false;
		}
	};



	
};
}


/*-----------------------------------------------------------------+
|    void EnviaValSens(byte Destino, byte HostMcu, byte *Dados)    |
|                                                                  |
| Entradas: HostMcu - Numero do Microcontrolador                   |
|           Dados - dados a serem enviados pelo CAN                |
|           Destino - Identificador do Master                      |
|                                                                  |
| Saidas: Nenhuma                                                  |
|                                                                  |
| Objectivo: Esta Rotina envia uma mensagem com um comprimento     |
|            fixo de 8 bytes.                                      |
+-----------------------------------------------------------------*/

void EnviaValSens(byte Destino, byte HostMcu, byte *ValSens)
{
byte Msg[8], i;

	Msg[0] = HostMcu;

	for(i = 0; i<8; i++)
		Msg[i+1] = ValSens[i];

	CANSendMessage((unsigned long)Destino, Msg, 8, CAN_TX_FLAGS);

}