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

#include "Tipos.h"
#include "Usart.h"

/*------------------------------------------------------------------+
|   EscrValSensPe(byte NumPe, byte *Msg, matriz2 *Pes)              |
|                                                                   |
| Entradas: NumPe - qual o pe, (0)esquerdo (1)direito               |
|           *Msg - retira os valores dos sensores de for‡a,         |
|                  o vector deve apontar local vals sens for‡a      |
|           Pes - matriz para os sensores for‡a pes                 |
|                                                                   |
| Saidas: Pes                                                       |
|                                                                   |
| Objectivos: Escrever os valores sensores for‡a dos pes enviados   |
|             pelo can                                              |
+------------------------------------------------------------------*/

static void EscrValSensPe(byte NumPe, byte *Msg, byte *Pes)
{
byte i;

for (i=0;i<4;i++)
	Pes[NumPe*4+i] = Msg[i];

}

/*------------------------------------------------------------------+
|    AnalisaMsgCan(byte PicRemoto, byte *Msg, byte CompMsg,         | 
|			matriz1 *Sensores, matriz2 *SensPe, byte MCU)           |
|                                                                   |
| Entradas: PicRemoto - Nome do mcu que enviou dados                |
|           Msg - msg recebida pelo can                             |
|           ComMsg - Comprimento da mensagem recebida               |
|           Sensores - matriz de valores potenciometros servos      |
|           SensPe - matriz sensores dos pes                        |
|           MCU - indica qual os MCU referentes aos pes             |
|                                                                   |
| Saidas: Sensores, SensPe                                          |
|                                                                   |
| Objectivos: Analisar a msg recebida pelo Can e retirar os valores |
|            referentes a cada sensor                               |
+------------------------------------------------------------------*/

void AnalisaMsgCan(byte *Msg, byte CompMsg,\
				   byte *Sensores, byte *SensPe, byte MCU)
{
byte i, Dados[5];

	for(i=1; i<4; i++)//retira os val dos potenciometros
		Sensores[(Msg[0]-2)*3+(i-1)] = Msg[i];

	for(i=4;i<8;i++)//retira os sensores dos pes
		Dados[i-4] = Msg[i];

//testa qual e o mcu

	if (Msg[0] == 6)//MCU >> 4 pe esquerdo
		EscrValSensPe(0, Dados, SensPe);//aponta pra posicao a seguir
	else
		if(Msg[0] == 7)//(MCU & 0b00001111) pe direito
			EscrValSensPe(1, Dados, SensPe);
}