Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
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 */
Generated on Wed Jul 13 2022 17:59:38 by
