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

/*------------------------------------------------------------------+
|   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, matriz2 *Pes)
{
byte i;

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

/*------------------------------------------------------------------+
|    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 PicRemoto, byte *Msg, byte CompMsg,
				matriz1 *Sensores, matriz2 *SensPe, byte MCU)
{
byte i;

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

//testa qual e o mcu
	if (PicRemoto == MCU>>4)//pe esquerdo
		EscrValSensPe(0, Msg, SensPe);
	else if(PicRemoto == (MCU & 0b00001111))//pe direito
		EscrValSensPe(1, Msg, SensPe);
}