controlador.cpp

00001 /**
00002 ******************************************************************************
00003 * @file    controlador.cpp
00004 * @author  Kleber Lima da Silva (kleber@ioton.cc)
00005 * @version V0.0.1
00006 * @date    05-Julho-2017
00007 * @brief   Biblioteca para controle de velocidade usando os encoders.
00008 ******************************************************************************
00009 * @attention
00010 *
00011 * COPYRIGHT(c) 2017 IOTON Technology
00012 *
00013 * Licensed under the Apache License, Version 2.0 (the "License");
00014 * you may not use this file except in compliance with the License.
00015 * You may obtain a copy of the License at
00016 *
00017 *     http://www.apache.org/licenses/LICENSE-2.0
00018 *
00019 * Unless required by applicable law or agreed to in writing, software
00020 * distributed under the License is distributed on an "AS IS" BASIS,
00021 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00022 * See the License for the specific language governing permissions and
00023 * limitations under the License.
00024 ******************************************************************************
00025 */
00026 
00027 #include "controlador.h"
00028 
00029 
00030 /******************************************************************************/
00031 /** @addtogroup Biblioteca do controlador de velocidade (perfil retangular)
00032 * @{
00033 */
00034 
00035 /* Variáveis Externas --------------------------------------------------------*/
00036 int32_t distancia_mm = 0;
00037 int32_t distancia_deg = 0;
00038 
00039 
00040 /* Variáveis Privadas --------------------------------------------------------*/
00041 int8_t tipo_movimento = 0;
00042 int32_t setpoint_dist = 0;
00043 int32_t setpoint_speed_esquerda_cnt = 0, setpoint_speed_direita_cnt = 0;
00044 int32_t encoder_esquerda_delta = 0, encoder_direita_delta = 0;
00045 int32_t encoder_esquerda_anterior_cnt = 0, encoder_direita_anterior_cnt = 0;
00046 int32_t pid_esquerda_out = 0, pid_direita_out = 0;
00047 int32_t erro_esquerda_anterior = 0, erro_direita_anterior = 0;
00048 bool bUsarSensores = false;
00049 float frontal_esquerdo = 0, lateral_esquerdo = 0, lateral_direito = 0, frontal_direito = 0;
00050 
00051 
00052 /*******************************************************************************
00053 *Função principal do controle de velocidade -----------------------------------
00054 */
00055 void controleVelocidade(void)
00056 {
00057   getSensoresParede(&frontal_esquerdo, &lateral_esquerdo, &lateral_direito, &frontal_direito);
00058   updateEncoders(); // Atualiza as distâncias e velocidades
00059   controladorPID(); // Controlador de velocidade
00060 }
00061 
00062 
00063 /* Função para selecionar os parâmetros do movimento ---------------------------
00064 * raio [mm]: 0 para movimento translacional ou raio para a curva
00065 * dist_ou_angulo [mm ou deg]: distância(translational ou angular) do movimento
00066 * speed [mm/s]: velocidade do movimento
00067 */
00068 void setMovimento(int32_t raio, int32_t dist_ou_angulo, int32_t speed)
00069 {
00070   if (raio == 0)
00071   {
00072     tipo_movimento = TRANSLACIONAL;
00073 
00074     /* Usado para o controle de velocidade */
00075     setpoint_speed_esquerda_cnt = SPEED_TO_COUNTS(speed);
00076     setpoint_speed_direita_cnt = setpoint_speed_esquerda_cnt;
00077   }
00078   else
00079   {
00080     tipo_movimento = ROTACIONAL;
00081 
00082     /* Calcula as velocidade a partir do raio */
00083     float speedX_mm_s = (float)speed;
00084     float speedW_rad_s = speedX_mm_s / (float)raio;
00085 
00086     int32_t speedX_cnt = SPEED_TO_COUNTS(abs(speed));
00087     int32_t speedW_cnt = RAD_S_TO_COUNTS(speedW_rad_s);
00088 
00089     if (raio < RODA_RODA)
00090     {
00091       speedX_cnt -= (abs(speedW_cnt) / 2);
00092       speedW_cnt /= 2;
00093     }
00094 
00095     /* Usado para o controle de velocidade */
00096     setpoint_speed_esquerda_cnt = speedX_cnt - speedW_cnt;
00097     setpoint_speed_direita_cnt = speedX_cnt + speedW_cnt;
00098   }
00099 
00100   /* Usado para indicar o fim do movimento */
00101   setpoint_dist = dist_ou_angulo;
00102 }
00103 
00104 
00105 /* Atualiza a leitura dos encoders ---------------------------------------------
00106 */
00107 void updateEncoders(void)
00108 {
00109   int32_t encoder_esquerda_cnt, encoder_direita_cnt;
00110 
00111   /* Atualiza a contagem dos encoders */
00112   encoder_esquerda_cnt = getEncoderEsquerda();
00113   encoder_direita_cnt = getEncoderDireita();
00114 
00115   /* Atualiza a alteração dos encoders (speed) */
00116   encoder_esquerda_delta = encoder_esquerda_cnt - encoder_esquerda_anterior_cnt;
00117   encoder_direita_delta = encoder_direita_cnt - encoder_direita_anterior_cnt;
00118 
00119   /* Guarda os valores anteriores */
00120   encoder_esquerda_anterior_cnt = encoder_esquerda_cnt;
00121   encoder_direita_anterior_cnt = encoder_direita_cnt;
00122 
00123   /* Calcula a distância (mm) e o ângulo (deg) */
00124   distancia_mm = COUNTS_TO_MM((encoder_direita_cnt + encoder_esquerda_cnt) / 2);
00125   distancia_deg = COUNTS_TO_DEG((encoder_direita_cnt - encoder_esquerda_cnt) / 2);
00126 }
00127 
00128 
00129 /* Controlador PID de velocidade dos motores -----------------------------------
00130 */
00131 void controladorPID(void)
00132 {
00133   int32_t erro_esquerda = 0, erro_direita = 0;
00134 
00135   /* Alinhamento através dos sensores */
00136   if (bUsarSensores == true)
00137   {
00138     int32_t sensor_feedback = (int32_t)getErroSensores() / K_SENSORES;
00139 
00140     erro_esquerda += sensor_feedback;
00141     erro_direita -= sensor_feedback;
00142   }
00143 
00144   /* Erro de velocidade */
00145   erro_esquerda += (setpoint_speed_esquerda_cnt - encoder_esquerda_delta);
00146   erro_direita += (setpoint_speed_direita_cnt - encoder_direita_delta);
00147 
00148   /* PD - motor da esquerda */
00149   pid_esquerda_out += (KP * erro_esquerda) +
00150   (KD * (erro_esquerda - erro_esquerda_anterior));
00151 
00152   /* PD - motor da direita */
00153   pid_direita_out += (KP * erro_direita) +
00154   (KD * (erro_direita - erro_direita_anterior));
00155 
00156   /* Guarda os valores anteriores */
00157   erro_esquerda_anterior = erro_esquerda;
00158   erro_direita_anterior = erro_direita;
00159 
00160   setMotores(pid_esquerda_out / 1000.0f, pid_direita_out / 1000.0f);
00161 }
00162 
00163 
00164 /* Indica o fim do movimento ---------------------------------------------------
00165 */
00166 bool isFinalMovimento(void)
00167 {
00168   if (tipo_movimento == TRANSLACIONAL && distancia_mm >= setpoint_dist)
00169   {
00170     return true;
00171   }
00172   else if (tipo_movimento == ROTACIONAL && abs(distancia_deg) >= setpoint_dist)
00173   {
00174     return true;
00175   }
00176   else if ((tipo_movimento == TRANSLACIONAL) && (frontal_esquerdo > CENTRO_FRONTAL && frontal_direito > CENTRO_FRONTAL))
00177   {
00178     return true;
00179   }
00180 
00181   return false;
00182 }
00183 
00184 
00185 /* Reseta os encoders e as variáveis do controlador ----------------------------
00186 */
00187 void resetControlador(void)
00188 {
00189   resetEncoderEsquerda();
00190   resetEncoderDireita();
00191 
00192   encoder_esquerda_anterior_cnt = 0;
00193   encoder_direita_anterior_cnt = 0;
00194 
00195   distancia_mm = 0;
00196   distancia_deg = 0;
00197 
00198   pid_esquerda_out = 0; pid_direita_out = 0;
00199   erro_esquerda_anterior = 0; erro_direita_anterior = 0;
00200   setpoint_speed_esquerda_cnt = 0; setpoint_speed_direita_cnt = 0;
00201 }
00202 
00203 
00204 /* Movimenta o micromouse para frente uma distância em [mm] --------------------
00205 * Se mm == 0: anda para frente até o próximo comando
00206 */
00207 void frente(int32_t mm)
00208 {
00209   if (mm != 0)
00210   {
00211     bUsarSensores = true;
00212     resetControlador();
00213     setMovimento(0, mm, SPEED_RETA);
00214 
00215     do
00216     {
00217       wait_ms(1);
00218     }
00219     while (isFinalMovimento() == false);
00220   }
00221   else
00222   {
00223     bUsarSensores = true;
00224     setMovimento(0, mm, SPEED_RETA);
00225     wait_ms(1);
00226   }
00227 }
00228 
00229 
00230 /* Realiza uma curva em torno do próprio eixo (graus < 0: para direita) --------
00231 */
00232 void curvaPivot(int16_t graus)
00233 {
00234   bUsarSensores = false;
00235   resetControlador();
00236   setMotores(0, 0);
00237   wait_ms(100);
00238 
00239   /* Seleciona o movimento de acordo com o sentido da curva */
00240   if (graus > 0) setMovimento(RODA_RODA / 2, graus, SPEED_CURVA);
00241   else setMovimento(RODA_RODA / 2, -graus, -SPEED_CURVA);
00242 
00243   do
00244   {
00245     wait_ms(1);
00246   }
00247   while (isFinalMovimento() == false);
00248 }
00249 
00250 
00251 /* Realiza uma curva em torno do próprio eixo e anda até a fronteira da célula
00252 *       (graus < 0: para direita)
00253 */
00254 void curva(int16_t graus)
00255 {
00256   frente(CELULA / 2);
00257   curvaPivot(graus);
00258   frente(CELULA / 2);
00259 }
00260 
00261 /* Retorna o erro de alinhamento dos sensores ----------------------------------
00262 */
00263 int16_t getErroSensores(void)
00264 {
00265   float erro = 0;
00266 
00267   /* ------ ALINHAMENTO LATERAL ------ */
00268   if (frontal_esquerdo < FRONTAL_TH && frontal_direito < FRONTAL_TH)
00269   {
00270 #if PID_TECNICA == 1
00271     if (lateral_esquerdo > CENTRO_LATERAL && lateral_direito < CENTRO_LATERAL)
00272     {
00273       erro = lateral_esquerdo - CENTRO_LATERAL;
00274     }
00275     else if (lateral_direito > CENTRO_LATERAL && lateral_esquerdo < CENTRO_LATERAL)
00276     {
00277       erro = CENTRO_LATERAL - lateral_direito;
00278     }
00279 #elif PID_TECNICA == 2
00280     if (lateral_esquerdo > LATERAL_TH && lateral_direito > LATERAL_TH)
00281     {
00282       erro = lateral_esquerdo - lateral_direito;
00283     }
00284     else if (lateral_esquerdo > LATERAL_TH)
00285     {
00286       erro = 2 * (lateral_esquerdo - CENTRO_LATERAL);
00287     }
00288     else if (lateral_direito > LATERAL_TH)
00289     {
00290       erro = 2 * (CENTRO_LATERAL - lateral_direito);
00291     }
00292 #endif
00293   }
00294   /* ------ ALINHAMENTO FRONTAL ------ */
00295   else if (frontal_esquerdo > ALINHAMENTO_FRONTAL && frontal_direito > ALINHAMENTO_FRONTAL)
00296   {
00297     erro = (frontal_direito - frontal_esquerdo);
00298   }
00299 
00300   return ((int16_t)(erro * 4095));//return erro;
00301 }
00302 
00303 /**
00304 * @}
00305 */
00306 
00307 
00308 /************************ (C) COPYRIGHT IOTON Technology **********************/
00309 /***********************************END OF FILE********************************/