Piccolo / Mbed 2 deprecated ejemplo_serial_protocol_array

Piccolo

Dependencies:   mbed

Fork of 02_LAB_serial_protocol by ferney alberto beltran molina

main.cpp@9:5008f9501dbf, 2017-09-30 (annotated)

Committer:
manuelitoys
Date:
Sat Sep 30 13:27:13 2017 +0000
Revision:
9:5008f9501dbf
Parent:
8:38ae341e2a4f
Child:
10:3eab080dbc12
Piccolo Funciones cuadrado y linea

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fabeltranm 0:55d11eeb0faf 1 #include "mbed.h"
fabeltranm 0:55d11eeb0faf 2
fabeltranm 0:55d11eeb0faf 3 Serial command(USBTX, USBRX);
fabeltranm 2:200a9507b696 4 DigitalOut led(LED1);
fabeltranm 0:55d11eeb0faf 5 #define DEBUG 1
manuelitoys 8:38ae341e2a4f 6 PwmOut myServoX(PB_3);
manuelitoys 8:38ae341e2a4f 7 PwmOut myServoY(PB_6);
manuelitoys 8:38ae341e2a4f 8 PwmOut myServoZ(PB_4);
fabeltranm 0:55d11eeb0faf 9 //*****************************************************************************
fabeltranm 0:55d11eeb0faf 10 // COMANDO MOVER MOTOR
fabeltranm 0:55d11eeb0faf 11 // |POS 1|POS 2|POS 3|POS 4| POS 5|
fabeltranm 7:fab201aa45b7 12 // | < | #C | a | b | > |
fabeltranm 0:55d11eeb0faf 13 //
fabeltranm 6:cb6b868465c3 14 // #C -> indica el comando
fabeltranm 6:cb6b868465c3 15 // a,b,c,d parametros del comando
fabeltranm 6:cb6b868465c3 16 // <,> -> inicio, y fin de comando
fabeltranm 0:55d11eeb0faf 17 // el inicio de comando no se almacena en el buffer
fabeltranm 0:55d11eeb0faf 18 //*****************************************************************************
fabeltranm 0:55d11eeb0faf 19
fabeltranm 0:55d11eeb0faf 20 // VARIABLES PARA DEFINIR EL COMMANDO
fabeltranm 0:55d11eeb0faf 21 #define BUFF_SIZE 4
fabeltranm 6:cb6b868465c3 22 #define COMM_N 0
fabeltranm 6:cb6b868465c3 23 #define INITPARAMETER 1
manuelitoys 9:5008f9501dbf 24 #define MAXPOS 50
manuelitoys 9:5008f9501dbf 25 #define POSDRAW 10
manuelitoys 9:5008f9501dbf 26 #define SS_TIME 100
fabeltranm 7:fab201aa45b7 27 // COMANDOS
fabeltranm 7:fab201aa45b7 28 #define LED_NC 0
fabeltranm 7:fab201aa45b7 29 #define DOT_NC 1
fabeltranm 7:fab201aa45b7 30 #define LINE_NC 2
manuelitoys 8:38ae341e2a4f 31 #define CUADRADO 3
fabeltranm 7:fab201aa45b7 32 #define CICLE_NC 4
fabeltranm 7:fab201aa45b7 33 #define HOME_NC 5
fabeltranm 7:fab201aa45b7 34
manuelitoys 8:38ae341e2a4f 35 int coord2pulse(float coord)
manuelitoys 8:38ae341e2a4f 36 {
manuelitoys 8:38ae341e2a4f 37 if(0 <= coord <= MAXPOS)
manuelitoys 8:38ae341e2a4f 38 return int(750+coord*1900/50);// u6
manuelitoys 8:38ae341e2a4f 39 return 750;
manuelitoys 8:38ae341e2a4f 40
manuelitoys 8:38ae341e2a4f 41 }
manuelitoys 9:5008f9501dbf 42 void vertex2d(float x, float y){
manuelitoys 9:5008f9501dbf 43
manuelitoys 9:5008f9501dbf 44 int pulseX = coord2pulse(x);
manuelitoys 9:5008f9501dbf 45 int pulseY = coord2pulse(y);
manuelitoys 9:5008f9501dbf 46
manuelitoys 9:5008f9501dbf 47 myServoX.pulsewidth_us(pulseX);
manuelitoys 9:5008f9501dbf 48 myServoY.pulsewidth_us(pulseY);
manuelitoys 9:5008f9501dbf 49
manuelitoys 9:5008f9501dbf 50 }
manuelitoys 8:38ae341e2a4f 51 void draw(){
manuelitoys 8:38ae341e2a4f 52 myServoZ.pulsewidth_us(POSDRAW);
manuelitoys 8:38ae341e2a4f 53 // myServoY.pulsewidth_us(750);
manuelitoys 8:38ae341e2a4f 54 }
manuelitoys 8:38ae341e2a4f 55 void nodraw(){
manuelitoys 8:38ae341e2a4f 56 myServoZ.pulsewidth_us(MAXPOS);
manuelitoys 8:38ae341e2a4f 57 // myServoY.pulsewidth_us(2000);
manuelitoys 8:38ae341e2a4f 58 }
fabeltranm 7:fab201aa45b7 59
fabeltranm 0:55d11eeb0faf 60 uint8_t buffer_command[BUFF_SIZE]={0,0,0,0};
fabeltranm 0:55d11eeb0faf 61
fabeltranm 1:0bcd96e56022 62
fabeltranm 1:0bcd96e56022 63 void print_num(uint8_t val)
fabeltranm 1:0bcd96e56022 64
fabeltranm 1:0bcd96e56022 65 {
fabeltranm 1:0bcd96e56022 66 if (val <10)
fabeltranm 1:0bcd96e56022 67 command.putc(val+0x30);
fabeltranm 1:0bcd96e56022 68 else
fabeltranm 2:200a9507b696 69 command.putc(val-9+0x40);
fabeltranm 1:0bcd96e56022 70
fabeltranm 1:0bcd96e56022 71 }
fabeltranm 1:0bcd96e56022 72 void print_bin2hex (uint8_t val)
fabeltranm 1:0bcd96e56022 73 {
fabeltranm 1:0bcd96e56022 74 command.printf(" 0x");
fabeltranm 1:0bcd96e56022 75 print_num(val>>4);
fabeltranm 1:0bcd96e56022 76 print_num(val&0x0f);
fabeltranm 1:0bcd96e56022 77
fabeltranm 1:0bcd96e56022 78
fabeltranm 1:0bcd96e56022 79 }
fabeltranm 1:0bcd96e56022 80
fabeltranm 0:55d11eeb0faf 81 // TODO : TIMEOUT UART SERIAL
fabeltranm 0:55d11eeb0faf 82 void Read_command()
fabeltranm 0:55d11eeb0faf 83 {
fabeltranm 0:55d11eeb0faf 84 for (uint8_t i=0; i<BUFF_SIZE;i++)
fabeltranm 0:55d11eeb0faf 85 buffer_command[i]=command.getc();
fabeltranm 0:55d11eeb0faf 86
fabeltranm 0:55d11eeb0faf 87 }
fabeltranm 0:55d11eeb0faf 88
fabeltranm 0:55d11eeb0faf 89 void echo_command()
fabeltranm 0:55d11eeb0faf 90 {
fabeltranm 0:55d11eeb0faf 91 for (uint8_t i=0; i<BUFF_SIZE;i++)
fabeltranm 1:0bcd96e56022 92 print_bin2hex(buffer_command[i]);
fabeltranm 0:55d11eeb0faf 93
fabeltranm 0:55d11eeb0faf 94 }
fabeltranm 0:55d11eeb0faf 95
fabeltranm 0:55d11eeb0faf 96
fabeltranm 2:200a9507b696 97 uint8_t check_command()
fabeltranm 0:55d11eeb0faf 98 {
fabeltranm 6:cb6b868465c3 99 if (buffer_command[BUFF_SIZE-1]== '>'){
fabeltranm 0:55d11eeb0faf 100
fabeltranm 2:200a9507b696 101 #if DEBUG
fabeltranm 6:cb6b868465c3 102 command.printf("\nComando:");
fabeltranm 6:cb6b868465c3 103 print_bin2hex(buffer_command[COMM_N]);
fabeltranm 2:200a9507b696 104 command.printf(" -> ");
fabeltranm 2:200a9507b696 105 #endif
fabeltranm 2:200a9507b696 106 return 1;
fabeltranm 2:200a9507b696 107 }
fabeltranm 0:55d11eeb0faf 108 #if DEBUG
fabeltranm 1:0bcd96e56022 109 command.printf("\n ERROR COMANDO -> ");
fabeltranm 0:55d11eeb0faf 110 echo_command();
fabeltranm 0:55d11eeb0faf 111 #endif
fabeltranm 2:200a9507b696 112 return 0;
fabeltranm 0:55d11eeb0faf 113
fabeltranm 0:55d11eeb0faf 114
fabeltranm 0:55d11eeb0faf 115 }
fabeltranm 6:cb6b868465c3 116 void command_led(int tm)
fabeltranm 5:bf9591a365a4 117 {
fabeltranm 5:bf9591a365a4 118 //EJEMPLO DE COMANDO
fabeltranm 5:bf9591a365a4 119 led=1;
fabeltranm 6:cb6b868465c3 120 wait(tm);
fabeltranm 5:bf9591a365a4 121 led=0;
fabeltranm 5:bf9591a365a4 122
fabeltranm 5:bf9591a365a4 123 }
manuelitoys 9:5008f9501dbf 124 void cuadrado(int x, int y, int lado){
manuelitoys 9:5008f9501dbf 125 int a = x+lado;
manuelitoys 9:5008f9501dbf 126 int b = y+lado;
manuelitoys 8:38ae341e2a4f 127
manuelitoys 8:38ae341e2a4f 128 #if DEBUG
manuelitoys 9:5008f9501dbf 129 command.printf("coord X = %f,coord Y = %f, lado = %f \n", x,y,lado);
manuelitoys 8:38ae341e2a4f 130 #endif
manuelitoys 8:38ae341e2a4f 131
manuelitoys 8:38ae341e2a4f 132 vertex2d(x ,y);
manuelitoys 8:38ae341e2a4f 133 draw();
manuelitoys 8:38ae341e2a4f 134 vertex2d(x ,b);
manuelitoys 8:38ae341e2a4f 135 #if DEBUG
manuelitoys 9:5008f9501dbf 136 command.printf("coord X = %f,coord Y = %f, lado = %f \n", x,b,lado);
manuelitoys 8:38ae341e2a4f 137 #endif
manuelitoys 8:38ae341e2a4f 138 vertex2d(a ,b);
manuelitoys 8:38ae341e2a4f 139 #if DEBUG
manuelitoys 9:5008f9501dbf 140 command.printf("coord X = %f,coord Y = %f, lado = %f \n", a,b,lado);
manuelitoys 8:38ae341e2a4f 141 #endif
manuelitoys 8:38ae341e2a4f 142 vertex2d(a ,y);
manuelitoys 8:38ae341e2a4f 143 #if DEBUG
manuelitoys 9:5008f9501dbf 144 command.printf("coord X = %f,coord Y = %f, lado = %f \n", a,y,lado);
manuelitoys 8:38ae341e2a4f 145 #endif
manuelitoys 8:38ae341e2a4f 146 vertex2d(x ,y);
manuelitoys 8:38ae341e2a4f 147 #if DEBUG
manuelitoys 9:5008f9501dbf 148 command.printf("coord X = %f,coord Y = %f, lado = %f \n", x,y,lado);
manuelitoys 8:38ae341e2a4f 149 #endif
manuelitoys 8:38ae341e2a4f 150 wait_ms(SS_TIME);
manuelitoys 8:38ae341e2a4f 151
manuelitoys 8:38ae341e2a4f 152 }
manuelitoys 8:38ae341e2a4f 153 void linea(float x1, float y1, float x2, float y2){
manuelitoys 8:38ae341e2a4f 154 vertex2d(x1 ,y1);
manuelitoys 8:38ae341e2a4f 155 vertex2d(x2 ,y2);
manuelitoys 8:38ae341e2a4f 156 }
fabeltranm 7:fab201aa45b7 157 void command_exe()
fabeltranm 7:fab201aa45b7 158 {
fabeltranm 7:fab201aa45b7 159
fabeltranm 7:fab201aa45b7 160 switch (buffer_command[COMM_N]){
fabeltranm 7:fab201aa45b7 161
fabeltranm 7:fab201aa45b7 162 case (LED_NC): command_led(buffer_command[INITPARAMETER]);
fabeltranm 7:fab201aa45b7 163 break;
fabeltranm 7:fab201aa45b7 164 case (DOT_NC):
fabeltranm 7:fab201aa45b7 165 #if DEBUG
fabeltranm 7:fab201aa45b7 166 command.printf("draw dot\n");
fabeltranm 7:fab201aa45b7 167 #endif
fabeltranm 7:fab201aa45b7 168
fabeltranm 7:fab201aa45b7 169 break;
fabeltranm 7:fab201aa45b7 170
fabeltranm 7:fab201aa45b7 171 case LINE_NC:
fabeltranm 7:fab201aa45b7 172 #if DEBUG
fabeltranm 7:fab201aa45b7 173 command.printf("draw line\n");
fabeltranm 7:fab201aa45b7 174 #endif
fabeltranm 7:fab201aa45b7 175
fabeltranm 7:fab201aa45b7 176
fabeltranm 7:fab201aa45b7 177 break;
fabeltranm 7:fab201aa45b7 178
manuelitoys 9:5008f9501dbf 179 case (CUADRADO):
manuelitoys 9:5008f9501dbf 180 #if DEBUG
manuelitoys 9:5008f9501dbf 181 command.printf("cuadrado \n");
manuelitoys 9:5008f9501dbf 182 #endif
manuelitoys 8:38ae341e2a4f 183 cuadrado(buffer_command[INITPARAMETER]);
manuelitoys 8:38ae341e2a4f 184
fabeltranm 7:fab201aa45b7 185
fabeltranm 7:fab201aa45b7 186
fabeltranm 7:fab201aa45b7 187 break;
fabeltranm 7:fab201aa45b7 188
fabeltranm 7:fab201aa45b7 189 default:
fabeltranm 7:fab201aa45b7 190
fabeltranm 7:fab201aa45b7 191 #if DEBUG
fabeltranm 7:fab201aa45b7 192 command.printf("comando no encontrado\n");
fabeltranm 7:fab201aa45b7 193 #endif
fabeltranm 7:fab201aa45b7 194
fabeltranm 7:fab201aa45b7 195
fabeltranm 7:fab201aa45b7 196 }
fabeltranm 7:fab201aa45b7 197 }
fabeltranm 7:fab201aa45b7 198
fabeltranm 7:fab201aa45b7 199
fabeltranm 7:fab201aa45b7 200
fabeltranm 0:55d11eeb0faf 201 int main() {
fabeltranm 0:55d11eeb0faf 202 #if DEBUG
fabeltranm 0:55d11eeb0faf 203 command.printf("inicio con debug\n");
fabeltranm 0:55d11eeb0faf 204 #else
fabeltranm 0:55d11eeb0faf 205 command.printf("inicio sin debug\n");
fabeltranm 0:55d11eeb0faf 206 #endif
fabeltranm 0:55d11eeb0faf 207 uint8_t val;
fabeltranm 0:55d11eeb0faf 208 while(1){
fabeltranm 0:55d11eeb0faf 209 val=command.getc();
fabeltranm 0:55d11eeb0faf 210 if (val== '<'){
fabeltranm 0:55d11eeb0faf 211 Read_command();
fabeltranm 4:bcc2d1bebb95 212 if (check_command()){
fabeltranm 7:fab201aa45b7 213 command_exe();
fabeltranm 6:cb6b868465c3 214 #if DEBUG
fabeltranm 6:cb6b868465c3 215 echo_command();
fabeltranm 6:cb6b868465c3 216 #endif
fabeltranm 6:cb6b868465c3 217 }
fabeltranm 3:7c26d338e372 218 }
fabeltranm 3:7c26d338e372 219
fabeltranm 0:55d11eeb0faf 220 }
fabeltranm 0:55d11eeb0faf 221 }
fabeltranm 0:55d11eeb0faf 222