luis marin / Mbed 2 deprecated BRAZO_01

control brazo robotico

Dependencies:   mbed

main.cpp@0:58800e1db02a, 2019-05-11 (annotated)

Committer:
mrdarka94
Date:
Sat May 11 03:22:07 2019 +0000
Revision:
0:58800e1db02a
lol

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mrdarka94 0:58800e1db02a 1 #include "mbed.h"
mrdarka94 0:58800e1db02a 2 #include "scolor_TCS3200.h"
mrdarka94 0:58800e1db02a 3
mrdarka94 0:58800e1db02a 4 /*********
mrdarka94 0:58800e1db02a 5 Este archivo es una modificacion al ejercicio propuesta por el ingeniero Ferney alberto Beltran
mrdarka94 0:58800e1db02a 6 molina.
mrdarka94 0:58800e1db02a 7 generar un programa que controle por el puerto serial el grado de 4 servo motores.
mrdarka94 0:58800e1db02a 8 por medio de la comunicacion serial el comando es
mrdarka94 0:58800e1db02a 9
mrdarka94 0:58800e1db02a 10 | | | | |
mrdarka94 0:58800e1db02a 11 | INITCMD | Tipo_Comando | Parametro_1 | Funcion |
mrdarka94 0:58800e1db02a 12 | 0xff | 0x01 | 0x01 | Pos. Home |
mrdarka94 0:58800e1db02a 13 | 0xff | 0x01 | 0x02 | Pos. Material |
mrdarka94 0:58800e1db02a 14 | 0xff | 0x01 | 0x03 | Pos. Celda 1 |
mrdarka94 0:58800e1db02a 15 | 0xff | 0x01 | 0x04 | Pos. Celda 2 |
mrdarka94 0:58800e1db02a 16 | 0xff | 0x01 | 0x05 | Pos. Celda 3 |
mrdarka94 0:58800e1db02a 17 | 0xff | 0x01 | 0x06 | Pos. Celda 4 |
mrdarka94 0:58800e1db02a 18 | 0xff | 0x02 | 0x01 | Abre Pinza |
mrdarka94 0:58800e1db02a 19 | 0xff | 0x02 | 0x02 | Cierra Pinza |
mrdarka94 0:58800e1db02a 20 | 0xff | 0x03 | 0x00 - 0xb4 | Mueve Motor 1 |
mrdarka94 0:58800e1db02a 21 | 0xff | 0x04 | 0x00 - 0xb4 | Mueve Motor 2 |
mrdarka94 0:58800e1db02a 22 | 0xff | 0x05 | 0x00 - 0xb4 | Mueve Motor 3 |
mrdarka94 0:58800e1db02a 23 | 0xff | 0x06 | 0x00 - 0xb4 | Mueve Motor 4 |
mrdarka94 0:58800e1db02a 24 | 0xff | 0x07 | 0x00 - 0x03 | Mueve Motor 1 |
mrdarka94 0:58800e1db02a 25 | 0xff | 0x08 | 0x00 - 0xb4 | Mueve Motor 2 |
mrdarka94 0:58800e1db02a 26 | 0xff | 0x09 | 0x00 - 0xb4 | Mueve Motor 3 |
mrdarka94 0:58800e1db02a 27
mrdarka94 0:58800e1db02a 28
mrdarka94 0:58800e1db02a 29 para enviar los comandos usar el programa Coolterm http://freeware.the-meiers.org/
mrdarka94 0:58800e1db02a 30
mrdarka94 0:58800e1db02a 31 # para el servo motor se debe modificar el ciclo util del PWM SEGUN:
mrdarka94 0:58800e1db02a 32 # _ _
mrdarka94 0:58800e1db02a 33 # | |_| |___
mrdarka94 0:58800e1db02a 34 # <-width->
mrdarka94 0:58800e1db02a 35 # <-------period 20ms--->
mrdarka94 0:58800e1db02a 36 # period = 20 ms
mrdarka94 0:58800e1db02a 37 # width = 1000us y 2000us
mrdarka94 0:58800e1db02a 38 # 1000us para 0 grados
mrdarka94 0:58800e1db02a 39 # 2000us para 180 grados
mrdarka94 0:58800e1db02a 40 _________
mrdarka94 0:58800e1db02a 41 SO | S1 | OUTPUT FREQUENCY SCALING | | S2 | S3 | PHOTODIODE TYPE |
mrdarka94 0:58800e1db02a 42 0 | 0 | power down | | 0 | 0 | Red |
mrdarka94 0:58800e1db02a 43 0 | 1 | 2% | | 0 | 1 | Blue |
mrdarka94 0:58800e1db02a 44 1 | 0 | 20% | | 1 | 0 | Clear (no filter) |
mrdarka94 0:58800e1db02a 45 1 | 1 | 100% | | 1 | 1 | Green |
mrdarka94 0:58800e1db02a 46
mrdarka94 0:58800e1db02a 47
mrdarka94 0:58800e1db02a 48 *********/
mrdarka94 0:58800e1db02a 49
mrdarka94 0:58800e1db02a 50 // configuracion comunicación serial
mrdarka94 0:58800e1db02a 51 Serial command(USBTX, USBRX);
mrdarka94 0:58800e1db02a 52 // seleccion de los pines a usar en la stm32f411re
mrdarka94 0:58800e1db02a 53 PwmOut myservo1(PB_4);
mrdarka94 0:58800e1db02a 54 PwmOut myservo2(PB_3);
mrdarka94 0:58800e1db02a 55 PwmOut myservo3(PB_10);
mrdarka94 0:58800e1db02a 56 PwmOut myservo4(PC_7);
mrdarka94 0:58800e1db02a 57 scolor_TCS3200 scolor(PA_8, PA_9, PB_6, PA_7, PA_6);
mrdarka94 0:58800e1db02a 58 AnalogIn analog_value1(A1);
mrdarka94 0:58800e1db02a 59 AnalogIn analog_value2(A2);
mrdarka94 0:58800e1db02a 60 AnalogIn analog_value3(A3);
mrdarka94 0:58800e1db02a 61 DigitalIn abrir(PA_10);
mrdarka94 0:58800e1db02a 62 DigitalIn cerrar(PB_5);
mrdarka94 0:58800e1db02a 63
mrdarka94 0:58800e1db02a 64
mrdarka94 0:58800e1db02a 65
mrdarka94 0:58800e1db02a 66
mrdarka94 0:58800e1db02a 67
mrdarka94 0:58800e1db02a 68
mrdarka94 0:58800e1db02a 69 #define INITCMD 0xFF
mrdarka94 0:58800e1db02a 70 #define INITELE 0xFE
mrdarka94 0:58800e1db02a 71 #define DEGREES_MAX 180
mrdarka94 0:58800e1db02a 72 #define CMD 0x01
mrdarka94 0:58800e1db02a 73
mrdarka94 0:58800e1db02a 74 // definición de las variables globales
mrdarka94 0:58800e1db02a 75
mrdarka94 0:58800e1db02a 76
mrdarka94 0:58800e1db02a 77 uint8_t Tipo_Comando; // varable que almacena el numero de seleccion de comando
mrdarka94 0:58800e1db02a 78 uint8_t Parametro_1; // varable que almacena el parametro a ejecutar
mrdarka94 0:58800e1db02a 79 uint8_t velociraptor =10 ;
mrdarka94 0:58800e1db02a 80 uint32_t dpulse=0;
mrdarka94 0:58800e1db02a 81
mrdarka94 0:58800e1db02a 82
mrdarka94 0:58800e1db02a 83 // definición de las funciones
mrdarka94 0:58800e1db02a 84 void setup_uart();
mrdarka94 0:58800e1db02a 85 void setup_servo();
mrdarka94 0:58800e1db02a 86 void leer_datos();
mrdarka94 0:58800e1db02a 87 void leer_color();
mrdarka94 0:58800e1db02a 88 void servo(uint8_t T_C, uint8_t Par_1);
mrdarka94 0:58800e1db02a 89 uint32_t degrees2usec(uint8_t grados);
mrdarka94 0:58800e1db02a 90 uint8_t cmd;
mrdarka94 0:58800e1db02a 91
mrdarka94 0:58800e1db02a 92
mrdarka94 0:58800e1db02a 93 int main() {
mrdarka94 0:58800e1db02a 94
mrdarka94 0:58800e1db02a 95 setup_servo();
mrdarka94 0:58800e1db02a 96 setup_uart();
mrdarka94 0:58800e1db02a 97 abrir.mode(PullUp);
mrdarka94 0:58800e1db02a 98 cerrar.mode(PullUp);
mrdarka94 0:58800e1db02a 99
mrdarka94 0:58800e1db02a 100 while(1){
mrdarka94 0:58800e1db02a 101 leer_datos();
mrdarka94 0:58800e1db02a 102 servo(Tipo_Comando, Parametro_1);
mrdarka94 0:58800e1db02a 103
mrdarka94 0:58800e1db02a 104 }
mrdarka94 0:58800e1db02a 105 }
mrdarka94 0:58800e1db02a 106
mrdarka94 0:58800e1db02a 107
mrdarka94 0:58800e1db02a 108
mrdarka94 0:58800e1db02a 109
mrdarka94 0:58800e1db02a 110
mrdarka94 0:58800e1db02a 111 void setup_uart(){
mrdarka94 0:58800e1db02a 112 command.baud(115200);
mrdarka94 0:58800e1db02a 113 }
mrdarka94 0:58800e1db02a 114
mrdarka94 0:58800e1db02a 115 void setup_servo(){
mrdarka94 0:58800e1db02a 116 myservo1.period_ms(20);
mrdarka94 0:58800e1db02a 117 myservo1.pulsewidth_us(1350);
mrdarka94 0:58800e1db02a 118 myservo2.period_ms(20);
mrdarka94 0:58800e1db02a 119 myservo2.pulsewidth_us(900);
mrdarka94 0:58800e1db02a 120 myservo3.period_ms(20);
mrdarka94 0:58800e1db02a 121 myservo3.pulsewidth_us(1900);
mrdarka94 0:58800e1db02a 122 myservo4.period_ms(20);
mrdarka94 0:58800e1db02a 123 myservo4.pulsewidth_us(1550);
mrdarka94 0:58800e1db02a 124 }
mrdarka94 0:58800e1db02a 125
mrdarka94 0:58800e1db02a 126 // funcion leer_datos obtiene los comandos enviados desde cool Terminal
mrdarka94 0:58800e1db02a 127 void leer_datos(){
mrdarka94 0:58800e1db02a 128 while(command.getc()!= INITCMD);
mrdarka94 0:58800e1db02a 129 Tipo_Comando=command.getc();
mrdarka94 0:58800e1db02a 130 Parametro_1=command.getc();
mrdarka94 0:58800e1db02a 131
mrdarka94 0:58800e1db02a 132 }
mrdarka94 0:58800e1db02a 133
mrdarka94 0:58800e1db02a 134
mrdarka94 0:58800e1db02a 135 //Esta funcion convierte de grados a microsengudos, segun el ancho de pulso del servomotor
mrdarka94 0:58800e1db02a 136 uint32_t degrees2usec(uint8_t grados){
mrdarka94 0:58800e1db02a 137 if(grados <= DEGREES_MAX)
mrdarka94 0:58800e1db02a 138 return float(440+grados*17.33/2);// u6
mrdarka94 0:58800e1db02a 139 return 440;
mrdarka94 0:58800e1db02a 140 }
mrdarka94 0:58800e1db02a 141
mrdarka94 0:58800e1db02a 142
mrdarka94 0:58800e1db02a 143 void servo(uint8_t T_C, uint8_t Par_1){
mrdarka94 0:58800e1db02a 144
mrdarka94 0:58800e1db02a 145 dpulse=degrees2usec(Par_1);
mrdarka94 0:58800e1db02a 146
mrdarka94 0:58800e1db02a 147 switch(T_C){
mrdarka94 0:58800e1db02a 148 case 1:
mrdarka94 0:58800e1db02a 149 if (Par_1==1){//posicion home
mrdarka94 0:58800e1db02a 150 myservo3.pulsewidth_us(degrees2usec(180));
mrdarka94 0:58800e1db02a 151 wait(1);
mrdarka94 0:58800e1db02a 152 myservo2.pulsewidth_us(degrees2usec(50));
mrdarka94 0:58800e1db02a 153 wait(1);
mrdarka94 0:58800e1db02a 154 myservo1.pulsewidth_us(degrees2usec(110));
mrdarka94 0:58800e1db02a 155
mrdarka94 0:58800e1db02a 156
mrdarka94 0:58800e1db02a 157 }
mrdarka94 0:58800e1db02a 158 if (Par_1==2){//posicion material
mrdarka94 0:58800e1db02a 159
mrdarka94 0:58800e1db02a 160 myservo3.pulsewidth_us(degrees2usec(180));
mrdarka94 0:58800e1db02a 161 wait(1);
mrdarka94 0:58800e1db02a 162 myservo2.pulsewidth_us(degrees2usec(50));
mrdarka94 0:58800e1db02a 163 wait(1);
mrdarka94 0:58800e1db02a 164 myservo1.pulsewidth_us(degrees2usec(90));
mrdarka94 0:58800e1db02a 165 wait(1);
mrdarka94 0:58800e1db02a 166
mrdarka94 0:58800e1db02a 167
mrdarka94 0:58800e1db02a 168
mrdarka94 0:58800e1db02a 169 myservo3.pulsewidth_us(degrees2usec(70));
mrdarka94 0:58800e1db02a 170 wait(1);
mrdarka94 0:58800e1db02a 171 myservo2.pulsewidth_us(degrees2usec(50));
mrdarka94 0:58800e1db02a 172 wait(1);
mrdarka94 0:58800e1db02a 173 myservo1.pulsewidth_us(degrees2usec(170));
mrdarka94 0:58800e1db02a 174 wait(1);
mrdarka94 0:58800e1db02a 175 myservo2.pulsewidth_us(degrees2usec(90));
mrdarka94 0:58800e1db02a 176 wait(1);
mrdarka94 0:58800e1db02a 177
mrdarka94 0:58800e1db02a 178 }
mrdarka94 0:58800e1db02a 179 if (Par_1==3){//posicion celda 1
mrdarka94 0:58800e1db02a 180
mrdarka94 0:58800e1db02a 181
mrdarka94 0:58800e1db02a 182 myservo3.pulsewidth_us(degrees2usec(180));
mrdarka94 0:58800e1db02a 183 wait(1);
mrdarka94 0:58800e1db02a 184 myservo2.pulsewidth_us(degrees2usec(50));
mrdarka94 0:58800e1db02a 185 wait(1);
mrdarka94 0:58800e1db02a 186 myservo1.pulsewidth_us(degrees2usec(90));
mrdarka94 0:58800e1db02a 187 wait(1);
mrdarka94 0:58800e1db02a 188
mrdarka94 0:58800e1db02a 189
mrdarka94 0:58800e1db02a 190
mrdarka94 0:58800e1db02a 191
mrdarka94 0:58800e1db02a 192 myservo1.pulsewidth_us(degrees2usec(35));
mrdarka94 0:58800e1db02a 193 wait(1);
mrdarka94 0:58800e1db02a 194 myservo2.pulsewidth_us(degrees2usec(90));
mrdarka94 0:58800e1db02a 195 wait(1);
mrdarka94 0:58800e1db02a 196 myservo3.pulsewidth_us(degrees2usec(90));
mrdarka94 0:58800e1db02a 197 wait(1);
mrdarka94 0:58800e1db02a 198
mrdarka94 0:58800e1db02a 199 }
mrdarka94 0:58800e1db02a 200 if (Par_1==4){//posicion celda 2
mrdarka94 0:58800e1db02a 201
mrdarka94 0:58800e1db02a 202
mrdarka94 0:58800e1db02a 203 myservo3.pulsewidth_us(degrees2usec(180));
mrdarka94 0:58800e1db02a 204 wait(1);
mrdarka94 0:58800e1db02a 205 myservo2.pulsewidth_us(degrees2usec(50));
mrdarka94 0:58800e1db02a 206 wait(1);
mrdarka94 0:58800e1db02a 207 myservo1.pulsewidth_us(degrees2usec(90));
mrdarka94 0:58800e1db02a 208 wait(1);
mrdarka94 0:58800e1db02a 209
mrdarka94 0:58800e1db02a 210
mrdarka94 0:58800e1db02a 211
mrdarka94 0:58800e1db02a 212
mrdarka94 0:58800e1db02a 213 myservo1.pulsewidth_us(degrees2usec(70));
mrdarka94 0:58800e1db02a 214 wait(1);
mrdarka94 0:58800e1db02a 215 myservo2.pulsewidth_us(degrees2usec(90));
mrdarka94 0:58800e1db02a 216 wait(1);
mrdarka94 0:58800e1db02a 217 myservo3.pulsewidth_us(degrees2usec(90));
mrdarka94 0:58800e1db02a 218 wait(1);
mrdarka94 0:58800e1db02a 219
mrdarka94 0:58800e1db02a 220 }
mrdarka94 0:58800e1db02a 221 if (Par_1==5){//posicion celda 3
mrdarka94 0:58800e1db02a 222
mrdarka94 0:58800e1db02a 223 myservo3.pulsewidth_us(degrees2usec(180));
mrdarka94 0:58800e1db02a 224 wait(1);
mrdarka94 0:58800e1db02a 225 myservo2.pulsewidth_us(degrees2usec(50));
mrdarka94 0:58800e1db02a 226 wait(1);
mrdarka94 0:58800e1db02a 227 myservo1.pulsewidth_us(degrees2usec(90));
mrdarka94 0:58800e1db02a 228 wait(1);
mrdarka94 0:58800e1db02a 229
mrdarka94 0:58800e1db02a 230
mrdarka94 0:58800e1db02a 231
mrdarka94 0:58800e1db02a 232
mrdarka94 0:58800e1db02a 233 myservo1.pulsewidth_us(degrees2usec(105));
mrdarka94 0:58800e1db02a 234 wait(1);
mrdarka94 0:58800e1db02a 235 myservo2.pulsewidth_us(degrees2usec(90));
mrdarka94 0:58800e1db02a 236 wait(1);
mrdarka94 0:58800e1db02a 237 myservo3.pulsewidth_us(degrees2usec(90));
mrdarka94 0:58800e1db02a 238 wait(1);
mrdarka94 0:58800e1db02a 239
mrdarka94 0:58800e1db02a 240
mrdarka94 0:58800e1db02a 241 }
mrdarka94 0:58800e1db02a 242 if (Par_1==6){//posicion celda 4
mrdarka94 0:58800e1db02a 243
mrdarka94 0:58800e1db02a 244
mrdarka94 0:58800e1db02a 245
mrdarka94 0:58800e1db02a 246 myservo3.pulsewidth_us(degrees2usec(180));
mrdarka94 0:58800e1db02a 247 wait(1);
mrdarka94 0:58800e1db02a 248 myservo2.pulsewidth_us(degrees2usec(50));
mrdarka94 0:58800e1db02a 249 wait(1);
mrdarka94 0:58800e1db02a 250 myservo1.pulsewidth_us(degrees2usec(90));
mrdarka94 0:58800e1db02a 251 wait(1);
mrdarka94 0:58800e1db02a 252
mrdarka94 0:58800e1db02a 253
mrdarka94 0:58800e1db02a 254
mrdarka94 0:58800e1db02a 255 myservo1.pulsewidth_us(degrees2usec(140));
mrdarka94 0:58800e1db02a 256 wait(1);
mrdarka94 0:58800e1db02a 257 myservo2.pulsewidth_us(degrees2usec(90));
mrdarka94 0:58800e1db02a 258 wait(1);
mrdarka94 0:58800e1db02a 259 myservo3.pulsewidth_us(degrees2usec(90));
mrdarka94 0:58800e1db02a 260 wait(1);
mrdarka94 0:58800e1db02a 261
mrdarka94 0:58800e1db02a 262
mrdarka94 0:58800e1db02a 263 }
mrdarka94 0:58800e1db02a 264 break;
mrdarka94 0:58800e1db02a 265
mrdarka94 0:58800e1db02a 266 case 2:
mrdarka94 0:58800e1db02a 267 if (Par_1==1){//abrir pinza
mrdarka94 0:58800e1db02a 268 myservo4.pulsewidth_us(degrees2usec(100));
mrdarka94 0:58800e1db02a 269 }
mrdarka94 0:58800e1db02a 270
mrdarka94 0:58800e1db02a 271 if (Par_1==2){//cerrar pinza
mrdarka94 0:58800e1db02a 272 myservo4.pulsewidth_us(degrees2usec(170));
mrdarka94 0:58800e1db02a 273 }
mrdarka94 0:58800e1db02a 274 break;
mrdarka94 0:58800e1db02a 275
mrdarka94 0:58800e1db02a 276 case 3:
mrdarka94 0:58800e1db02a 277 myservo1.pulsewidth_us(dpulse);
mrdarka94 0:58800e1db02a 278
mrdarka94 0:58800e1db02a 279
mrdarka94 0:58800e1db02a 280 break;
mrdarka94 0:58800e1db02a 281
mrdarka94 0:58800e1db02a 282 case 4:
mrdarka94 0:58800e1db02a 283 myservo2.pulsewidth_us(dpulse);
mrdarka94 0:58800e1db02a 284
mrdarka94 0:58800e1db02a 285 break;
mrdarka94 0:58800e1db02a 286 case 5:
mrdarka94 0:58800e1db02a 287 myservo3.pulsewidth_us(dpulse);
mrdarka94 0:58800e1db02a 288
mrdarka94 0:58800e1db02a 289 break;
mrdarka94 0:58800e1db02a 290 case 6:
mrdarka94 0:58800e1db02a 291 if(Par_1>=120)
mrdarka94 0:58800e1db02a 292 myservo4.pulsewidth_us(dpulse);
mrdarka94 0:58800e1db02a 293
mrdarka94 0:58800e1db02a 294 break;
mrdarka94 0:58800e1db02a 295
mrdarka94 0:58800e1db02a 296 case 7:
mrdarka94 0:58800e1db02a 297 if(Par_1==1){//velocida baja
mrdarka94 0:58800e1db02a 298 myservo1.period_ms(40);
mrdarka94 0:58800e1db02a 299 myservo1.pulsewidth_us(1350);
mrdarka94 0:58800e1db02a 300 myservo2.period_ms(40);
mrdarka94 0:58800e1db02a 301 myservo2.pulsewidth_us(900);
mrdarka94 0:58800e1db02a 302 myservo3.period_ms(40);
mrdarka94 0:58800e1db02a 303 myservo3.pulsewidth_us(1900);
mrdarka94 0:58800e1db02a 304 myservo4.period_ms(40);
mrdarka94 0:58800e1db02a 305 myservo4.pulsewidth_us(1550);
mrdarka94 0:58800e1db02a 306 }
mrdarka94 0:58800e1db02a 307
mrdarka94 0:58800e1db02a 308 else if(Par_1==2){//velocidad media
mrdarka94 0:58800e1db02a 309 myservo1.period_ms(20);
mrdarka94 0:58800e1db02a 310 myservo1.pulsewidth_us(1350);
mrdarka94 0:58800e1db02a 311 myservo2.period_ms(20);
mrdarka94 0:58800e1db02a 312 myservo2.pulsewidth_us(900);
mrdarka94 0:58800e1db02a 313 myservo3.period_ms(20);
mrdarka94 0:58800e1db02a 314 myservo3.pulsewidth_us(1900);
mrdarka94 0:58800e1db02a 315 myservo4.period_ms(20);
mrdarka94 0:58800e1db02a 316 myservo4.pulsewidth_us(1550);
mrdarka94 0:58800e1db02a 317 }
mrdarka94 0:58800e1db02a 318
mrdarka94 0:58800e1db02a 319 else{ //velocidad alta
mrdarka94 0:58800e1db02a 320 myservo1.period_ms(10);
mrdarka94 0:58800e1db02a 321 myservo1.pulsewidth_us(1350);
mrdarka94 0:58800e1db02a 322 myservo2.period_ms(10);
mrdarka94 0:58800e1db02a 323 myservo2.pulsewidth_us(900);
mrdarka94 0:58800e1db02a 324 myservo3.period_ms(10);
mrdarka94 0:58800e1db02a 325 myservo3.pulsewidth_us(1900);
mrdarka94 0:58800e1db02a 326 myservo4.period_ms(10);
mrdarka94 0:58800e1db02a 327 myservo4.pulsewidth_us(1550);
mrdarka94 0:58800e1db02a 328 }
mrdarka94 0:58800e1db02a 329
mrdarka94 0:58800e1db02a 330 break;
mrdarka94 0:58800e1db02a 331 case 8:
mrdarka94 0:58800e1db02a 332 float meas_r1,meas_r2,meas_r3;
mrdarka94 0:58800e1db02a 333 float meas_m1,meas_m2, meas_m3;
mrdarka94 0:58800e1db02a 334 float grado1, grado2 , grado3;
mrdarka94 0:58800e1db02a 335 bool x=false;
mrdarka94 0:58800e1db02a 336 if(Par_1==1){
mrdarka94 0:58800e1db02a 337
mrdarka94 0:58800e1db02a 338 while(!x){
mrdarka94 0:58800e1db02a 339
mrdarka94 0:58800e1db02a 340
mrdarka94 0:58800e1db02a 341 meas_r1 = analog_value1.read(); // Read the analog input value (value from 0.0 to 1.0 = full ADC conversion range)
mrdarka94 0:58800e1db02a 342 meas_r2 = analog_value2.read();
mrdarka94 0:58800e1db02a 343 meas_r3 = analog_value3.read();
mrdarka94 0:58800e1db02a 344
mrdarka94 0:58800e1db02a 345 meas_m1 = (meas_r1 * 180) ; // Converts value in the 0V-3.3V range
mrdarka94 0:58800e1db02a 346 meas_m2 = (meas_r2 * 180) ;
mrdarka94 0:58800e1db02a 347 meas_m3 = (meas_r3 * 180) ;
mrdarka94 0:58800e1db02a 348
mrdarka94 0:58800e1db02a 349
mrdarka94 0:58800e1db02a 350 if(meas_m1<= 180){
mrdarka94 0:58800e1db02a 351 grado1=(440+(meas_m1+20)*20.33/2);// u6
mrdarka94 0:58800e1db02a 352 myservo1.pulsewidth_us(grado1);}
mrdarka94 0:58800e1db02a 353 else{ myservo1.pulsewidth_us(440);}
mrdarka94 0:58800e1db02a 354
mrdarka94 0:58800e1db02a 355 if(meas_m2>55){
mrdarka94 0:58800e1db02a 356 grado2=(440+(meas_m2-40)*20.33/2);// u6
mrdarka94 0:58800e1db02a 357 myservo2.pulsewidth_us(grado2);
mrdarka94 0:58800e1db02a 358 }
mrdarka94 0:58800e1db02a 359 else{ myservo2.pulsewidth_us(1300);}
mrdarka94 0:58800e1db02a 360
mrdarka94 0:58800e1db02a 361
mrdarka94 0:58800e1db02a 362
mrdarka94 0:58800e1db02a 363 if(meas_m3<= 55){
mrdarka94 0:58800e1db02a 364 grado3=(440+(meas_m3+30)*20.33/2);// u6
mrdarka94 0:58800e1db02a 365 myservo3.pulsewidth_us(grado3);}
mrdarka94 0:58800e1db02a 366 else{ myservo3.pulsewidth_us(1900);}
mrdarka94 0:58800e1db02a 367
mrdarka94 0:58800e1db02a 368 if(!abrir){myservo4.pulsewidth_us(degrees2usec(120));}
mrdarka94 0:58800e1db02a 369
mrdarka94 0:58800e1db02a 370 if(!cerrar){myservo4.pulsewidth_us(degrees2usec(170));}
mrdarka94 0:58800e1db02a 371
mrdarka94 0:58800e1db02a 372 if(command.readable()){
mrdarka94 0:58800e1db02a 373 leer_datos();
mrdarka94 0:58800e1db02a 374 servo(Tipo_Comando, Parametro_1);
mrdarka94 0:58800e1db02a 375 if(Par_1==2){
mrdarka94 0:58800e1db02a 376 break;}
mrdarka94 0:58800e1db02a 377 }
mrdarka94 0:58800e1db02a 378
mrdarka94 0:58800e1db02a 379
mrdarka94 0:58800e1db02a 380 wait(0.05); // 1 second
mrdarka94 0:58800e1db02a 381
mrdarka94 0:58800e1db02a 382 }}
mrdarka94 0:58800e1db02a 383
mrdarka94 0:58800e1db02a 384
mrdarka94 0:58800e1db02a 385
mrdarka94 0:58800e1db02a 386 case 9:
mrdarka94 0:58800e1db02a 387 long red;
mrdarka94 0:58800e1db02a 388 long green;
mrdarka94 0:58800e1db02a 389 long blue;
mrdarka94 0:58800e1db02a 390 long clear;
mrdarka94 0:58800e1db02a 391
mrdarka94 0:58800e1db02a 392 red = scolor.ReadRed();
mrdarka94 0:58800e1db02a 393 green = scolor.ReadGreen();
mrdarka94 0:58800e1db02a 394 blue = scolor.ReadBlue();
mrdarka94 0:58800e1db02a 395 clear = scolor.ReadClear();
mrdarka94 0:58800e1db02a 396
mrdarka94 0:58800e1db02a 397 if(red<green && red<blue){
mrdarka94 0:58800e1db02a 398
mrdarka94 0:58800e1db02a 399 if (red<8){
mrdarka94 0:58800e1db02a 400 command.printf("0xFE 0x04 \n ");
mrdarka94 0:58800e1db02a 401 }
mrdarka94 0:58800e1db02a 402 else{
mrdarka94 0:58800e1db02a 403 command.printf("0xFE 0x01 \n ");}
mrdarka94 0:58800e1db02a 404
mrdarka94 0:58800e1db02a 405 }
mrdarka94 0:58800e1db02a 406
mrdarka94 0:58800e1db02a 407 if(green<red && green<blue){
mrdarka94 0:58800e1db02a 408 command.printf("0xFE 0x03 \n ");
mrdarka94 0:58800e1db02a 409 }
mrdarka94 0:58800e1db02a 410 if(blue<green && blue<red){
mrdarka94 0:58800e1db02a 411 command.printf("0xFE 0x02 \n ");
mrdarka94 0:58800e1db02a 412 }
mrdarka94 0:58800e1db02a 413
mrdarka94 0:58800e1db02a 414
mrdarka94 0:58800e1db02a 415
mrdarka94 0:58800e1db02a 416 // command.printf("RED: %5d GREEN: %5d BLUE: %5d CLEAR: %5d \n ", red, green, blue, clear);}
mrdarka94 0:58800e1db02a 417 break;
mrdarka94 0:58800e1db02a 418
mrdarka94 0:58800e1db02a 419
mrdarka94 0:58800e1db02a 420 }}