luis marin
control brazo robotico
Diff: main.cpp
- Revision:
- 0:58800e1db02a
diff -r 000000000000 -r 58800e1db02a main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat May 11 03:22:07 2019 +0000
@@ -0,0 +1,420 @@
+#include "mbed.h"
+#include "scolor_TCS3200.h"
+
+/*********
+Este archivo es una modificacion al ejercicio propuesta por el ingeniero Ferney alberto Beltran
+molina.
+generar un programa que controle por el puerto serial el grado de 4 servo motores.
+por medio de la comunicacion serial el comando es
+
+| | | | |
+| INITCMD | Tipo_Comando | Parametro_1 | Funcion |
+| 0xff | 0x01 | 0x01 | Pos. Home |
+| 0xff | 0x01 | 0x02 | Pos. Material |
+| 0xff | 0x01 | 0x03 | Pos. Celda 1 |
+| 0xff | 0x01 | 0x04 | Pos. Celda 2 |
+| 0xff | 0x01 | 0x05 | Pos. Celda 3 |
+| 0xff | 0x01 | 0x06 | Pos. Celda 4 |
+| 0xff | 0x02 | 0x01 | Abre Pinza |
+| 0xff | 0x02 | 0x02 | Cierra Pinza |
+| 0xff | 0x03 | 0x00 - 0xb4 | Mueve Motor 1 |
+| 0xff | 0x04 | 0x00 - 0xb4 | Mueve Motor 2 |
+| 0xff | 0x05 | 0x00 - 0xb4 | Mueve Motor 3 |
+| 0xff | 0x06 | 0x00 - 0xb4 | Mueve Motor 4 |
+| 0xff | 0x07 | 0x00 - 0x03 | Mueve Motor 1 |
+| 0xff | 0x08 | 0x00 - 0xb4 | Mueve Motor 2 |
+| 0xff | 0x09 | 0x00 - 0xb4 | Mueve Motor 3 |
+
+
+para enviar los comandos usar el programa Coolterm http://freeware.the-meiers.org/
+
+# para el servo motor se debe modificar el ciclo util del PWM SEGUN:
+# _ _
+# | |_| |___
+# <-width->
+# <-------period 20ms--->
+# period = 20 ms
+# width = 1000us y 2000us
+# 1000us para 0 grados
+# 2000us para 180 grados
+_________
+SO | S1 | OUTPUT FREQUENCY SCALING | | S2 | S3 | PHOTODIODE TYPE |
+ 0 | 0 | power down | | 0 | 0 | Red |
+ 0 | 1 | 2% | | 0 | 1 | Blue |
+ 1 | 0 | 20% | | 1 | 0 | Clear (no filter) |
+ 1 | 1 | 100% | | 1 | 1 | Green |
+
+
+*********/
+
+// configuracion comunicación serial
+Serial command(USBTX, USBRX);
+// seleccion de los pines a usar en la stm32f411re
+PwmOut myservo1(PB_4);
+PwmOut myservo2(PB_3);
+PwmOut myservo3(PB_10);
+PwmOut myservo4(PC_7);
+scolor_TCS3200 scolor(PA_8, PA_9, PB_6, PA_7, PA_6);
+AnalogIn analog_value1(A1);
+AnalogIn analog_value2(A2);
+AnalogIn analog_value3(A3);
+DigitalIn abrir(PA_10);
+DigitalIn cerrar(PB_5);
+
+
+
+
+
+
+#define INITCMD 0xFF
+#define INITELE 0xFE
+#define DEGREES_MAX 180
+#define CMD 0x01
+
+// definición de las variables globales
+
+
+uint8_t Tipo_Comando; // varable que almacena el numero de seleccion de comando
+uint8_t Parametro_1; // varable que almacena el parametro a ejecutar
+uint8_t velociraptor =10 ;
+uint32_t dpulse=0;
+
+
+// definición de las funciones
+void setup_uart();
+void setup_servo();
+void leer_datos();
+void leer_color();
+void servo(uint8_t T_C, uint8_t Par_1);
+uint32_t degrees2usec(uint8_t grados);
+uint8_t cmd;
+
+
+int main() {
+
+ setup_servo();
+ setup_uart();
+ abrir.mode(PullUp);
+cerrar.mode(PullUp);
+
+ while(1){
+ leer_datos();
+ servo(Tipo_Comando, Parametro_1);
+
+ }
+}
+
+
+
+
+
+void setup_uart(){
+ command.baud(115200);
+}
+
+void setup_servo(){
+ myservo1.period_ms(20);
+ myservo1.pulsewidth_us(1350);
+ myservo2.period_ms(20);
+ myservo2.pulsewidth_us(900);
+ myservo3.period_ms(20);
+ myservo3.pulsewidth_us(1900);
+ myservo4.period_ms(20);
+ myservo4.pulsewidth_us(1550);
+}
+
+// funcion leer_datos obtiene los comandos enviados desde cool Terminal
+void leer_datos(){
+ while(command.getc()!= INITCMD);
+ Tipo_Comando=command.getc();
+ Parametro_1=command.getc();
+
+}
+
+
+//Esta funcion convierte de grados a microsengudos, segun el ancho de pulso del servomotor
+uint32_t degrees2usec(uint8_t grados){
+ if(grados <= DEGREES_MAX)
+ return float(440+grados*17.33/2);// u6
+ return 440;
+}
+
+
+void servo(uint8_t T_C, uint8_t Par_1){
+
+ dpulse=degrees2usec(Par_1);
+
+ switch(T_C){
+ case 1:
+ if (Par_1==1){//posicion home
+ myservo3.pulsewidth_us(degrees2usec(180));
+ wait(1);
+ myservo2.pulsewidth_us(degrees2usec(50));
+ wait(1);
+ myservo1.pulsewidth_us(degrees2usec(110));
+
+
+ }
+ if (Par_1==2){//posicion material
+
+ myservo3.pulsewidth_us(degrees2usec(180));
+ wait(1);
+ myservo2.pulsewidth_us(degrees2usec(50));
+ wait(1);
+ myservo1.pulsewidth_us(degrees2usec(90));
+ wait(1);
+
+
+
+ myservo3.pulsewidth_us(degrees2usec(70));
+ wait(1);
+ myservo2.pulsewidth_us(degrees2usec(50));
+ wait(1);
+ myservo1.pulsewidth_us(degrees2usec(170));
+ wait(1);
+ myservo2.pulsewidth_us(degrees2usec(90));
+ wait(1);
+
+ }
+ if (Par_1==3){//posicion celda 1
+
+
+ myservo3.pulsewidth_us(degrees2usec(180));
+ wait(1);
+ myservo2.pulsewidth_us(degrees2usec(50));
+ wait(1);
+ myservo1.pulsewidth_us(degrees2usec(90));
+ wait(1);
+
+
+
+
+ myservo1.pulsewidth_us(degrees2usec(35));
+ wait(1);
+ myservo2.pulsewidth_us(degrees2usec(90));
+ wait(1);
+ myservo3.pulsewidth_us(degrees2usec(90));
+ wait(1);
+
+ }
+ if (Par_1==4){//posicion celda 2
+
+
+ myservo3.pulsewidth_us(degrees2usec(180));
+ wait(1);
+ myservo2.pulsewidth_us(degrees2usec(50));
+ wait(1);
+ myservo1.pulsewidth_us(degrees2usec(90));
+ wait(1);
+
+
+
+
+ myservo1.pulsewidth_us(degrees2usec(70));
+ wait(1);
+ myservo2.pulsewidth_us(degrees2usec(90));
+ wait(1);
+ myservo3.pulsewidth_us(degrees2usec(90));
+ wait(1);
+
+ }
+ if (Par_1==5){//posicion celda 3
+
+ myservo3.pulsewidth_us(degrees2usec(180));
+ wait(1);
+ myservo2.pulsewidth_us(degrees2usec(50));
+ wait(1);
+ myservo1.pulsewidth_us(degrees2usec(90));
+ wait(1);
+
+
+
+
+ myservo1.pulsewidth_us(degrees2usec(105));
+ wait(1);
+ myservo2.pulsewidth_us(degrees2usec(90));
+ wait(1);
+ myservo3.pulsewidth_us(degrees2usec(90));
+ wait(1);
+
+
+ }
+ if (Par_1==6){//posicion celda 4
+
+
+
+ myservo3.pulsewidth_us(degrees2usec(180));
+ wait(1);
+ myservo2.pulsewidth_us(degrees2usec(50));
+ wait(1);
+ myservo1.pulsewidth_us(degrees2usec(90));
+ wait(1);
+
+
+
+ myservo1.pulsewidth_us(degrees2usec(140));
+ wait(1);
+ myservo2.pulsewidth_us(degrees2usec(90));
+ wait(1);
+ myservo3.pulsewidth_us(degrees2usec(90));
+ wait(1);
+
+
+ }
+ break;
+
+ case 2:
+ if (Par_1==1){//abrir pinza
+ myservo4.pulsewidth_us(degrees2usec(100));
+ }
+
+ if (Par_1==2){//cerrar pinza
+ myservo4.pulsewidth_us(degrees2usec(170));
+ }
+ break;
+
+ case 3:
+ myservo1.pulsewidth_us(dpulse);
+
+
+ break;
+
+ case 4:
+ myservo2.pulsewidth_us(dpulse);
+
+ break;
+ case 5:
+ myservo3.pulsewidth_us(dpulse);
+
+ break;
+ case 6:
+ if(Par_1>=120)
+ myservo4.pulsewidth_us(dpulse);
+
+ break;
+
+ case 7:
+ if(Par_1==1){//velocida baja
+ myservo1.period_ms(40);
+ myservo1.pulsewidth_us(1350);
+ myservo2.period_ms(40);
+ myservo2.pulsewidth_us(900);
+ myservo3.period_ms(40);
+ myservo3.pulsewidth_us(1900);
+ myservo4.period_ms(40);
+ myservo4.pulsewidth_us(1550);
+ }
+
+ else if(Par_1==2){//velocidad media
+ myservo1.period_ms(20);
+ myservo1.pulsewidth_us(1350);
+ myservo2.period_ms(20);
+ myservo2.pulsewidth_us(900);
+ myservo3.period_ms(20);
+ myservo3.pulsewidth_us(1900);
+ myservo4.period_ms(20);
+ myservo4.pulsewidth_us(1550);
+ }
+
+ else{ //velocidad alta
+ myservo1.period_ms(10);
+ myservo1.pulsewidth_us(1350);
+ myservo2.period_ms(10);
+ myservo2.pulsewidth_us(900);
+ myservo3.period_ms(10);
+ myservo3.pulsewidth_us(1900);
+ myservo4.period_ms(10);
+ myservo4.pulsewidth_us(1550);
+ }
+
+ break;
+ case 8:
+ float meas_r1,meas_r2,meas_r3;
+ float meas_m1,meas_m2, meas_m3;
+ float grado1, grado2 , grado3;
+ bool x=false;
+ if(Par_1==1){
+
+ while(!x){
+
+
+ meas_r1 = analog_value1.read(); // Read the analog input value (value from 0.0 to 1.0 = full ADC conversion range)
+ meas_r2 = analog_value2.read();
+ meas_r3 = analog_value3.read();
+
+ meas_m1 = (meas_r1 * 180) ; // Converts value in the 0V-3.3V range
+ meas_m2 = (meas_r2 * 180) ;
+ meas_m3 = (meas_r3 * 180) ;
+
+
+ if(meas_m1<= 180){
+ grado1=(440+(meas_m1+20)*20.33/2);// u6
+ myservo1.pulsewidth_us(grado1);}
+ else{ myservo1.pulsewidth_us(440);}
+
+ if(meas_m2>55){
+ grado2=(440+(meas_m2-40)*20.33/2);// u6
+ myservo2.pulsewidth_us(grado2);
+ }
+ else{ myservo2.pulsewidth_us(1300);}
+
+
+
+ if(meas_m3<= 55){
+ grado3=(440+(meas_m3+30)*20.33/2);// u6
+ myservo3.pulsewidth_us(grado3);}
+ else{ myservo3.pulsewidth_us(1900);}
+
+ if(!abrir){myservo4.pulsewidth_us(degrees2usec(120));}
+
+ if(!cerrar){myservo4.pulsewidth_us(degrees2usec(170));}
+
+ if(command.readable()){
+ leer_datos();
+ servo(Tipo_Comando, Parametro_1);
+ if(Par_1==2){
+ break;}
+ }
+
+
+ wait(0.05); // 1 second
+
+ }}
+
+
+
+ case 9:
+ long red;
+ long green;
+ long blue;
+ long clear;
+
+ red = scolor.ReadRed();
+ green = scolor.ReadGreen();
+ blue = scolor.ReadBlue();
+ clear = scolor.ReadClear();
+
+ if(red<green && red<blue){
+
+ if (red<8){
+ command.printf("0xFE 0x04 \n ");
+ }
+ else{
+ command.printf("0xFE 0x01 \n ");}
+
+ }
+
+ if(green<red && green<blue){
+ command.printf("0xFE 0x03 \n ");
+ }
+ if(blue<green && blue<red){
+ command.printf("0xFE 0x02 \n ");
+ }
+
+
+
+ // command.printf("RED: %5d GREEN: %5d BLUE: %5d CLEAR: %5d \n ", red, green, blue, clear);}
+ break;
+
+
+}}
\ No newline at end of file