/*=======================================================================+ | DEPARTAMENTO MECANICA UNIVERSIDADE AVEIRO | | Projecto Humanoide (PHUA - MODULROB) | | | | Programadores: | | David Manuel Costa Gameiro N:20281 | | Filipe Carvalho Mostardinha N:22085 | | | | Nome Ficheiro: MasterSec.c | | | | SOURCE FILE | +=======================================================================*/ #include "MasterSec.h" /*---------------------------------------------------------------------+ | InitTab(PernBrac *Membros) | | Entradas: Membros - ponteiro para a estrutura indicando o MCU e o | | PWM de controlo de cada junta | | Saida: nada | | | | Objectivo: Inicializar a estrutura com os dados de cada membro | +---------------------------------------------------------------------*/ void InitTab(PernaBraco *Membros) { // Perna Esquerda Membros[0].Articula[0].NumMCU = 2; Membros[0].Articula[0].PWM = 0; Membros[0].Articula[1].NumMCU = 2; Membros[0].Articula[1].PWM = 1; Membros[0].Articula[2].NumMCU = 2; Membros[0].Articula[2].PWM = 2; Membros[0].Articula[3].NumMCU = 4; Membros[0].Articula[3].PWM = 0; Membros[0].Articula[4].NumMCU = 6; Membros[0].Articula[4].PWM = 0; Membros[0].Articula[5].NumMCU = 6; Membros[0].Articula[5].PWM = 1; // Perna Direita Membros[1].Articula[0].NumMCU = 3; Membros[1].Articula[0].PWM = 0; Membros[1].Articula[1].NumMCU = 3; Membros[1].Articula[1].PWM = 1; Membros[1].Articula[2].NumMCU = 3; Membros[1].Articula[2].PWM = 2; Membros[1].Articula[3].NumMCU = 5; Membros[1].Articula[3].PWM = 0; Membros[1].Articula[4].NumMCU = 7; Membros[1].Articula[4].PWM = 0; Membros[1].Articula[5].NumMCU = 7; Membros[1].Articula[5].PWM = 1; //Braco Esquerdo Membros[2].Articula[0].NumMCU = 2; Membros[2].Articula[0].PWM = 0; Membros[2].Articula[1].NumMCU = 2; Membros[2].Articula[1].PWM = 1; Membros[2].Articula[2].NumMCU = 2; Membros[2].Articula[2].PWM = 2; Membros[2].Articula[3].NumMCU = 4; Membros[2].Articula[3].PWM = 0; Membros[2].Articula[4].NumMCU = 6; Membros[2].Articula[4].PWM = 0; Membros[2].Articula[5].NumMCU = 6; Membros[2].Articula[5].PWM = 1; // braco Direito Membros[3].Articula[0].NumMCU = 3; Membros[3].Articula[0].PWM = 0; Membros[3].Articula[1].NumMCU = 3; Membros[3].Articula[1].PWM = 1; Membros[3].Articula[2].NumMCU = 3; Membros[3].Articula[2].PWM = 2; Membros[3].Articula[3].NumMCU = 5; Membros[3].Articula[3].PWM = 0; Membros[3].Articula[4].NumMCU = 7; Membros[3].Articula[4].PWM = 0; Membros[3].Articula[5].NumMCU = 7; Membros[3].Articula[5].PWM = 1; } /*---------------------------------------------------------------------+ | EnviaSensPe(byte Pe, matriz1 *SensPe) | | | | Entradas: Pe - indica o pe a enviar, (0) Pe esq (1) Pe dir | | SensPe - matriz com os valores dos sensores dos pes | | | | Saida: nada | | | | Objectivo: Monta e envia apenas os dados referentes ao Sens for‡a | +---------------------------------------------------------------------*/ static void EnviaSensPes(byte Pe, matriz1 *SensPe) { byte Msg[4]; Msg[0] = *SensPe[Pe][0];//1§ sensor do pe Msg[1] = *SensPe[Pe][1];//2§ sensor do pe Msg[2] = *SensPe[Pe][2];//3§ sensor do pe Msg[3] = *SensPe[Pe][3];//4§ sensor do pe EnvCh(Msg, 4); } /*---------------------------------------------------------------------+ | EnviaArtPos(byte NumMCU, byte Articulacao, byte NumServo, | | matriz1 *ValSens) | | | | Entradas: NumMCU - qual o MCU | | Articulacao - numero da Articulacao no membro | | NumServo - qual a posicao desejada(posica do motor) | | ValSens - matriz com os valores dos potenciometros | | | | Saida: nada | | | | Objectivo: Monta e envia os dados referentes aos Potenciometros mot | +---------------------------------------------------------------------*/ static void EnviaArtPos(byte NumMCU, byte Articulacao,\ byte NumServo, matriz1 *ValSens) { byte MsgEnvUsart[2]; MsgEnvUsart[0] = (UM_BYTE | Articulacao); //comeca em zero, e nao em dois como NumMCU MsgEnvUsart[1] = *ValSens[NumMCU-1][NumServo]; EnvCh(MsgEnvUsart, 2); } /*---------------------------------------------------------------------+ | HomePosition(Servos) | | | | Entradas: Servos - matriz com as posi‡oes a colocar nos servos | | | | Saida: nada | | | | Objectivo: Analisar a msg, de forma a tomar uma decisao | +---------------------------------------------------------------------*/ void HomePosition(matriz1 *Servos) { *Servos[0][0]=30;*Servos[0][1]=30;*Servos[0][2]=30;//junta 2 \ // -> Ancas *Servos[1][0]=30;*Servos[1][1]=30;*Servos[1][2]=30;//junta 3 / *Servos[2][0]=0;*Servos[2][1]=0XFF;*Servos[2][2]=0XFF;//junta 4 \ // -> Joelhos *Servos[3][0]=0;*Servos[3][1]=0XFF;*Servos[3][2]=0XFF;//junta 5 / *Servos[4][0]=0;*Servos[4][1]=0;*Servos[4][2]=0XFF;//junta 6 \ // -> Tornozelos *Servos[5][0]=0;*Servos[5][1]=0;*Servos[5][2]=0XFF;//junta 7 / } /*---------------------------------------------------------------------+ |AnalisaMsg(PernBrac * Membros, byte *Msg, byte *Inputs, byte *Outputs)| | | | Entradas: Membros - estrutura indicando o MCU, PWM de controlo | | Msg - Msg a analisar | | Inputs - matriz com os dados a enviar pelo Can | | Outputs - matriz dados a ler pra enviar pela porta serie | | | | Saida: nada | | | | Objectivo: Analisar a msg, de forma a tomar uma decisao | +---------------------------------------------------------------------*/ void AnalisaMsgUsart(PernaBraco * Membros, byte *Msg, matriz1 *Servos, matriz1 *ValSens, matriz2 *SensPe) { byte MsgEnvUsart[2]; byte NumMembro, NumMCU, Articulacao, NumServo, contador, SideTop; Articulacao = Msg[0] & BITS_IDENT;// retirar o numero da articulacao SideTop = (Msg[0] & BITS_SIDE_TOP);//retira os bits do membro a usar NumMembro = SideTop >> 3; //qual e o membro na matriz de valores NumMCU = Membros[NumMembro].Articula[Articulacao].NumMCU; NumServo = Membros[NumMembro].Articula[Articulacao].PWM; switch (Msg[0] & BITS_TIPO_MSG) // tipo de mensagem requerida { case LER_TDOS_SENS://pede o valor {//pede os valores de todos os sensores dos membro if (Msg[2]==0xFF) { for(contador=0;contador>4), SensPe); } else //apenas os sensores de uma determinada junta { //envia tdas as posicoes dos motores desta junta for (contador = 0; (contador < 3); contador++) EnviaArtPos(NumMCU, (SideTop | Articulacao),\ (contador | SideTop), ValSens); if(!(Msg[0] & BIT_TOP))//perante uma perna //qual a perna EnviaSensPes(((Msg[0] & BIT_SIDE)>>4), SensPe); } break; } case LER_POS_ART:{ EnvCh(&Articulacao, 1); EnviaArtPos(NumMCU, (SideTop | Articulacao), NumServo, ValSens); break;} case LER_SENS_PE:{ //qual o lado e enviar sensores pe MsgEnvUsart[0] = (QUATRO_BYTES | (contador|SideTop)); EnvCh(MsgEnvUsart, 1); EnviaSensPes(((Msg[0] & BIT_SIDE)>>4), SensPe); break;} case NOV_POS_MOT: { if(Msg[2]==0xFF)//queremos colocar tdos os servos na homeposition HomePosition(Servos); else *Servos[NumMCU][NumServo] = Msg[1]; break;} } }