luis marin
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;
}}