Ricardo Soto / Mbed 2 deprecated IMTarmProgramable

IMTcchallenge para prepos

Dependencies:   Servo mbed

main.cpp@1:d47f3431441b, 2015-10-16 (annotated)

Committer:
Soto
Date:
Fri Oct 16 04:51:42 2015 +0000
Revision:
1:d47f3431441b
Parent:
0:21188b66200a 
version1.2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Soto 0:21188b66200a 1 /* Programa para el control de
Soto 0:21188b66200a 2 un brazon robotico con 5 servos
Soto 0:21188b66200a 3
Soto 0:21188b66200a 4 Carrera Ingenieria Mecatronica
Soto 0:21188b66200a 5 IMT
Soto 0:21188b66200a 6 */
Soto 0:21188b66200a 7
Soto 0:21188b66200a 8 #include "mbed.h"
Soto 0:21188b66200a 9 #include "Servo.h"
Soto 0:21188b66200a 10 #include <vector>
Soto 0:21188b66200a 11
Soto 0:21188b66200a 12 //*****************Servos y sus Puertos*******
Soto 0:21188b66200a 13
Soto 0:21188b66200a 14 Servo Garra(PTC9);
Soto 0:21188b66200a 15 Servo Muneca(PTC8);
Soto 0:21188b66200a 16 Servo Codo(PTA5);
Soto 0:21188b66200a 17 Servo Hombro(PTA4);
Soto 0:21188b66200a 18 Servo Eje(PTA12);
Soto 0:21188b66200a 19
Soto 0:21188b66200a 20 //*********Definicion de Funciones************
Soto 0:21188b66200a 21
Soto 0:21188b66200a 22 void ServosMovingTogether(float, float, float, float, float);
Soto 0:21188b66200a 23
Soto 0:21188b66200a 24 //*********Macros Defines*********************
Soto 1:d47f3431441b 25 #define MIDSTEPDELAY 1 //modificar este valor para el tiempo entre el movimiento de un motor y otro
Soto 1:d47f3431441b 26 #define GENERALLOOPDELAY 1 //Modificar este valor para el tiempo entre una secuencia de movimiento de 5 motores y la secuencia siguiente
Soto 1:d47f3431441b 27 #define LOOPSNUMBER 1 //Modificar este valor segun a la cantidad de veces que quiere repetir la rutina
Soto 0:21188b66200a 28
Soto 0:21188b66200a 29 //Comando para iniciar un comunicacion entre la FRDM-KL-Z y la computador
Soto 0:21188b66200a 30
Soto 0:21188b66200a 31 Serial pc(USBTX, USBRX);
Soto 0:21188b66200a 32 //--------------------------------------------------------------------
Soto 0:21188b66200a 33 //**************Inicio del programa principal*************************
Soto 0:21188b66200a 34 //--------------------------------------------------------------------
Soto 0:21188b66200a 35 int main(){
Soto 0:21188b66200a 36
Soto 1:d47f3431441b 37 char StartBit= '\0';
Soto 1:d47f3431441b 38 int temp= LOOPSNUMBER;
Soto 1:d47f3431441b 39
Soto 0:21188b66200a 40 pc.baud(9600); //Velocidad de la comunicacion entre la tarjeta y la computadora
Soto 0:21188b66200a 41
Soto 0:21188b66200a 42 printf("\nBienvenido\n\n Iniciando la rutina del IMTarm");
Soto 0:21188b66200a 43 while(1){ //Loop infinito
Soto 0:21188b66200a 44
Soto 1:d47f3431441b 45 printf("\nPresione cualquer tecla para empezar\n");
Soto 1:d47f3431441b 46 StartBit=pc.getc();
Soto 1:d47f3431441b 47
Soto 1:d47f3431441b 48 if(StartBit!=0)
Soto 1:d47f3431441b 49 {
Soto 1:d47f3431441b 50 StartBit='\0';
Soto 1:d47f3431441b 51 do
Soto 1:d47f3431441b 52 {
Soto 0:21188b66200a 53 ServosMovingTogether(0.5,0.5,0.5,0.5,0.5);
Soto 1:d47f3431441b 54 //---------------------| | | | | Garra
Soto 1:d47f3431441b 55 //---------------------| | | | Muneca
Soto 1:d47f3431441b 56 //---------------------| | | Codo
Soto 1:d47f3431441b 57 //---------------------| | Hombro
Soto 1:d47f3431441b 58 //---------------------| Eje
Soto 1:d47f3431441b 59
Soto 1:d47f3431441b 60 wait(GENERALLOOPDELAY);
Soto 1:d47f3431441b 61 //*********************************************************
Soto 1:d47f3431441b 62 //******Modificar a partir de este punto*******************
Soto 1:d47f3431441b 63 //*********************************************************
Soto 1:d47f3431441b 64
Soto 1:d47f3431441b 65 ServosMovingTogether(0.125,0.6,0.05,0.975,0.975);
Soto 1:d47f3431441b 66 wait(GENERALLOOPDELAY);
Soto 0:21188b66200a 67
Soto 1:d47f3431441b 68
Soto 1:d47f3431441b 69 ServosMovingTogether(0.125,0.55,0.0,0.975,0.775);
Soto 1:d47f3431441b 70 wait(GENERALLOOPDELAY);
Soto 1:d47f3431441b 71
Soto 1:d47f3431441b 72
Soto 1:d47f3431441b 73 ServosMovingTogether(0.125,0.35,0.40,0.975,0.775);
Soto 1:d47f3431441b 74 wait(GENERALLOOPDELAY);
Soto 1:d47f3431441b 75
Soto 1:d47f3431441b 76
Soto 1:d47f3431441b 77 ServosMovingTogether(0.825,0.35,0.40,0.975,0.975);
Soto 1:d47f3431441b 78 wait(GENERALLOOPDELAY);
Soto 1:d47f3431441b 79 //***********Dejar de Modificar******************
Soto 1:d47f3431441b 80 }while(--temp);
Soto 1:d47f3431441b 81 }
Soto 0:21188b66200a 82 }//Fin del Loop infinito
Soto 0:21188b66200a 83
Soto 0:21188b66200a 84 //+++++++++++++++++++++++++++++++++++++++++++++++
Soto 0:21188b66200a 85 //-----------------------------------------------
Soto 0:21188b66200a 86 }//********FIN DEL PROGRAMA PRINCIPAL*************
Soto 0:21188b66200a 87 //-----------------------------------------------
Soto 0:21188b66200a 88 //+++++++++++++++++++++++++++++++++++++++++++++++
Soto 0:21188b66200a 89
Soto 0:21188b66200a 90
Soto 0:21188b66200a 91 //-----------------------------------------------
Soto 0:21188b66200a 92 //Funcion para mover los 5 servos en secuencia
Soto 0:21188b66200a 93 //-----------------------------------------------
Soto 0:21188b66200a 94 void ServosMovingTogether(float EjePos, float HombroPos, float CodoPos, float MunecaPos, float GarraPos)
Soto 0:21188b66200a 95 {
Soto 0:21188b66200a 96 Eje=EjePos;
Soto 1:d47f3431441b 97 wait (MIDSTEPDELAY);
Soto 0:21188b66200a 98
Soto 1:d47f3431441b 99 Muneca=MunecaPos;
Soto 1:d47f3431441b 100 wait (MIDSTEPDELAY);
Soto 0:21188b66200a 101
Soto 0:21188b66200a 102 Codo=CodoPos;
Soto 1:d47f3431441b 103 wait (MIDSTEPDELAY);
Soto 1:d47f3431441b 104
Soto 1:d47f3431441b 105 Hombro=HombroPos;
Soto 1:d47f3431441b 106 wait (MIDSTEPDELAY);
Soto 0:21188b66200a 107
Soto 0:21188b66200a 108 Garra=GarraPos;
Soto 1:d47f3431441b 109 wait (MIDSTEPDELAY);
Soto 0:21188b66200a 110 }