main.cpp

00001 /* #############################################################################
00002 **    Archivo        : main.c
00003 **    Proyecto       : STM32F103C8_Servos
00004 **    Procesador     : STM32F103C8T6
00005 **    Plataforma     : Blue Pill
00006 **    Herramienta    : Mbed Compiler
00007 **    Compilador     : Mbed Online C Compiler
00008 **    Version        : 5.14
00009 **    Fecha/Hora     : 20-12-2020, 12:20, # CodeGen: 0
00010 **    Descripción    :
00011 **         Este proyecto realiza el control de dos servomotores tipo Futaba.
00012 **         This module contains user's application code.
00013 **   Componentes     : ADC, GPIO, Timer, etc .
00014 **   Configuraciones : Includes, Stacks y Drivers externos
00015 **   Autores         :
00016 **         ATEAM Development Group:
00017 **          - Antulio Morgado Valle
00018 **
00019 **   Versión        : Beta
00020 **   Revisión       : A
00021 **   Release        : 0
00022 **   Bugs & Fixes   :
00023 **   Date           : 20/10/2019
00024 **                    Added support for Led_RGB
00025 **                    22/09/2018 
00026 **                    Added LCD Menu, Beta version (with bugs)
00027 **
00028 ** ###########################################################################*/
00029 /*
00030 ::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
00031 :  Includes
00032 ::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
00033 */
00034 #include "stm32f103c8t6.h"
00035 #include "mbed.h"
00036 #include "AnalogIn.h"
00037 //#include "Led_RGB.h"
00038 #include "Serial.h"
00039 //#include "stdio.h"
00040 
00041 /*
00042 :...............................................................................
00043 :  Definiciones
00044 :...............................................................................
00045 */
00046 #define  on             0               // Estado para boton presionado
00047 #define off             1               // Estado para boton sin presionar
00048 #define hold            2               // Estado para boton mantenido
00049 #define release         3               // Estado para boton liberado
00050 #define FALSE           0               // Estado FALSO
00051 #define TRUE            1               // Estado VERDADERO
00052 #define Ticker_Rate     1000            // Periodo de interrupción (us)
00053 #define Baud_Rate       115200          // Velocidad de Transmisión (Bauds)
00054                                         // Velocidades Permitidas:
00055                                         // 300, 600, 1200, 2400, 4800, 9600,
00056                                         // 14400, 19600, 28800, 38400, 57600
00057                                         // 115200, 230400
00058 /*
00059 +-------------------------------------------------------------------------------
00060 |  Configuración de Puertos 
00061 +-------------------------------------------------------------------------------
00062 */
00063 //confSysClock();     // Configure system clock (72MHz HSE clock, 48MHz USB clock)
00064 Ticker      TimerInt;               // Inicializa la Interrupción por Timer
00065 DigitalOut  led_monitor(LED1);      // Inicializa el LED Monitor (Interno)
00066 Serial      terminal(PA_2, PA_3);   // Inicializa la Comunicación Serial a la PC
00067 PwmOut      servo_1(PB_0);          // Inicializa el PWM para el Servo 1
00068 PwmOut      servo_2(PB_1);          // Inicializa el PWM para el Servo 2
00069 AnalogIn    pot_1 (PA_0);           // Inicializa Canal Analógico para Potenciometro 1
00070 AnalogIn    pot_2 (PA_1);           // Inicializa Canal Analógico para Potenciometro 2
00071 /*
00072 +-------------------------------------------------------------------------------
00073 |  Variables Globales de Usuario 
00074 +-------------------------------------------------------------------------------
00075 */
00076 uint16_t Rate=Ticker_Rate/2;        // Velocidad de barrido (500us = 0.5ms)
00077 uint16_t X_Pos;                     // Posición para X 
00078 uint16_t Y_Pos;                     // Posición para Y 
00079 float    Canal_8;           /* Variable para el valor del Canal del ADC_8 */
00080 float    Canal_9;           /* Variable para el valor del Canal del ADC_9 */
00081 float    Duty1 = 0.05;             /* Variable para el Duty Cycle del Servo_1_1 */
00082 float    Duty2 = 0.05;             /* Variable para el Duty Cycle del Servo_1_2 */
00083 float    Duty3;             /* Variable para el Duty Cycle del PWM_1 */
00084 float    Duty4;             /* Variable para el Duty Cycle del PWM_2 */
00085 uint16_t counter=250;               // Cuenta inicial de 250us
00086 bool     flag1;                     // Semáforo 1
00087 /* END variables */
00088 
00089 /*
00090 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00091 |  Definición de Funciones Prototipo y Rutinas de los Vectores de Interrupción
00092 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00093 */
00094         // Las Definiciones de Funciones Prototipo 
00095         // y Apuntadores a Vectores de Interrupciones van aquí !
00096 void TimerInt_OnInterrupt(void);  
00097 void Servo_Move(void);
00098 /* END prototypes */
00099 
00100 /* END definitions */  
00101 /*
00102 #===============================================================================
00103 |
00104 |               P R O G R A M A     P R I N C I P A L
00105 |
00106 #=============================================================================== 
00107 */
00108 int main()          
00109 {                           // Inicialización de variables, puertos e interrupciones
00110 confSysClock();             // Inicialización del Sistema, Configure system clock (72MHz HSE clock, 48MHz USB clock)
00111 TimerInt.attach_us(&TimerInt_OnInterrupt, Ticker_Rate);   // Le asigna el periodo de interrupción de 1ms (Ticker_Rate=1000)
00112 
00113 terminal.baud(Baud_Rate);   // Se configura la velocidad de transmisión e inicia la comunicación serial.    
00114 terminal.printf(" System is Wake Up!.\n\r");    // e inicia la  comunicación serial
00115 terminal.printf ("\t\e[1;37;41mInstituto Politecnico Nacional\e[0m\n\r");
00116 terminal.printf ("\e[1;37;42mESCUELA SUPERIOR DE INGENIERIA MECANICA Y ELECTRICA\e[0m\n\r\v");
00117 terminal.printf ("\t\e[1;34;40mManejo \e[1;37;40mde \e[1;32;40mlos \e[1;33;40mServo \e[1;31;40mMotores\e[0m\n\r\v\v\e[0m");
00118 
00119     while (true)                // El Lazo del Programa principal está aquí !!!
00120     {
00121         if (flag1)  
00122         {
00123             Servo_Move();   // Llama a la rutina para mover los servo motores.
00124             terminal.printf("\t\e[1;33;41mPosicion X: %3d\x0df\t\e[1;37;41mPosicion Y: %3d\x0df\033[0m\r", X_Pos, Y_Pos); 
00125             flag1 = false; 
00126         }
00127     }
00128 }
00129 /* END main */
00130 
00131 /*
00132 ................................................................................
00133 :  Rutinas de los Vectores de Interrupción
00134 ................................................................................
00135 */
00136                 // Las Rutinas de Atención a Interrupciones van aquí !
00137 void TimerInt_OnInterrupt()          // Rutina de Atención al Ticker
00138 {
00139     counter--;                      // Aquí va la Rutina de Servicio !
00140     if (!counter)
00141     {
00142         led_monitor = !led_monitor; // Parapadeo del LED por Interrupción (LED Toggled)
00143         flag1 = true;               // Permite actualizar los servos
00144         counter = Rate;             // Restablece el contador, Le asigna el periodo de 0.5ms (Rate=500)
00145     } 
00146 }
00147 /* END Events */   
00148 /*
00149 ________________________________________________________________________________
00150 |
00151 |  Funciones Prototipo
00152 |_______________________________________________________________________________
00153 */
00154                 // Las Funciones Prototipo van aquí !
00155 void Servo_Move()             // Routine for Servo move 
00156 {
00157         // Read Pot value and calculate position. 
00158         // So to turn the Servo move to right, left or stay
00159     Duty1 = pot_1.read() * 0.1;
00160     Duty2 = pot_2.read() * 0.1;    
00161     servo_1 = Duty1;          // moves Servo 1 to new position
00162     servo_2 = Duty2;          // moves Servo 2 to new position
00163     X_Pos = Duty1 * 1850;       // calculates X position on degrees
00164     Y_Pos = Duty2 * 1850;       // calculates Y position on degrees
00165 }
00166 /* END functions */    
00167                 
00168 /* END program */
00169 
00170 /*+-----------------------------------------------------------------------------       
00171 *+ La documentación va aquí!:
00172 *+-----------------------------------------------------------------------------       
00173 *+  Conexión al Potenciometro:
00174 *+ 
00175 *+  Como recomendación hay que poner un filtro pasa bajas para evitar que el 
00176 *+  ruido al convertidor analógico/digital.
00177 *+  
00178 *+      3.3V
00179 *+       O                
00180 *+       |
00181 *+       |
00182 *+       |          Conexión de los Potenciometros
00183 *+       |          
00184 *+       /
00185 *+       \  10K 
00186 *+       /          100
00187 *+       \  <----/\/\/\/\--------------  PIN (Analog In)
00188 *+       \                  |
00189 *+       /                  |
00190 *+       \                -----   0.1uF
00191 *+       |                -----
00192 *+       |                  |
00193 *+       |                  |
00194 *+       |                  |
00195 *+    ___|___            ___|___
00196 *+     _____              _____
00197 *+      ___                ___
00198 *+       _                  _
00199 *+
00200 *+      GND                GND
00201 *+
00202 *+----------------------------------------------------------------------------*
00203 *+  Conexión al Servomotor:
00204 *+ 
00205 *+  Como recomendación hay que instalar un driver para acoplar los niveles de 
00206 *+  de voltaje y excitar a los servo motores.
00207 *+  
00208 *+      Utilizando el CD4041
00209 *+
00210 *+              5V
00211 *+               o
00212 *+               | 14
00213 *+               |
00214 *+  3.3V         |         5V
00215 *+             |\|   1     _______
00216 *+             | \-------  Servo_1
00217 *+          3  |  \
00218 *+ PA_6   -----|   >
00219 *+             |  /  2
00220 *+             | /o------  Servo_1
00221 *+             |/
00222 *+
00223 *+
00224 *+             |\    11    _______
00225 *+             | \-------  Servo_2
00226 *+          13 |  \
00227 *+ PA_7   -----|   >
00228 *+             |  /  12
00229 *+             | /o------  Servo_2
00230 *+             |/|
00231 *+               |
00232 *+               |
00233 *+               | 7
00234 *+            ___|___
00235 *+             _____
00236 *+              ___
00237 *+               _
00238 *+
00239 *+              GND                
00240 *+
00241 *+  
00242 *+      Utilizando el SN74HC04,  SN74HC14 o CD4069
00243 *+         
00244 *+  
00245 *+              5V
00246 *+               o
00247 *+               | 14
00248 *+               |
00249 *+  3.3V         |                            5V
00250 *+             |\|            |\    
00251 *+             | \            | \
00252 *+          1  |  \   2    3  |  \  4
00253 *+ PA_6   -----|   >o---------|   >o--------  Servo 1 (Signal In)
00254 *+             |  /           |  /  
00255 *+             | /            | /
00256 *+             |/             |/
00257 *+
00258 *+
00259 *+             |\             |\    
00260 *+             | \            | \
00261 *+          13 |  \  12    11 |  \  10
00262 *+ PA_7   -----|   >o---------|   >o--------  Servo 2 (Signal In)
00263 *+             |  /           |  /  
00264 *+             | /            | /
00265 *+             |/|            |/
00266 *+               |
00267 *+               |
00268 *+               | 7
00269 *+            ___|___
00270 *+             _____
00271 *+              ___
00272 *+               _
00273 *+
00274 *+              GND    
00275 *+
00276 *+
00277 *+----------------------------------------------------------------------------*
00278 *+
00279 *+      CARACTERISTCAS DEL SERVO MOTOR FUTABA
00280 *+
00281 **  El servomotor espera ver un pulso cada 20 milisegundos (ms) y la longitud 
00282 *+  del pulso determinará qué tan lejos gira el motor. Por ejemplo, un pulso de 
00283 *+  1.5 ms hará que el motor gire a la posición de 90°. Más corto que 1.5ms lo 
00284 *+  mueve en sentido contrario a las agujas del reloj hacia la posición de 0°, 
00285 *+  y más de 1.5ms girará el servo en sentido horario hacia la posición de 180°.
00286 *+
00287 *+        Pin Out Conector Futaba
00288 *+
00289 *+   Negative  ---------------  Black     
00290 *+   Positive  ---------------  Red
00291 *+   Signal In ---------------  White, Yellow or any other color
00292 *+
00293 *+   
00294 *+
00295 *+----------------------------------------------------------------------------*
00296 *+      ALGORITMO
00297 *+
00298 *+ El control del Servo Motor se hace mediante un PWM modificado, es decir
00299 *+ Periodo:                     20 ms
00300 *+ Desplazamiento Mínimo:       1 ms    (0°)    5% Modulación
00301 *+ Desplazamiento al Centro:    1.5 ms  (90°)   13.3% Modulación
00302 *+ Desplazamiento Máximo        2 ms    (180°)  10.0% Modulación
00303 *+  
00304 *+ Desplazamiento = ADC_Value / 20 + 0.05  (0.05 + 0.1)
00305 *+
00306 *+----------------------------------------------------------------------------*
00307 *+
00308 *+   Powered by
00309 *+
00310 *+          T H E     A N T U L I U ' S   T E A M   R&D  Unltd 
00311 *+
00312 *+  Research, Development, Systems, Equipment, Support & Spare Parts.
00313 *+  (C) Copyright 1989-2019, All Rights Reserved            
00314 *+
00315 *+  Welcome to The Beaners Proyect Community!                   
00316 *+----------------------------------------------------------------------------*/
00317 
00318 /* END documentation */ 
00319 
00320 /* END Mbed */