Andres Castañeda
/
Entrega
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
main.cpp
- Committer:
- andrescas
- Date:
- 2018-11-14
- Revision:
- 1:9430a3a15f7a
- Parent:
- 0:4b1e8862ad53
- Child:
- 2:e5c7ae640dfd
File content as of revision 1:9430a3a15f7a:
#include "mbed.h"
Serial pc(USBTX, USBRX); // leer comnunicacion serial
Serial pcBT(D8, D2); // tx, rx
AnalogIn analog_value(A0); // definimos puerto para leer analogamente el joystick
PwmOut myServo1(PC_7); // definir puertos de los servos
PwmOut myServo2(PB_4);
PwmOut myServo3(PB_5);
PwmOut myServo4(PB_8);
PwmOut myServo5(PA_6);
PwmOut myServo6(PB_9);
PwmOut myServo7(PA_8);
PwmOut myServo8(PA_7);
DigitalOut led(LED1,0); //led indicador de interrupcion
DigitalOut S0(PA_13); //definimos los puertos del sensor
DigitalOut S1(PA_14);
DigitalOut S2(PA_15);
DigitalOut S3(PC_8);
DigitalIn leer(PC_9); // puerto donde leemos la frecuencia que envia el sensor
Timer tiempo; // timer para uso del sensor
Timeout timeout; //creamos el objeto timeout
unsigned int lectura(); //respuesta de lectura de sensor
unsigned int inicio=0, final=0, resultado=0, rojo=0,verde=0,azul=0;// variables que se usan en los procesos
float meas_r; //variables del joystick
float meas_v;
void moving();
void grupomover();
void mover_ser(uint8_t nmotor, uint8_t pos);
void timer_interrupt();
void read_command(); // definimos los 4 numeros hexadecimales enviados
void read_commandbt(); // definimos los 4 numeros hexadecimales enviados
void adelante();
void atras();
void init_servo();
char init_c;
int read_cc;
uint32_t grados;
uint32_t nmotor;
int endc;
int main()
{
pc.baud(9600); // leer en el monitor serial
pc.printf("inicio\n");
timeout.attach(&timer_interrupt,0.5); // llamamos la interrupcion del timeout cada 0.5 segundos
pc.attach(&read_command, Serial::RxIrq); //llamamos la interrupcion serialmente
pcBT.attach(&read_commandbt, Serial::RxIrq);//llamamos la interrupcion serialmente
S0=1,S1=0; //calibracion de lectura del sensor
S2=0,S3=0;
init_servo();
while(1) {
wait(1);
pc.printf("modo joystick\n");
meas_r = analog_value.read(); // se lee la entrada analoga del joystick
meas_v = meas_r * 3300; // se convierte la variable a 3.3v
pc.printf("medida = %f = %.0f mV\n", meas_r, meas_v); //muestra en el monitor lectura del joystick
if (meas_v < 1000) {
pc.printf("adelante\n");
adelante();
}
if (meas_v > 3000) {
pc.printf("atras\n");
atras();
}
}
}
unsigned int lectura() // lectura del sensor
{
tiempo.start();
while(leer) {}
while(!leer) {} // inicia desde el inicio de cambio de flanco
//pc.printf("sale");
while(leer) {}
tiempo.reset();
inicio=tiempo.read_us();
while(!leer) {}
final=tiempo.read_us();
resultado=(final-inicio);
return resultado;
}
void init_servo() //inicializamos servomotores
{
myServo1.period_ms(20); // periodo de cada servo
myServo2.period_ms(20);
myServo3.period_ms(20);
myServo4.period_ms(20);
myServo5.period_ms(20);
myServo6.period_ms(20);
myServo7.period_ms(20);
myServo8.period_ms(20);
}
int coord2us(float coord)
{
if(0 <= coord <= 180)
return int(750+coord*1750/180);// cambiar de numero de entrada serial a grados
return 750;
}
void read_command()
{
init_c=pc.getc();
if( init_c==0xFF) {
read_cc=pc.getc();
nmotor=pc.getc();
grados=pc.getc();
endc=pc.getc();
if(endc==0xFD) {
switch (read_cc) {
case 0x01:
moving();
break;
case 0x02:
grupomover();
break;
default:
pc.printf("error \nSe espera 0xFEF0 o 0xFFF0 \n");
break ;
}
} else
pc.printf("Error de codigo de cierre \n");
} else
pc.printf("Error de codigo de inicio. \n");
}
void read_commandbt()
{
init_c=pc.getc();
if( init_c==0xFF) {
read_cc=pc.getc();
nmotor=pc.getc();
grados=pc.getc();
endc=pc.getc();
if(endc==0xFD) {
switch (read_cc) {
case 0x01:
moving();
break;
case 0x02:
grupomover();
break;
default:
pc.printf("error \nSe espera 0xFEF0 o 0xFFF0 \n");
break ;
}
} else
pc.printf("Error de codigo de cierre \n");
} else
pc.printf("Error de codigo de inicio. \n");
}
void moving()
{
pc.printf("mueve\n");
mover_ser(nmotor,grados); // llama el void para mover servo con grados
pc.printf("fin mueve\n");
}
void mover_ser(uint8_t nmotor, uint8_t grados) // definimos que leen los 8 servos y le asiganamos a cada variable un servo
{
int pulsea = coord2us(grados);
switch (nmotor) {
case 0x01:
myServo1.pulsewidth_us(pulsea);
break; // caso para cada servo
case 0x02:
myServo2.pulsewidth_us(pulsea);
break;
case 0x03:
myServo3.pulsewidth_us(pulsea);
break;
case 0x04:
myServo4.pulsewidth_us(pulsea);
break;
case 0x05:
myServo5.pulsewidth_us(pulsea);
break;
case 0x06:
myServo6.pulsewidth_us(pulsea);
break;
case 0x07:
myServo7.pulsewidth_us(pulsea);
break;
case 0x08:
myServo8.pulsewidth_us(pulsea);
break;
default:
pc.printf("no hay mas motores error de comando\n");
break ;
}
}
void timer_interrupt() //la respuesta de cada interrupcion externamente por timer
{
led = !led;
S2=0,S3=0;
rojo=lectura(); //pc.printf("rojo --%d\n",rojo);
S2=1,S3=1;
verde=lectura(); //pc.printf("verde --%d\n",verde);
S2=0,S3=1;
azul=lectura(); //pc.printf("azul --%d\n",azul);
if (rojo>=100 and rojo <=400) { // lee el rojo------------
if (verde>=450 and verde <=700) {
if (azul>=300 and azul <=600) {
//POSICION ORIGINAL
pc.printf("POSICION ORIGINAL --%d\n",rojo);
myServo1.pulsewidth_us(1025);
wait (0.5);
myServo2.pulsewidth_us(1711);
wait (0.5);
myServo1.pulsewidth_us(1412);
wait (0.5);
myServo7.pulsewidth_us(1025);
wait (0.5);
myServo8.pulsewidth_us(1319);
wait (0.5);
myServo7.pulsewidth_us(1417);
wait (0.5);
myServo2.pulsewidth_us(1515);
myServo8.pulsewidth_us(1515);
wait (0.5);
myServo3.pulsewidth_us(1221);
wait (0.5);
/* myServo2.pulsewidth_us(1800);
wait(0.5);
myServo8.pulsewidth_us(1600);
wait(0.5);
myServo4.pulsewidth_us(1400);
wait(0.5);
myServo6.pulsewidth_us(1600);
wait(0.5);
myServo1.pulsewidth_us(1200);
wait(0.5);
myServo3.pulsewidth_us(1200);
wait(0.5);
myServo5.pulsewidth_us(1200);
wait(0.5);
myServo7.pulsewidth_us(1200);
wait(1);*/
pc.printf("rojo --%d\n",rojo);
}
}
}
if (rojo>=200 and rojo <=450) { // lee el verde --------------------------------------
if (verde>=200 and verde <=450) {
if (azul>=200 and azul <=450) {
// GIRO A LA DERECHA
pc.printf("GIRO A LA DERECHA --%d\n",verde);
myServo1.pulsewidth_us(1200);
myServo3.pulsewidth_us(1200);
myServo5.pulsewidth_us(1200);
myServo7.pulsewidth_us(1200);
wait(0.5);
myServo1.pulsewidth_us(2000);
wait(0.5);
myServo2.pulsewidth_us(1000);
wait(0.5);
myServo1.pulsewidth_us(1200);
wait(0.5);
myServo7.pulsewidth_us(2000);
wait(0.5);
myServo8.pulsewidth_us(1000);
wait(0.5);
myServo7.pulsewidth_us(1200);
wait(0.5);
myServo5.pulsewidth_us(2000);
wait(0.5);
myServo6.pulsewidth_us(1000);
wait(0.5);
myServo5.pulsewidth_us(1200);
wait(0.5);
myServo3.pulsewidth_us(2000);
wait(0.5);
myServo4.pulsewidth_us(1000);
wait(0.5);
myServo3.pulsewidth_us(1200);
wait(0.5);
myServo2.pulsewidth_us(2200);
myServo6.pulsewidth_us(2200);
wait(0.5);
myServo4.pulsewidth_us(2200);
myServo8.pulsewidth_us(2200);
wait(1);
pc.printf("verde --%d\n",verde);
}
}
}
if (rojo>=400 and rojo <=800) { // lee el azul------------------------------------
if (verde>=200 and verde <=500) {
if (azul>=1 and azul <=400) {
// GIRO A LA IZQUIERDA
pc.printf("GIRO A LA IZQUIERDA --%d\n",azul);
myServo1.pulsewidth_us(1200);
myServo3.pulsewidth_us(1200);
myServo5.pulsewidth_us(1200);
myServo7.pulsewidth_us(1200);
wait(1);
myServo1.pulsewidth_us(2000);
wait(0.5);
myServo2.pulsewidth_us(2200);
wait(0.5);
myServo1.pulsewidth_us(1200);
wait(0.5);
myServo7.pulsewidth_us(2000);
wait(0.5);
myServo8.pulsewidth_us(2200);
wait(0.5);
myServo7.pulsewidth_us(1200);
wait(0.5);
myServo5.pulsewidth_us(2000);
wait(0.5);
myServo6.pulsewidth_us(2200);
wait(0.5);
myServo5.pulsewidth_us(1200);
wait(0.5);
myServo3.pulsewidth_us(2000);
wait(0.5);
myServo4.pulsewidth_us(2200);
wait(0.5);
myServo3.pulsewidth_us(1200);
wait(0.5);
myServo2.pulsewidth_us(1200);
myServo6.pulsewidth_us(1200);
wait(0.5);
myServo4.pulsewidth_us(1200);
myServo8.pulsewidth_us(1200);
wait(1);
pc.printf("azul --%d\n",azul);
}
}
}
timeout.attach(&timer_interrupt, 0.5); // reiniciamos el timeout para la siguiente lectura
}
void grupomover()
{
pc.printf("mueve grupo \n");
switch (nmotor) {
case 0x01: {
myServo1.pulsewidth_us(550);
myServo2.pulsewidth_us(1800);
pc.printf("primer caso\n");
break;
}
case 0x02: {
myServo3.pulsewidth_us(550);
myServo4.pulsewidth_us(1800);
pc.printf("segundo caso \n");
break ;
}
case 0x03: {
myServo5.pulsewidth_us(550);
myServo6.pulsewidth_us(1800);
pc.printf("tercer caso \n");
break;
}
case 0x04: {
myServo7.pulsewidth_us(500);
myServo8.pulsewidth_us(1800);
pc.printf("cuarto caso \n");
break;
default:
pc.printf("solo 4 casos error en comando\n");
break ;
}
}
pc.printf("fin mueve grupo\n");
}
void adelante()
{
// PASO DERECHA-DELANTERA
myServo8.pulsewidth_us(1000);
wait(0.5);
myServo7.pulsewidth_us(2000);
wait(0.5);
myServo8.pulsewidth_us(2000);
wait(0.5);
myServo7.pulsewidth_us(1200);
wait(0.5);
myServo8.pulsewidth_us(1000);
wait(0.5);
// PASO IZQUIERDA-TRASERA
myServo4.pulsewidth_us(1600);
wait(0.5);
myServo3.pulsewidth_us(2000);
wait(0.5);
myServo4.pulsewidth_us(800);
wait(0.5);
myServo3.pulsewidth_us(1200);
wait(0.5);
myServo4.pulsewidth_us(1600);
wait(0.5);
// PASO IZQUIERDA-DELANTERA
myServo2.pulsewidth_us(2400);
wait(0.5);
myServo1.pulsewidth_us(2000);
wait(0.5);
myServo2.pulsewidth_us(1200);
wait(0.5);
myServo1.pulsewidth_us(1200);
wait(0.5);
myServo2.pulsewidth_us(2400);
wait(0.5);
// PASO DERECHA-TRASERA
myServo6.pulsewidth_us(1500);
wait(0.5);
myServo5.pulsewidth_us(2000);
wait(0.5);
myServo6.pulsewidth_us(2300);
wait(0.5);
myServo5.pulsewidth_us(1200);
wait(0.5);
myServo6.pulsewidth_us(1500);
wait(0.5);
}
void atras()
{
// PASO DERECHA-DELANTERA
myServo8.pulsewidth_us(1000);
wait(0.5);
myServo7.pulsewidth_us(1200);
wait(0.5);
myServo8.pulsewidth_us(2000);
wait(0.5);
myServo7.pulsewidth_us(2000);
wait(0.5);
myServo8.pulsewidth_us(1000);
wait(0.5);
myServo7.pulsewidth_us(1200);
wait(0.5);
// PASO IZQUIERDA-TRASERA
myServo4.pulsewidth_us(1600);
wait(0.5);
myServo3.pulsewidth_us(1200);
wait(0.5);
myServo4.pulsewidth_us(800);
wait(0.5);
myServo3.pulsewidth_us(2000);
wait(0.5);
myServo4.pulsewidth_us(1600);
wait(0.5);
myServo3.pulsewidth_us(1200);
wait(0.5);
// PASO IZQUIERDA-DELANTERA
myServo2.pulsewidth_us(2400);
wait(0.5);
myServo1.pulsewidth_us(1200);
wait(0.5);
myServo2.pulsewidth_us(1200);
wait(0.5);
myServo1.pulsewidth_us(2000);
wait(0.5);
myServo2.pulsewidth_us(2400);
wait(0.5);
myServo1.pulsewidth_us(1200);
wait(0.5);
// PASO DERECHA-TRASERA
myServo6.pulsewidth_us(1500);
wait(0.5);
myServo5.pulsewidth_us(1200);
wait(0.5);
myServo6.pulsewidth_us(2300);
wait(0.5);
myServo5.pulsewidth_us(2000);
wait(0.5);
myServo6.pulsewidth_us(1500);
wait(0.5);
myServo5.pulsewidth_us(1200);
wait(0.5);
}