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

/*------------------------------------------------------------------+
|                           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 posi‡„o correspondendo a 0§
#define MIN_CONTROL_SERV 0.8E-3
//pwm da posi‡„o correspondendo a 180§
#define MAX_CONTROL_SERV 2.1E-3

#define MAX_ANG 140

const int MIN_PWM = (MIN_CONTROL_SERV*FREQ_RES*1E3);
const int MAX_PWM = (MAX_CONTROL_SERV*(FREQ_RES*1E3-20E3));

/////////////////////////
//   Comunicacao CAN   //
/////////////////////////
//pino indicando erro no CAN
#define ERRO_CAN PORTBbits.RB4

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

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

float Relacao = 0.0;

byte HostMCU;
byte MasterID;

//Rotina de interrupt mais priorit ria
void ServoIsr(void);
//Rotina de interrupt menos priorit ria
void ComIsr(void);

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


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

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

InitPic(FREQ_RES, FREQ_OSC, BAUD);


//InitCan();



while(2)
{
	//Rotina respons vel pela leitura dos sensores
};
}


/*-----------------------------------------------------------------+
|    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)
	{
	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;

    INTCONbits.INT0IF = false;
  };
}



/*-----------------------------------------------------------------+
|    void ComIsr(void)                                             |
|                                                                  |
| Entradas: Nenhuma                                                |
|                                                                  |
| Saidas: Nenhum                                                   |
|                                                                  |
| Objectivo: Analisar a mensagem que chega pelo Can                |
+-----------------------------------------------------------------*/
#pragma interrupt ComIsr
void ComIsr(void)
{
byte tmp;

bool acabou;
byte Msg[4], CompMsg;
enum CAN_RX_MSG_FLAGS Avisos;

//--------------------------------------------------------------
//Actualizar nome do MCU
if(PIR1bits.RCIF)
{
	HostMCU = RCREG; //nome do PIC

	//Inicializa o CAN, sabendo agora o numero do PIC
	InitCan();
	
tmp = 12;
EnvCh(&tmp, 1);	

	CANSetOperationMode(CAN_OP_MODE_NORMAL);
};

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

	acabou = CANReceiveMessage((unsigned long *)MasterID, Msg,
								 &CompMsg, &Avisos);
	if (acabou)
	{
		//Actualizacao valores pwm
		if(Msg[0] <= MAX_ANG)
			pwm1 = MIN_PWM + Relacao*Msg[0];
		
		if(Msg[1] <= MAX_ANG)
			pwm2 = MIN_PWM + Relacao*Msg[1];

		if(Msg[2] <= MAX_ANG)
			pwm2 = MIN_PWM + Relacao*Msg[2];

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

		tmp = 15;
		EnvCh(&tmp, 1);	
	}
	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;
		}
	};

	//Envia os dados dos diversos sensores

	
};

}