Ernesto Palacios
Proyecto de Tesis en Mecatrónica. Universidad Técnica del Norte. Ernesto Palacios <mecatronica.mid@gmail.com>
Dependencies: EthernetNetIf HTTPServer QEI_hw RPCInterface mbed
Revision 32:4483d6c225e5, committed 2014-05-07
- Comitter:
- Yo_Robot
- Date:
- Wed May 07 01:05:05 2014 +0000
- Parent:
- 31:7e2cdd547cb2
- Child:
- 33:e6ff02c3e0f5
- Commit message:
- Funciona falta arreglar el c?lculo de las RPM con el encoder
Changed in this revision
--- a/main.cpp Sat May 03 00:14:17 2014 +0000
+++ b/main.cpp Wed May 07 01:05:05 2014 +0000
@@ -27,9 +27,9 @@
Serial pc( USBTX, USBRX ); //Comunicacion Serial directa al computador
Serial RS_232(p13, p14); //Comunicacion Serial para MAX232
-QEIHW encoder( QEI_DIRINV_NONE, // CW = postive; CCW = negative
+QEIHW encoder( QEI_DIRINV_CMPL, // CW = postive; CCW = negative
QEI_SIGNALMODE_QUAD, // Absolute encoder
- QEI_CAPMODE_2X, // PhA anb PhB
+ QEI_CAPMODE_4X, // PhA anb PhB
QEI_INVINX_NONE // Not inverted index
);
@@ -75,17 +75,17 @@
RPCFunction ReadEnc(&getENC_eth, "ENC");
RPCFunction ClrEnc (&setENC_eth, "CLR");
RPCFunction ReadRPM(&getRPM_eth, "RPM");
-
+RPCFunction GoHome(&setHOME_eth, "INI");
int main() {
pin_alm.fall( &ISR_Alarm ); // Entrada de la alarma driver
- // limite_1.rise( &ISR_Alm_encoder ); // Alarma de colisión 1
- // limite_2.rise( &ISR_Adv_encoder ); // Alarma de colisión 1
- // limite_3.rise( &ISR_Adv_motor ); // Alarma de colisión 1
- // limite_4.rise( &ISR_Alm_motor ); // Alarma de colisión 1
+ limite_1.rise( &ISR_Alm_encoder ); // Alarma de colisión 1
+ limite_2.rise( &ISR_Adv_encoder ); // Alarma de colisión 1
+ limite_3.rise( &ISR_Adv_motor ); // Alarma de colisión 1
+ limite_4.rise( &ISR_Alm_motor ); // Alarma de colisión 1
encoder.SetDigiFilter(480UL); // number of clock cycles the signal must be at high to sense it
encoder.SetMaxPosition(0xFFFFFFFF); // max number before overflow - may be changed for 360 or a multiple
--- a/setup.cpp Sat May 03 00:14:17 2014 +0000
+++ b/setup.cpp Wed May 07 01:05:05 2014 +0000
@@ -37,9 +37,11 @@
int fq_actual = 0; // Ultimo valor seteado para el tren de pulsos
+int bandera_inicio = 0;
+
int velocidad_rpm()
{
- return encoder.GetPosition();
+ return encoder.CalculateRPM( encoder.GetVelocityCap(), 360 );
}
void clear_encoder()
@@ -108,6 +110,7 @@
{
setPTO( value );
fq_actual = value;
+ bandera_inicio = 0; //En caso que haya estado yendo a HOME, cancelar
// Envia un OK de comando recibido
if( isPC )
@@ -121,6 +124,8 @@
{
setPTO( value * 1000 );
fq_actual = value;
+ bandera_inicio = 0; //En caso que haya estado yendo a HOME, cancelar
+
// Envia un OK de comando recibido
if( isPC )
pc.printf("OK\r\n");
@@ -147,6 +152,7 @@
stopTimer2();
pin_dir = value;
wait_us( 2 );
+ bandera_inicio = 0; //En caso que haya estado yendo a HOME, cancelar
if ( fq_actual != 0 )
{
@@ -180,8 +186,10 @@
float pulsos = value; //Numero de pulsos a generar
t_alto = (float)(pulsos / fq_posicion); //Tiempo que debe ser generado el tren de pulsos.
+ bandera_inicio = 0; //En caso que haya estado yendo a HOME, cancelar
+
//DEBUG
- pc.printf("Tiempo en timer en seg = %f", t_alto);
+ //pc.printf("Tiempo en timer en seg = %f", t_alto);
stopTimer2(); //Deten el tren de pulsos
setPTO( fq_posicion ); //Nueva frecuencia de salida
@@ -205,9 +213,10 @@
{
float pulsos = value * 1000; //Numero de pulsos a generar
t_alto = (float)(pulsos / fq_posicion); //Tiempo que debe ser generado el tren de pulsos.
-
+ bandera_inicio = 0; //En caso que haya estado yendo a HOME, cancelar
+
//DEBUG
- pc.printf("Tiempo en timer en seg = %f", t_alto);
+ //pc.printf("Tiempo en timer en seg = %f", t_alto);
stopTimer2(); //Deten el tren de pulsos
setPTO( fq_posicion ); //Nueva frecuencia de salida
@@ -231,9 +240,10 @@
{
float pulsos = value * 1000000; //Numero de pulsos a generar
t_alto = (float)(pulsos / fq_posicion); //Tiempo que debe ser generado el tren de pulsos.
-
+ bandera_inicio = 0; //En caso que haya estado yendo a HOME, cancelar
+
//DEBUG
- pc.printf("Tiempo en timer en seg = %f", t_alto);
+ //pc.printf("Tiempo en timer en seg = %f", t_alto);
stopTimer2(); //Deten el tren de pulsos
setPTO( fq_posicion ); //Nueva frecuencia de salida
@@ -277,6 +287,21 @@
case 'S': //Encender el Servo
pin_son = value;
+ bandera_inicio = 0; //En caso que haya estado yendo a HOME, cancelar
+
+ // Envia un OK de comando recibido
+ if( isPC )
+ pc.printf("OK\r\n");
+ else
+ RS_232.printf("OK\r\n");
+
+ break;
+
+ case 'I': //Ir al inicio del recorrido
+
+ pin_dir = 1; //Mover hacia el motor
+ bandera_inicio = 1; // Informar a ISR_Adv_motor() acerca del Homing
+ setPTO( value ); // a la velocidad seteada
// Envia un OK de comando recibido
if( isPC )
@@ -295,6 +320,7 @@
RS_232.printf("NA\r\n");
break;
+
}
}
@@ -340,9 +366,9 @@
void ISR_Alm_encoder()
{
if( isPC )
- pc.printf("A0\r\n");
+ pc.printf("A3\r\n");
else
- RS_232.printf("A0\r\n");
+ RS_232.printf("A3\r\n");
}
@@ -351,10 +377,14 @@
*/
void ISR_Alm_motor()
{
- if( isPC )
- pc.printf("A0\r\n");
- else
- RS_232.printf("A0\r\n");
+ wait_us( 50 );
+ if ( limite_4 == 1)
+ {
+ if( isPC )
+ pc.printf("A0\r\n");
+ else
+ RS_232.printf("A0\r\n");
+ }
}
@@ -363,11 +393,15 @@
*/
void ISR_Adv_encoder()
{
+ wait_ms( 50 );
+ if ( limite_2 == 1)
+ {
- if( isPC )
- pc.printf("A2\r\n");
- else
- RS_232.printf("A2\r\n");
+ if( isPC )
+ pc.printf("A2\r\n");
+ else
+ RS_232.printf("A2\r\n");
+ }
}
@@ -377,10 +411,41 @@
*/
void ISR_Adv_motor()
{
- if( isPC )
- pc.printf("A1\r\n");
- else
- RS_232.printf("A1\r\n");
+ if ( bandera_inicio == 1 )
+ {
+ setPTO( 0 ); //detener el carro
+ pin_dir = 0; //hacer que se aleje del motor
+
+
+ stopTimer2(); //Deten el tren de pulsos
+ setPTO( 20000 ); //Nueva frecuencia de salida
+ startTimer2(); //Inicia el tren de pulsos
+ wait_ms( 100 ); //Espera hasta llegar a la posicion
+ stopTimer2(); //Posicion alcanzada ALTO.
+
+ setPTO (0); // Detener el carro cuando este en posición valida
+ wait_ms(100);
+ //Encerar el contador de encoder y de velocidad
+ encoder.Reset(QEI_RESET_POS);
+ encoder.Reset(QEI_RESET_VEL);
+
+ bandera_inicio = 0; //Limpiar la bandera
+
+ // Envia un IN de proceso terminado
+ if( isPC )
+ pc.printf("IN\r\n");
+ else
+ RS_232.printf("IN\r\n");
+ }
+
+ else{
+
+ if( isPC )
+ pc.printf("A1\r\n");
+ else
+ RS_232.printf("A1\r\n");
+ }
+
}
@@ -754,6 +819,42 @@
}
+void setHOME_eth( char * input, char * output )
+{
+ int value = atoi( input );
+ pin_dir = 1; //Mover hacia el motor
+ bandera_inicio = 1; // Informar a ISR_Adv_motor() acerca del Homing
+ setPTO( value * 1000 ); // a la velocidad seteada en KiloHertzios
+
+ if( pin_alm == 0 )
+ {
+ if ( limite_2 == 1 ) // Alarma muy cerca al encoder
+ {
+ sprintf( output,"A2\r\n" );
+ }
+
+ if ( limite_3 == 1 ) // Alarma muy cerca al encoder
+ {
+ if (bandera_inicio == 1)
+ {
+ sprintf( output,"IN\r\n" );
+ }
+ sprintf( output,"A1\r\n" );
+ }
+ if ( limite_1 == 1 || limite_4 == 1 ) // ERROR DE POSICION
+ {
+ sprintf( output,"A0\r\n" );
+ }
+
+ else
+ sprintf( output,"OK\r\n" );
+ }
+ else
+ sprintf( output,"AL" );
+
+
+}
+
--- a/setup.h Sat May 03 00:14:17 2014 +0000 +++ b/setup.h Wed May 07 01:05:05 2014 +0000 @@ -160,6 +160,11 @@ void getRPM_eth( char * input, char * output ); +/** @brief: Lleva el carro hasta el inicio del recorrido y encera el encoder + */ +void setHOME_eth( char * input, char * output ); + + //________________ FUNCIONES ENCODER __________________// /** @brief: Cambiar Direccion @ PTO