Piccolo / Mbed 2 deprecated ejemplo_serial_protocol_array

Piccolo

Dependencies:   mbed

Fork of 02_LAB_serial_protocol by ferney alberto beltran molina

main.cpp@10:3eab080dbc12, 2017-10-07 (annotated)

Committer:
Vanessa620
Date:
Sat Oct 07 02:50:21 2017 +0000
Revision:
10:3eab080dbc12
Parent:
9:5008f9501dbf
Child:
11:b4a3db5a0305
Cambios de codigo funciones

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
Vanessa620 10:3eab080dbc12 21 #define BUFF_SIZE 6
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
Vanessa620 10:3eab080dbc12 27 #define PI 3.1415926
fabeltranm 7:fab201aa45b7 28 // COMANDOS
fabeltranm 7:fab201aa45b7 29 #define LED_NC 0
fabeltranm 7:fab201aa45b7 30 #define DOT_NC 1
fabeltranm 7:fab201aa45b7 31 #define LINE_NC 2
manuelitoys 8:38ae341e2a4f 32 #define CUADRADO 3
fabeltranm 7:fab201aa45b7 33 #define CICLE_NC 4
Vanessa620 10:3eab080dbc12 34 #define HOME 5
fabeltranm 7:fab201aa45b7 35
manuelitoys 8:38ae341e2a4f 36 int coord2pulse(float coord)
manuelitoys 8:38ae341e2a4f 37 {
manuelitoys 8:38ae341e2a4f 38 if(0 <= coord <= MAXPOS)
manuelitoys 8:38ae341e2a4f 39 return int(750+coord*1900/50);// u6
manuelitoys 8:38ae341e2a4f 40 return 750;
manuelitoys 8:38ae341e2a4f 41
manuelitoys 8:38ae341e2a4f 42 }
manuelitoys 9:5008f9501dbf 43 void vertex2d(float x, float y){
manuelitoys 9:5008f9501dbf 44
manuelitoys 9:5008f9501dbf 45 int pulseX = coord2pulse(x);
manuelitoys 9:5008f9501dbf 46 int pulseY = coord2pulse(y);
manuelitoys 9:5008f9501dbf 47
manuelitoys 9:5008f9501dbf 48 myServoX.pulsewidth_us(pulseX);
manuelitoys 9:5008f9501dbf 49 myServoY.pulsewidth_us(pulseY);
manuelitoys 9:5008f9501dbf 50
manuelitoys 9:5008f9501dbf 51 }
manuelitoys 8:38ae341e2a4f 52 void draw(){
manuelitoys 8:38ae341e2a4f 53 myServoZ.pulsewidth_us(POSDRAW);
manuelitoys 8:38ae341e2a4f 54 // myServoY.pulsewidth_us(750);
manuelitoys 8:38ae341e2a4f 55 }
manuelitoys 8:38ae341e2a4f 56 void nodraw(){
manuelitoys 8:38ae341e2a4f 57 myServoZ.pulsewidth_us(MAXPOS);
manuelitoys 8:38ae341e2a4f 58 // myServoY.pulsewidth_us(2000);
manuelitoys 8:38ae341e2a4f 59 }
fabeltranm 7:fab201aa45b7 60
Vanessa620 10:3eab080dbc12 61 uint8_t buffer_command[BUFF_SIZE]={0,0,0,0,0};
fabeltranm 0:55d11eeb0faf 62
fabeltranm 1:0bcd96e56022 63
fabeltranm 1:0bcd96e56022 64 void print_num(uint8_t val)
fabeltranm 1:0bcd96e56022 65
fabeltranm 1:0bcd96e56022 66 {
fabeltranm 1:0bcd96e56022 67 if (val <10)
fabeltranm 1:0bcd96e56022 68 command.putc(val+0x30);
fabeltranm 1:0bcd96e56022 69 else
fabeltranm 2:200a9507b696 70 command.putc(val-9+0x40);
fabeltranm 1:0bcd96e56022 71
fabeltranm 1:0bcd96e56022 72 }
fabeltranm 1:0bcd96e56022 73 void print_bin2hex (uint8_t val)
fabeltranm 1:0bcd96e56022 74 {
fabeltranm 1:0bcd96e56022 75 command.printf(" 0x");
fabeltranm 1:0bcd96e56022 76 print_num(val>>4);
fabeltranm 1:0bcd96e56022 77 print_num(val&0x0f);
fabeltranm 1:0bcd96e56022 78
fabeltranm 1:0bcd96e56022 79
fabeltranm 1:0bcd96e56022 80 }
fabeltranm 1:0bcd96e56022 81
fabeltranm 0:55d11eeb0faf 82 // TODO : TIMEOUT UART SERIAL
fabeltranm 0:55d11eeb0faf 83 void Read_command()
fabeltranm 0:55d11eeb0faf 84 {
fabeltranm 0:55d11eeb0faf 85 for (uint8_t i=0; i<BUFF_SIZE;i++)
fabeltranm 0:55d11eeb0faf 86 buffer_command[i]=command.getc();
fabeltranm 0:55d11eeb0faf 87
fabeltranm 0:55d11eeb0faf 88 }
fabeltranm 0:55d11eeb0faf 89
fabeltranm 0:55d11eeb0faf 90 void echo_command()
fabeltranm 0:55d11eeb0faf 91 {
fabeltranm 0:55d11eeb0faf 92 for (uint8_t i=0; i<BUFF_SIZE;i++)
fabeltranm 1:0bcd96e56022 93 print_bin2hex(buffer_command[i]);
fabeltranm 0:55d11eeb0faf 94
fabeltranm 0:55d11eeb0faf 95 }
fabeltranm 0:55d11eeb0faf 96
fabeltranm 0:55d11eeb0faf 97
fabeltranm 2:200a9507b696 98 uint8_t check_command()
fabeltranm 0:55d11eeb0faf 99 {
fabeltranm 6:cb6b868465c3 100 if (buffer_command[BUFF_SIZE-1]== '>'){
fabeltranm 0:55d11eeb0faf 101
fabeltranm 2:200a9507b696 102 #if DEBUG
fabeltranm 6:cb6b868465c3 103 command.printf("\nComando:");
fabeltranm 6:cb6b868465c3 104 print_bin2hex(buffer_command[COMM_N]);
fabeltranm 2:200a9507b696 105 command.printf(" -> ");
fabeltranm 2:200a9507b696 106 #endif
fabeltranm 2:200a9507b696 107 return 1;
fabeltranm 2:200a9507b696 108 }
fabeltranm 0:55d11eeb0faf 109 #if DEBUG
fabeltranm 1:0bcd96e56022 110 command.printf("\n ERROR COMANDO -> ");
fabeltranm 0:55d11eeb0faf 111 echo_command();
fabeltranm 0:55d11eeb0faf 112 #endif
fabeltranm 2:200a9507b696 113 return 0;
fabeltranm 0:55d11eeb0faf 114
fabeltranm 0:55d11eeb0faf 115
fabeltranm 0:55d11eeb0faf 116 }
fabeltranm 6:cb6b868465c3 117 void command_led(int tm)
fabeltranm 5:bf9591a365a4 118 {
fabeltranm 5:bf9591a365a4 119 //EJEMPLO DE COMANDO
fabeltranm 5:bf9591a365a4 120 led=1;
fabeltranm 6:cb6b868465c3 121 wait(tm);
fabeltranm 5:bf9591a365a4 122 led=0;
fabeltranm 5:bf9591a365a4 123
fabeltranm 5:bf9591a365a4 124 }
Vanessa620 10:3eab080dbc12 125 void cuadrado(uint8_t x, uint8_t y, uint8_t lado){
manuelitoys 9:5008f9501dbf 126 int a = x+lado;
manuelitoys 9:5008f9501dbf 127 int b = y+lado;
manuelitoys 8:38ae341e2a4f 128
manuelitoys 8:38ae341e2a4f 129 #if DEBUG
Vanessa620 10:3eab080dbc12 130 command.printf("coord X = %i,coord Y = %i, lado = %i \n", x,y,lado);
Vanessa620 10:3eab080dbc12 131
manuelitoys 8:38ae341e2a4f 132 #endif
manuelitoys 8:38ae341e2a4f 133
manuelitoys 8:38ae341e2a4f 134 vertex2d(x ,y);
manuelitoys 8:38ae341e2a4f 135 draw();
manuelitoys 8:38ae341e2a4f 136 vertex2d(x ,b);
manuelitoys 8:38ae341e2a4f 137 #if DEBUG
Vanessa620 10:3eab080dbc12 138 command.printf("coord X = %i,coord Y = %i, lado = %i \n", x,b,lado);
manuelitoys 8:38ae341e2a4f 139 #endif
manuelitoys 8:38ae341e2a4f 140 vertex2d(a ,b);
manuelitoys 8:38ae341e2a4f 141 #if DEBUG
Vanessa620 10:3eab080dbc12 142 command.printf("coord X = %i,coord Y = %i, lado = %i \n", a,b,lado);
manuelitoys 8:38ae341e2a4f 143 #endif
manuelitoys 8:38ae341e2a4f 144 vertex2d(a ,y);
manuelitoys 8:38ae341e2a4f 145 #if DEBUG
Vanessa620 10:3eab080dbc12 146 command.printf("coord X = %i,coord Y = %i, lado = %i \n", a,y,lado);
manuelitoys 8:38ae341e2a4f 147 #endif
manuelitoys 8:38ae341e2a4f 148 vertex2d(x ,y);
manuelitoys 8:38ae341e2a4f 149 #if DEBUG
Vanessa620 10:3eab080dbc12 150 command.printf("coord X = %i,coord Y = %i, lado = %i \n", x,y,lado);
manuelitoys 8:38ae341e2a4f 151 #endif
manuelitoys 8:38ae341e2a4f 152 wait_ms(SS_TIME);
manuelitoys 8:38ae341e2a4f 153
manuelitoys 8:38ae341e2a4f 154 }
Vanessa620 10:3eab080dbc12 155 void linea(uint8_t x1, uint8_t y1, uint8_t x2, uint8_t y2){
Vanessa620 10:3eab080dbc12 156
manuelitoys 8:38ae341e2a4f 157 vertex2d(x1 ,y1);
manuelitoys 8:38ae341e2a4f 158 vertex2d(x2 ,y2);
Vanessa620 10:3eab080dbc12 159 #if DEBUG
Vanessa620 10:3eab080dbc12 160 command.printf("coord X1 = %i,coord Y1 = %i, coord X2 = %i , coord Y2 = %i \n", x1,y1,x2,y2);
Vanessa620 10:3eab080dbc12 161 #endif
Vanessa620 10:3eab080dbc12 162
manuelitoys 8:38ae341e2a4f 163 }
Vanessa620 10:3eab080dbc12 164 void home(){
Vanessa620 10:3eab080dbc12 165 vertex2d(0,0);
Vanessa620 10:3eab080dbc12 166 }
Vanessa620 10:3eab080dbc12 167 void punto(uint8_t x, uint8_t y){
Vanessa620 10:3eab080dbc12 168 vertex2d(x,y);
Vanessa620 10:3eab080dbc12 169 #if DEBUG
Vanessa620 10:3eab080dbc12 170 command.printf("Coord x= %i, coord y=%i \n",x,y);
Vanessa620 10:3eab080dbc12 171 #endif
Vanessa620 10:3eab080dbc12 172 }
Vanessa620 10:3eab080dbc12 173 void circle(uint8_t cx, uint8_t cy, uint8_t radio)
Vanessa620 10:3eab080dbc12 174 {
Vanessa620 10:3eab080dbc12 175 int y;
Vanessa620 10:3eab080dbc12 176 int x;
Vanessa620 10:3eab080dbc12 177 wait(2); vertex2d(cx,cy);
Vanessa620 10:3eab080dbc12 178
Vanessa620 10:3eab080dbc12 179
Vanessa620 10:3eab080dbc12 180 for(double i=PI/2; i<=PI ;i=i+0.5)
Vanessa620 10:3eab080dbc12 181 {
Vanessa620 10:3eab080dbc12 182 x=radio*cos(i);
Vanessa620 10:3eab080dbc12 183 y=radio*sin(i);
Vanessa620 10:3eab080dbc12 184 vertex2d(cx-x,cy-y);
Vanessa620 10:3eab080dbc12 185 printf("position x =%.2f\n",x);
Vanessa620 10:3eab080dbc12 186 printf("position y =%.2f\n",y);
Vanessa620 10:3eab080dbc12 187 }
Vanessa620 10:3eab080dbc12 188 wait(1);
Vanessa620 10:3eab080dbc12 189 for(double i=0; i<=PI/2 ;i+=0.5)
Vanessa620 10:3eab080dbc12 190 {
Vanessa620 10:3eab080dbc12 191 x=radio*cos(i);
Vanessa620 10:3eab080dbc12 192 y=radio*sin(i);
Vanessa620 10:3eab080dbc12 193 vertex2d(x+cx,y+cy);
Vanessa620 10:3eab080dbc12 194 printf("position x =%.2f\n",x);
Vanessa620 10:3eab080dbc12 195 printf("position y =%.2f\n",y);
Vanessa620 10:3eab080dbc12 196 }
Vanessa620 10:3eab080dbc12 197
Vanessa620 10:3eab080dbc12 198 for(double i=PI; i<=(3*PI)/2 ;i=i+0.5)
Vanessa620 10:3eab080dbc12 199 {
Vanessa620 10:3eab080dbc12 200 x=radio*cos(i);
Vanessa620 10:3eab080dbc12 201 y=radio*sin(i);
Vanessa620 10:3eab080dbc12 202 vertex2d(x+y,cx+cy);
Vanessa620 10:3eab080dbc12 203 printf("position x =%.2f\n",x);
Vanessa620 10:3eab080dbc12 204 printf("position y =%.2f\n",y);
Vanessa620 10:3eab080dbc12 205 }
Vanessa620 10:3eab080dbc12 206
Vanessa620 10:3eab080dbc12 207 for(double i=3*PI/2; i<=2*PI ;i=i+0.1)
Vanessa620 10:3eab080dbc12 208 {
Vanessa620 10:3eab080dbc12 209 x=radio*cos(i);
Vanessa620 10:3eab080dbc12 210 y=radio*sin(i);
Vanessa620 10:3eab080dbc12 211 vertex2d(cx+x,cy-y);
Vanessa620 10:3eab080dbc12 212 printf("position x =%.2f\n",x);
Vanessa620 10:3eab080dbc12 213 printf("position y =%.2f\n",y);
Vanessa620 10:3eab080dbc12 214 }
Vanessa620 10:3eab080dbc12 215 }
Vanessa620 10:3eab080dbc12 216
fabeltranm 7:fab201aa45b7 217 void command_exe()
fabeltranm 7:fab201aa45b7 218 {
fabeltranm 7:fab201aa45b7 219
fabeltranm 7:fab201aa45b7 220 switch (buffer_command[COMM_N]){
fabeltranm 7:fab201aa45b7 221
fabeltranm 7:fab201aa45b7 222 case (LED_NC): command_led(buffer_command[INITPARAMETER]);
Vanessa620 10:3eab080dbc12 223 #if DEBUG
Vanessa620 10:3eab080dbc12 224 command.printf("led on 7 off\n");
Vanessa620 10:3eab080dbc12 225 #endif
fabeltranm 7:fab201aa45b7 226 break;
fabeltranm 7:fab201aa45b7 227 case (DOT_NC):
fabeltranm 7:fab201aa45b7 228 #if DEBUG
Vanessa620 10:3eab080dbc12 229 command.printf("Punto \n");
fabeltranm 7:fab201aa45b7 230 #endif
Vanessa620 10:3eab080dbc12 231 punto(buffer_command[INITPARAMETER], buffer_command[INITPARAMETER+1]);
fabeltranm 7:fab201aa45b7 232 break;
fabeltranm 7:fab201aa45b7 233
Vanessa620 10:3eab080dbc12 234 case (LINE_NC):
fabeltranm 7:fab201aa45b7 235 #if DEBUG
fabeltranm 7:fab201aa45b7 236 command.printf("draw line\n");
fabeltranm 7:fab201aa45b7 237 #endif
Vanessa620 10:3eab080dbc12 238 linea(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]);
fabeltranm 7:fab201aa45b7 239
fabeltranm 7:fab201aa45b7 240 break;
fabeltranm 7:fab201aa45b7 241
manuelitoys 9:5008f9501dbf 242 case (CUADRADO):
manuelitoys 9:5008f9501dbf 243 #if DEBUG
manuelitoys 9:5008f9501dbf 244 command.printf("cuadrado \n");
manuelitoys 9:5008f9501dbf 245 #endif
Vanessa620 10:3eab080dbc12 246 cuadrado(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2]);
manuelitoys 8:38ae341e2a4f 247
fabeltranm 7:fab201aa45b7 248
fabeltranm 7:fab201aa45b7 249
fabeltranm 7:fab201aa45b7 250 break;
Vanessa620 10:3eab080dbc12 251 case (HOME):
Vanessa620 10:3eab080dbc12 252 #if DEBUG
Vanessa620 10:3eab080dbc12 253 command.printf("Yendo a Home \n");
Vanessa620 10:3eab080dbc12 254 #endif
Vanessa620 10:3eab080dbc12 255 home();
Vanessa620 10:3eab080dbc12 256 break;
fabeltranm 7:fab201aa45b7 257
fabeltranm 7:fab201aa45b7 258 default:
fabeltranm 7:fab201aa45b7 259
fabeltranm 7:fab201aa45b7 260 #if DEBUG
fabeltranm 7:fab201aa45b7 261 command.printf("comando no encontrado\n");
fabeltranm 7:fab201aa45b7 262 #endif
fabeltranm 7:fab201aa45b7 263
fabeltranm 7:fab201aa45b7 264
fabeltranm 7:fab201aa45b7 265 }
fabeltranm 7:fab201aa45b7 266 }
fabeltranm 7:fab201aa45b7 267
fabeltranm 7:fab201aa45b7 268
fabeltranm 7:fab201aa45b7 269
fabeltranm 0:55d11eeb0faf 270 int main() {
fabeltranm 0:55d11eeb0faf 271 #if DEBUG
fabeltranm 0:55d11eeb0faf 272 command.printf("inicio con debug\n");
fabeltranm 0:55d11eeb0faf 273 #else
fabeltranm 0:55d11eeb0faf 274 command.printf("inicio sin debug\n");
fabeltranm 0:55d11eeb0faf 275 #endif
fabeltranm 0:55d11eeb0faf 276 uint8_t val;
fabeltranm 0:55d11eeb0faf 277 while(1){
fabeltranm 0:55d11eeb0faf 278 val=command.getc();
fabeltranm 0:55d11eeb0faf 279 if (val== '<'){
fabeltranm 0:55d11eeb0faf 280 Read_command();
fabeltranm 4:bcc2d1bebb95 281 if (check_command()){
fabeltranm 7:fab201aa45b7 282 command_exe();
fabeltranm 6:cb6b868465c3 283 #if DEBUG
fabeltranm 6:cb6b868465c3 284 echo_command();
fabeltranm 6:cb6b868465c3 285 #endif
fabeltranm 6:cb6b868465c3 286 }
fabeltranm 3:7c26d338e372 287 }
fabeltranm 3:7c26d338e372 288
fabeltranm 0:55d11eeb0faf 289 }
fabeltranm 0:55d11eeb0faf 290 }
fabeltranm 0:55d11eeb0faf 291