luis marin
/
BRAZO_01
control brazo robotico
main.cpp
- Committer:
- mrdarka94
- Date:
- 2019-05-11
- Revision:
- 0:58800e1db02a
File content as of revision 0:58800e1db02a:
#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; }}