/*=======================================================================+ | 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 #include "MasterSec.h" #include "adc.h" #include "tipos.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, byte *SensPe) { byte Msg[4]; Msg[0] = SensPe[Pe*4];//1§ sensor do pe Msg[1] = SensPe[Pe*4+1];//2§ sensor do pe Msg[2] = SensPe[Pe*4+2];//3§ sensor do pe Msg[3] = SensPe[Pe*4+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, byte *ValSens) { byte MsgEnvUsart[2]; MsgEnvUsart[0] = (UM_BYTE | Articulacao); //comeca em zero, e nao em dois como NumMCU MsgEnvUsart[0] = ValSens[(NumMCU-2)*3+NumServo]; EnvCh(MsgEnvUsart, 1); } /*---------------------------------------------------------------------+ | 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(byte *Servos) { Servos[0]=30; Servos[1]=90;Servos[2]=130;//junta 2 \ // -> Ancas Servos[3]=60;Servos[4]=23;Servos[5]=180;//junta 3 / Servos[6]=45;Servos[7]=255;Servos[8]=255;//junta 4 \ // -> Joelhos Servos[9]=0;Servos[10]=255;Servos[11]=255;//junta 5 / Servos[12]=0;Servos[13]=120;Servos[14]=255;//junta 6 \ // -> Tornozelos Servos[15]=120;Servos[16]=40;Servos[17]=255;//junta 7 / } /*---------------------------------------------------------------------+ |AnalisaMsg(PernBrac * Membros, byte *Msg, byte *Servos, byte *ValSens\| | byte *SensPe, byte *SensInerc) | | | | Entradas: Membros - estrutura indicando o MCU, PWM de controlo | | Msg - Msg a analisar | | Servos - matriz com as posiçoes dos servos a actualizar | | ValSens - matriz valor dos sensores das juntas, Posicao | | SensPe - matriz com os valores sensores Pes | | SensInerc - valor dos sensores de Inercia | | | | Saida: nada | | | | Objectivo: Analisar a msg, de forma a tomar uma decisao | +---------------------------------------------------------------------*/ void AnalisaMsgUsart(PernaBraco * Membros, byte *Msg, byte *Servos, byte *ValSens, byte *SensPe, byte *SensInerc) { byte MsgEnvUsart[2], i, a[4]; int tmp; 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-1].NumMCU; NumServo = Membros[NumMembro].Articula[Articulacao-1].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[1]==0xFF) { for(contador=0;contador>3), 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+1)),\ (contador | SideTop), ValSens); } break; } case LER_POS_ART:{ EnviaArtPos(NumMCU, (SideTop | Articulacao), NumServo, ValSens); break;} case LER_SENS_PE:{ //qual o lado e enviar sensores pe if(Articulacao) { // MsgEnvUsart[0] = (QUATRO_BYTES | (contador|SideTop)); // EnvCh(MsgEnvUsart, 1); EnviaSensPes(((Msg[0] & BIT_SIDE)>>3), SensPe); } else { for(i=0;i<3;i++) { tmp = LerADC(i); a[i] = (byte)((tmp-(int)180)<0 ? 0 : (tmp-(int)180));//(tmp-180) // a[0] = (byte)(tmp >> 2); // a[1] = (byte)(tmp & 0b0000000011); } EnvCh(a, 3); } break;} case NOV_POS_MOT: { if(Msg[2]==0xFF)//queremos colocar tdos os servos na homeposition HomePosition(Servos); else Servos[(NumMCU-2)*3+NumServo] = Msg[1]; break; } } }