Pedro Campos
test
Diff: main.cpp
- Revision:
- 2:82fdfeec5799
- Parent:
- 1:da6afd392792
- Child:
- 3:55227f9877e0
--- a/main.cpp Mon Apr 20 11:42:55 2020 +0000
+++ b/main.cpp Wed May 27 07:50:09 2020 +0000
@@ -1,8 +1,8 @@
-#include "mbed.h"
+#include "declaraciones.h"
#include "EthernetInterface.h"
+#include "gps.h"
-
-#define IP "192.168.1.227"
+#define IP "192.168.1.247"
#define GATEWAY "192.168.1.1"
#define MASK "255.255.255.0"
@@ -10,10 +10,13 @@
// Network interface
EthernetInterface net;
+
+//GPS gps(PC_10, PC_11, PF_8);
+
// CANALES DE COMUNICACION
Serial pc(SERIAL_TX, SERIAL_RX, 115200);
Serial serGPS(PC_10, PC_11, 9600);
-Serial serSERVO(PC_12, PD_2, 115200);
+RawSerial serSERVO(PC_12, PD_2, 115200);
Serial serRS485(PD_5, PD_6, 115200);
// SALIDAS PARA LEDS
@@ -22,8 +25,8 @@
DigitalOut led3(LED3, 0);
// SALIDAS DE CONTROL COMUNICACIONES
-DigitalOut RS485_DIR(PE_3, 0); //DIR RS485 a RX
-DigitalOut RS485_OE(PF_7, 0); //OE RS485 a tri-state
+DigitalOut RS485_DIR(PE_3, 0); //DIR RS485 a RX
+DigitalOut RS485_OE(PF_7, 0); //OE RS485 a tri-state
// SALIDAS PARA CONTROL DEL SERVO
DigitalOut control_PWR_servo(PF_9, 1); // Alimentacion del inverter para 220V del servo
@@ -44,6 +47,12 @@
DigitalIn SRV_D_VEL(PF_2); //Entrada desde SERVO signo nivel de velocidad 1=positivo
DigitalIn SRV_D_TRQ(PF_1); //Entrada desde SERVO signo nivel de torque 1=positivo
+// ENTRADAS DEL SELECTOR
+DigitalIn SEL0(PC_8); //Selector 1 = BABOR/ESTRIBOR, ON(0)=BABOR OFF(1)=ESTRIBOR
+DigitalIn SEL1(PC_9); //Selector 2
+DigitalIn SEL2(PG_2); //Selector 3
+DigitalIn SEL3(PG_3); //Selector 4
+
// SALIDAS PWM
PwmOut SRV_C_TRQ_P(PD_13); // Salida PWM para control del torque POSITIVO del SERVO
PwmOut SRV_C_TRQ_N(PA_0); // Salida PWM para control del torque NEGATIVO del SERVO
@@ -58,188 +67,302 @@
-
-long tiempo_ms = 0;
-long tiempo = 0;
-
-float velocidad = 0.0;
-float torque = 0.0;
+// VARIABLES DE SISTEMA
+volatile unsigned long tiempo_ms = 0;
+volatile bool f_ms = false; // se pone en cada interrupcion de ms
-Timer timer;
+long t_ult_recepcion; // guarda tiempo ultima recepción
+byte nMando; // numero de mando 0=babor 1=estribor
+int servo; // servo recibido en ultima comunicacion
+volatile float latitud;
+volatile float longitud;
+volatile long fecha_hora;
+volatile int n_satelites;
+volatile float HDOP;
+volatile int rumbo;
+volatile int velocidad; // MNPH
+volatile int timon_babor;
+volatile int timon_estribor;
+volatile enEstadoDireccion estado_direccion;
+volatile byte EstadoAnteriorServo; //Guarda estado anterior del servo para salir de piloto-automatico o rumbo
+volatile byte estado_otro_servo; // Estado del otro servo
+volatile int retardoActualizacion; // para no visualizar estado durante este tiempo en ms
+volatile bool act_display; // flag para actualizar display
+volatile bool trama_estado_valida;
+volatile bool trama_posicion_valida;
-Ticker flipper;
+// VARIABLE CONTROL SERVO
+enEstadoServo estado_servo;
+int16_t velocidad_servo;
+int16_t torque_servo;
+long desviacion_servo;
+volatile int pos_actual_servo = 0;
+volatile int pos_objetivo_servo = 0;
+volatile unsigned long tempo_espera = 0;
+
+int torque_max; // torque máximo calculado
+int out_torque = TORQUE_MINIMO; // nivel de torque de TORQUE_MINIMO a TORQUE_MAXIMO
+
+int cons_timon_babor = 0; // consigna % de timon babor bajado
+int cons_timon_estribor = 0; // consigna % de timon estribor bajado
+
+float diferencia_rumbo = 0.0; // Diferencia al Rumbo deseado
-
-void flip() {
- led2 = !led2;
- led3 = !led3;
+st_datos_servo datos_servo; // ESTRUCTURA DE VARIABLES DE ESTADO DEL SERVO DE DIRECCION
+st_datos_GPS datos_GPS; // ESTRUCTURA DE DATOS DE ESTADO DEL GPS
+
+
+// VARIABLES CONFIGURACION DESDE ORDENADOR
+int cfg_hora = 7200; // Suma dos horas a UTC
+
+
+// VARIABLES GENERALES
+float ana_velocidad = 0.0;
+float ana_torque = 0.0;
+bool newtime = false;
+ntp_packet in_data; // para recibir fecha y hora del servidor
+
+// VARIABLES DE COMUNICACION RS485
+volatile bool f_rs485_busy = false; // comunicacion ocupada
+
+// TIMERS
+Timer timer;
+Ticker millis; // Interrupcion para cuenta de milisegundos
+Ticker tick250us; // Interrupcion de 250us para com rs485
+
+
+
+
+
+
+
+
+// RUTINAS
+
+// CUENTA MILISEGUNDOS DESDE RESET
+void cuenta_ms() {
+ tiempo_ms++;
+ posicion_servo(); // AJUSTA POSICION SERVO SI ES NECESARIO
+ f_ms = true; // se pone en cada interrupcion de ms
}
-// Time protocol implementation : Address: time.nist.gov UDPPort: 37
-
-typedef struct {
- uint32_t secs; // Transmit Time-stamp seconds.
-}ntp_packet;
-/////////////////////////////////////////////////////////
-// PROCESA RECEPCION DESDE GPS
-void procesa_recepcion(void){ // Procesa recepcion GPS
/*
- while (serGPS.readable()) {
- char c = serGPS.getc();
- pc.putc(c);
- }
-
- while (serSERVO.readable()) {
- char c = serSERVO.getc();
- pc.putc(c);
- }
-*/
-}
-
-
/////////////////////////////////////////////////////////
// PROCESA RECEPCION DESDE SERIAL SERVO
void onSerialServo(void){ // Procesa recepcion SERVO
while (serSERVO.readable()) {
char c = serSERVO.getc();
- pc.putc(c);
+// pc.putc(c);
+ }
+}
+*/
+
+
+
+
+
+#define FACTOR_INC_RUMBO_BABOR 0.015
+#define FACTOR_INC_RUMBO_ESTRIBOR 0.015
+
+// PARA SIMULACION
+// Pocesa rumbo_GPS, llamada cada 250ms
+void procesa_rumbo_GPS(void){
+
+ datos_servo.rumbo -= datos_servo.timon_babor * FACTOR_INC_RUMBO_BABOR;
+ datos_servo.rumbo += datos_servo.timon_estribor * FACTOR_INC_RUMBO_ESTRIBOR;
+ // datos_servo.rumbo = flimit_decimales(datos_servo.rumbo, 2);
+ if(datos_servo.rumbo>360.0){
+ datos_servo.rumbo -= 360.0;
+ }
+ else if(datos_servo.rumbo<0.0){
+ datos_servo.rumbo += 360.0;
}
}
/////////////////////////////////////////////////////////
-// PROCESA RECEPCION DESDE SERIAL RS485
-void onSerialRS485(void){ // Procesa recepcion RS485
-
- while (serRS485.readable()) {
- char c = serRS485.getc();
- pc.putc(c);
- }
-}
+// PROGRAMA PRINCIPAL
+int main() {
+int nc=0;
+char out_buffer_2[50];
+static int posicion_mando_old = 0;
-
-int main() {
-
// Bring up the ethernet interface
+ pc.printf("\r\n\r\n");
+ pc.printf("RESET\r\n\r\n");
pc.printf("PROGRAMA CONTROL SERVO V0.1\r\n");
pc.printf("PRUEBAS DE COMUNICACION POR UDP\r\n");
- net.disconnect();
- int j = net.set_network(IP,MASK,GATEWAY);
- printf("set IP status: %i \r\n",j);
+ // Open Ethernet connection
+ int c=0;
+ do{
+ net.disconnect();
+ wait(2.0); // espera de 1s
+ int j = net.set_network(IP, MASK, GATEWAY);
+ printf("set IP status: %i\r\n", j);
- if(0 != net.connect()) {
- printf("Error connecting\r\n");
- return -1;
- }
+ c=net.connect();
+ if(0 != c) {
+ printf("Error connecting\r\n");
+ // return -1;
+ }
+ }while(c != 0);
// Show the network address
const char *ip = net.get_ip_address();
printf("IP address is: %s\r\n", ip ? ip : "No IP");
-
- UDPSocket sock(&net);
- SocketAddress sockAddr;
+
+ Thread t; // can pass in the stack size here if you run out of memory
+ t.start(&udp_main);
+
+
+ // Inicializa datos_servo ESTRUCTURA DE VARIABLES DE ESTADO DEL SERVO DE DIRECCION
+ datos_servo.n_servo = SEL0.read();
+ datos_servo.estado = 0;
+ datos_servo.latitud = 0.0;
+ datos_servo.longitud = 0.0;
+ datos_servo.fecha_hora = 0;
+ datos_servo.velocidad = 0.0; // MNPH
+ datos_servo.rumbo = 0.0;
+ datos_servo.ch_radio = 0;
+ datos_servo.trim_timon_bab = 0;
+ datos_servo.trim_trans_bab = 6;
+ datos_servo.trim_timon_estr = 0;
+ datos_servo.trim_trans_estr = 6;
+ datos_servo.timon_babor = 0; // %de timon babor bajado, para 100% 5s, 1% = 50ms
+ datos_servo.timon_estribor = 0; // %de timon estribor bajado, para 100% 5s
+ datos_servo.rumbo_fijado = 0.0; // Rumbo fijado al pulsar botón para RUMBO FIJO
+ datos_servo.rumbo_piloto_auto = 0.0; // Rumbo fijado por piloto automático
+
+ // Set handler to be called when UDP socket event occurrs.
+ // sock.attach(&onUDPSocketEvent);
timer.start();
-
- led2 = 1;
- led3 = 0;
- flipper.attach(&flip, 1.0); // the address of the function to be attached (flip) and the interval (2 seconds)
+ millis.attach(&cuenta_ms, 0.001); // the address of the function to be attached (cuenta_ms) and the interval (1 miliseconds)
+ tick250us.attach(&int250us, 0.0001); // Interrupcion de 250us para comunicaciones por RS485 con mando
+ led1 = led2 = led3 =1;
wait(0.5);
-
- serSERVO.attach(&onSerialServo); // Interrupcion de serial Servo
- serRS485.attach(&onSerialRS485); // Interrupcion de serial RS485 (MANDOS)
+ led1 = led2 = led3 =0;
+
+// serSERVO.attach(&onSerialServo); // Interrupcion de serial Servo
+// serRS485.attach(&onSerialRxRS485, RxIrq); // Interrupcion de RX serial RS485 (MANDOS)
+// serRS485.attach(&onSerialTxRS485, Serial::TxIrq); // Interrupcion de TX serial RS485 (MANDOS)
+
+ serGPS.attach(&onSerialGPS); // Interrupcion de serial GPS
RS485_OE = 1; // habilita conversor 3.3V a 5V para RS485
+/*
SRV_C_TRQ_P.period(0.001f); // 1 kHz or 1 ms
SRV_C_TRQ_N.period(0.001f); // 1 kHz or 1 ms
SRV_C_TRQ_P=0;
SRV_C_TRQ_N=0;
-
+*/
+
+ SERVO_ON = 1; // Pone servo en ON
+
+// uint8_t i=0;
+
+// SetDateTime(2020, 4, 25, 10, 00, 0); // Actualiza hora del RTC
+// ShowDateTime();
-// uint8_t i=0;
while(1){
- if(timer.read_us()>=250){
- procesa_recepcion(); // Procesa recepcion GPS
- tiempo++;
- timer.reset();
- if(tiempo%4000==0){ // Envia cada segundo
-// serSERVO.printf("PRUEBA DE COMUNICACION CON EL SERVO\r\n");
- RS485_DIR = 1; // pone en TX
- wait(0.1);
- serRS485.printf("PRUEBA DE COMUNICACION POR RS485\r\n");
- wait(0.1);
- RS485_DIR = 0; // pone en RX
+ while(f_ms!=true); // espera interrupcion de 1 ms
+ f_ms = false;
+
+ procesaGPS(); // Comprueba trama pendiente de GPS y la procesa
-// pc.printf("RDY:%u AL:%u POS:%u TORQ_L:%u Z_SPEED:%u \r\n", SERVO_RDY_IN.read(), SERVO_AL_IN.read(), SERVO_POS_IN.read(), SERVO_TORQ_L_IN.read(), SERVO_Z_SPEED_IN.read());
- pc.printf("velocidad:%3.3f torque:%3.3f\r\n", velocidad, torque);
-
+ if(datos_servo.estado == DIR_MASTER || datos_servo.estado == DIR_REPOSO)
+ datos_servo.posicion_mando = desviacion_servo;
+ else
+ datos_servo.posicion_mando = pos_objetivo_servo; // PARA PILOTO AUTOMATICO
+
+ if(datos_servo.posicion_mando != posicion_mando_old){ // Si cambia la posicion del mando envia trama
+ if(abs(desviacion_servo) >= 50 && datos_servo.estado == DIR_REPOSO && estado_otro_servo == DIR_REPOSO){
+ datos_servo.estado = DIR_MASTER;
}
- if(tiempo%40 == 0){
- velocidad = SRV_VEL.read(); // 1.0=VCC
- torque = SRV_TRQ.read();
- SRV_C_TRQ_P.pulsewidth(velocidad*0.001f);
- SRV_C_TRQ_N.pulsewidth(torque*0.001f);
- }
- }
-
- led1 = ((tiempo/4000)%2 == 0)?1:0;
+
+ if(datos_servo.estado == DIR_MASTER || datos_servo.estado == DIR_RUMBO || datos_servo.estado == DIR_PILOTO)
+ envia_posicion_mando(datos_servo.posicion_mando);
+ posicion_mando_old = datos_servo.posicion_mando;
+ }
+
+ // PROCESA CADA SEGUNDO
+ if(((tiempo_ms+500)%1000)==0){ // Envia cada segundo
+
+// getTime();
+
+// SetDateTime(2020, 4, 25, 10, 00, 0); // Actualiza hora del RTC
+// ShowDateTime();
+// pc.printf("tiempo_ms:%d\r\n", tiempo_ms);
+
+// pc.printf("RDY:%u AL:%u POS:%u TORQ_L:%u Z_SPEED:%u \r\n", SERVO_RDY_IN.read(), SERVO_AL_IN.read(), SERVO_POS_IN.read(), SERVO_TORQ_L_IN.read(), SERVO_Z_SPEED_IN.read());
+// pc.printf("velocidad:%3.3f torque:%3.3f fix:%d satelites:%s\r\n", ana_velocidad, ana_torque, gps.have_fix, gps.NmeaGpsData.NmeaSatelliteTracked);
+
+// procesa_servo(); // lee estado del servo, procesa torque y actualiza
+// envia_posicion_mando(datos_servo.posicion_mando);
+
+// printf("tiempo_ms=%d pos_objetivo_servo=%d SEL0=%d ESTADO=%d\r\n", tiempo_ms, pos_objetivo_servo, SEL0.read(), datos_servo.estado);
+ }
- SERVO_INC_POS = led2;
- SERVO_DEC_POS = led3;
+/*
+ //PROCESO CADA 10ms
+ if((tiempo_ms%10) == 0){
+ ana_velocidad = SRV_VEL.read(); // 1.0=VCC, lee entradas analogicas
+ ana_torque = SRV_TRQ.read();
+ SRV_C_TRQ_P.pulsewidth(ana_velocidad*0.001f); // Ajusta PWM de salida DAC
+ SRV_C_TRQ_N.pulsewidth(ana_torque*0.001f);
+ }
+*/
+ //PROCESO CADA 100ms
+ if((tiempo_ms%100) == 0){
+// procesa_rumbo_GPS(); // SIMULA RUMBO SEGUN TIMON PARA GPS
+// procesa_posicion_timones(); // Pocesa activación y posición timones, llamada cada 100ms
+ }
- SERVO_ON = led1; // Salida de control ON del servo (J7-2)
- SERVO_CLR_COUNT = led2; // Salida de control CLR_COUNT del servo (J7-1)
- SERVO_ALARM_CLR = led3; // Salida de control ALARM_CLR del servo (J7-4)
- SERVO_CONTROL_MODE = led1; // Salida de control CONTROL_MODE del servo (J7-3)
- SERVO_AUX_OUT = led2; // Salida de control AUX_OUT del servo (J7-10)
+ //PROCESO CADA 250ms
+ if((tiempo_ms%250) == 0){
+ procesa_servo(); // lee estado del servo, procesa torque y actualiza
+ procesa_posicion_timones(); // Pocesa activación y posición timones, llamada cada 250ms
+ actualiza_timones(); // ACTUALIZA TIMONES
+ procesa_rumbo_GPS(); // SIMULA RUMBO SEGUN TIMON PARA GPS
+ envia_estado_mando(); // cada 250ms
+ }
// control_PWR_servo = (pulsador.read() == 1)?0:1;
control_PWR_servo = 0; // encendido
-
- if(tiempo >= 40000){
- tiempo = 0;
+
+ // PROCESO CADA 10 SEGUNDOS
+ if((tiempo_ms%10000)==0){
- char out_buffer[] = "time";
- char out_buffer_2[50];
- int nc=0;
- if(0 > sock.sendto("time.nist.gov", 37, out_buffer, sizeof(out_buffer))) {
- printf("Error sending data\r\n");
- return -1;
- }
-
- ntp_packet in_data;
- int n = sock.recvfrom(&sockAddr, &in_data, sizeof(ntp_packet));
- in_data.secs = ntohl( in_data.secs ) - 2208988800; // 1900-1970
- printf("\r\n\r\nTime Received %lu seconds since 1/01/1900 00:00 GMT\r\n",
- (uint32_t)in_data.secs);
- nc = sprintf(out_buffer_2, "%s", ctime(( const time_t* )&in_data.secs) );
- printf("Time = %s\r\n", out_buffer_2);
-
- printf("Time Server Address: %s Port: %d\r\n\r\n",
- sockAddr.get_ip_address(), sockAddr.get_port());
-
- if(0 > sock.sendto("192.168.1.37", 6000, out_buffer_2, nc)) {
- printf("\r\nError sending data 2\r\n\r\n");
- return -1;
- }
+/*
+ if(pos_objetivo_servo == 0){
+ pos_objetivo_servo = 200;
+ escribe_parametro_servo(0, 13, TORQUE_MAXIMO);
+ }
+ else if(pos_objetivo_servo == 200){
+ pos_objetivo_servo = -200;
+ escribe_parametro_servo(0, 13, TORQUE_MAXIMO);
+ }
+ else if(pos_objetivo_servo == -200){
+ pos_objetivo_servo = 0;
+ escribe_parametro_servo(0, 13, TORQUE_MINIMO);
+ }
+ // printf("\r\n\r\npos_objetivo_servo=%d\r\n\r\n",pos_objetivo_servo);
+ */
}
// Thread::wait(10000);
}
- // Close the socket and bring down the network interface
- sock.close();
- net.disconnect();
return 0;
}