Pedro Campos / Mbed OS SERVOS_V0_3

test

main.cpp@7:60d157ef1134, 2021-07-27 (annotated)

Committer:
dapi08
Date:
Tue Jul 27 14:03:30 2021 +0000
Revision:
7:60d157ef1134
Parent:
6:c21017e969af
Child:
8:b4465148d206
Operativa, inicio incluir deteccion de centro volante con opto;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dapi08 7:60d157ef1134 1 /****************************************************
dapi08 7:60d157ef1134 2 CONTROL SERVOS PARA BARCO
dapi08 7:60d157ef1134 3 (C)2021 Pedro M. Campos
dapi08 7:60d157ef1134 4 -210726 En pruebas para instalación
dapi08 7:60d157ef1134 5
dapi08 7:60d157ef1134 6 *****************************************************/
dapi08 7:60d157ef1134 7
dapi08 7:60d157ef1134 8
dapi08 7:60d157ef1134 9
dapi08 2:82fdfeec5799 10 #include "declaraciones.h"
deepikabhavnani 0:cf516d904427 11 #include "EthernetInterface.h"
dapi08 2:82fdfeec5799 12 #include "gps.h"
dapi08 1:da6afd392792 13
dapi08 2:82fdfeec5799 14 #define IP "192.168.1.247"
dapi08 1:da6afd392792 15 #define GATEWAY "192.168.1.1"
dapi08 1:da6afd392792 16 #define MASK "255.255.255.0"
dapi08 1:da6afd392792 17
deepikabhavnani 0:cf516d904427 18
deepikabhavnani 0:cf516d904427 19 // Network interface
deepikabhavnani 0:cf516d904427 20 EthernetInterface net;
dapi08 1:da6afd392792 21
dapi08 2:82fdfeec5799 22
dapi08 2:82fdfeec5799 23 //GPS gps(PC_10, PC_11, PF_8);
dapi08 2:82fdfeec5799 24
dapi08 1:da6afd392792 25 // CANALES DE COMUNICACION
dapi08 1:da6afd392792 26 Serial pc(SERIAL_TX, SERIAL_RX, 115200);
dapi08 1:da6afd392792 27 Serial serGPS(PC_10, PC_11, 9600);
dapi08 2:82fdfeec5799 28 RawSerial serSERVO(PC_12, PD_2, 115200);
dapi08 1:da6afd392792 29 Serial serRS485(PD_5, PD_6, 115200);
dapi08 1:da6afd392792 30
dapi08 1:da6afd392792 31 // SALIDAS PARA LEDS
dapi08 1:da6afd392792 32 DigitalOut led1(LED1, 0); //LED1
dapi08 1:da6afd392792 33 DigitalOut led2(LED2, 0);
dapi08 1:da6afd392792 34 DigitalOut led3(LED3, 0);
dapi08 1:da6afd392792 35
dapi08 1:da6afd392792 36 // SALIDAS DE CONTROL COMUNICACIONES
dapi08 2:82fdfeec5799 37 DigitalOut RS485_DIR(PE_3, 0); //DIR RS485 a RX
dapi08 2:82fdfeec5799 38 DigitalOut RS485_OE(PF_7, 0); //OE RS485 a tri-state
dapi08 1:da6afd392792 39
dapi08 1:da6afd392792 40 // SALIDAS PARA CONTROL DEL SERVO
dapi08 1:da6afd392792 41 DigitalOut control_PWR_servo(PF_9, 1); // Alimentacion del inverter para 220V del servo
dapi08 1:da6afd392792 42 DigitalOut SERVO_ON(PG_1, 0); // Salida de control ON del servo (J7-2)
dapi08 1:da6afd392792 43 DigitalOut SERVO_CLR_COUNT(PG_0, 0); // Salida de control CLR_COUNT del servo (J7-1)
dapi08 1:da6afd392792 44 DigitalOut SERVO_ALARM_CLR(PF_0, 0); // Salida de control ALARM_CLR del servo (J7-4)
dapi08 1:da6afd392792 45 DigitalOut SERVO_CONTROL_MODE(PC_6, 0); // Salida de control CONTROL_MODE del servo (J7-3)
dapi08 1:da6afd392792 46 DigitalOut SERVO_INC_POS(PB_10, 0); // Salida de control INC_POS del servo (J1-2)
dapi08 1:da6afd392792 47 DigitalOut SERVO_DEC_POS(PB_11, 0); // Salida de control DEC_POS del servo (J1-3)
dapi08 1:da6afd392792 48 DigitalOut SERVO_AUX_OUT(PA_15, 0); // Salida de control AUX_OUT del servo (J7-10)
dapi08 1:da6afd392792 49
dapi08 1:da6afd392792 50 // ENTRADAS DE ESTADO DEL SERVO
dapi08 1:da6afd392792 51 DigitalIn SERVO_RDY_IN(PF_13); //Entrada desde SERVO_RDY, 1=activa;
dapi08 1:da6afd392792 52 DigitalIn SERVO_AL_IN(PE_9); //Entrada desde SERVO_AL, 1=activa;
dapi08 1:da6afd392792 53 DigitalIn SERVO_POS_IN(PE_11); //Entrada desde SERVO_POS, 1=activa;
dapi08 1:da6afd392792 54 DigitalIn SERVO_TORQ_L_IN(PF_14); //Entrada desde SERVO_TORQ_L, 1=activa;
dapi08 1:da6afd392792 55 DigitalIn SERVO_Z_SPEED_IN(PE_13); //Entrada desde SERVO_Z_SPEED, 1=activa;
dapi08 1:da6afd392792 56 DigitalIn SRV_D_VEL(PF_2); //Entrada desde SERVO signo nivel de velocidad 1=positivo
dapi08 1:da6afd392792 57 DigitalIn SRV_D_TRQ(PF_1); //Entrada desde SERVO signo nivel de torque 1=positivo
dapi08 1:da6afd392792 58
dapi08 2:82fdfeec5799 59 // ENTRADAS DEL SELECTOR
dapi08 2:82fdfeec5799 60 DigitalIn SEL0(PC_8); //Selector 1 = BABOR/ESTRIBOR, ON(0)=BABOR OFF(1)=ESTRIBOR
dapi08 2:82fdfeec5799 61 DigitalIn SEL1(PC_9); //Selector 2
dapi08 2:82fdfeec5799 62 DigitalIn SEL2(PG_2); //Selector 3
dapi08 2:82fdfeec5799 63 DigitalIn SEL3(PG_3); //Selector 4
dapi08 2:82fdfeec5799 64
dapi08 1:da6afd392792 65 // SALIDAS PWM
dapi08 1:da6afd392792 66 PwmOut SRV_C_TRQ_P(PD_13); // Salida PWM para control del torque POSITIVO del SERVO
dapi08 1:da6afd392792 67 PwmOut SRV_C_TRQ_N(PA_0); // Salida PWM para control del torque NEGATIVO del SERVO
dapi08 1:da6afd392792 68
dapi08 1:da6afd392792 69 // ENTRADAS ANALOGICAS
dapi08 1:da6afd392792 70 AnalogIn SRV_VEL(PA_3); // Entrada analogica de VELOCIDAD del servo
dapi08 1:da6afd392792 71 AnalogIn SRV_TRQ(PC_0); // Entrada analogica de TORQUE del servo
dapi08 1:da6afd392792 72
dapi08 1:da6afd392792 73
dapi08 1:da6afd392792 74 // ENTRADAS GENERALES
dapi08 1:da6afd392792 75 DigitalIn pulsador(PC_13); //Pulsador de usuario, 1=pulsado;
dapi08 7:60d157ef1134 76 DigitalIn centrado(PC_3); //Señal del opto de volante centrado 1=centrado;
dapi08 1:da6afd392792 77
dapi08 1:da6afd392792 78
dapi08 2:82fdfeec5799 79 // VARIABLES DE SISTEMA
dapi08 2:82fdfeec5799 80 volatile unsigned long tiempo_ms = 0;
dapi08 2:82fdfeec5799 81 volatile bool f_ms = false; // se pone en cada interrupcion de ms
dapi08 4:7564d0ffd595 82 int babor_estribor = 0; // si 0=control servo babor, si 1=control servo estribor
dapi08 1:da6afd392792 83
dapi08 1:da6afd392792 84
dapi08 2:82fdfeec5799 85 long t_ult_recepcion; // guarda tiempo ultima recepción
dapi08 5:420cb56093d3 86 //byte nMando; // numero de mando 0=babor 1=estribor
dapi08 2:82fdfeec5799 87 int servo; // servo recibido en ultima comunicacion
dapi08 2:82fdfeec5799 88 volatile float latitud;
dapi08 2:82fdfeec5799 89 volatile float longitud;
dapi08 2:82fdfeec5799 90 volatile long fecha_hora;
dapi08 2:82fdfeec5799 91 volatile int n_satelites;
dapi08 2:82fdfeec5799 92 volatile float HDOP;
dapi08 2:82fdfeec5799 93 volatile int rumbo;
dapi08 2:82fdfeec5799 94 volatile int velocidad; // MNPH
dapi08 2:82fdfeec5799 95 volatile int timon_babor;
dapi08 2:82fdfeec5799 96 volatile int timon_estribor;
dapi08 2:82fdfeec5799 97 volatile enEstadoDireccion estado_direccion;
dapi08 2:82fdfeec5799 98 volatile byte EstadoAnteriorServo; //Guarda estado anterior del servo para salir de piloto-automatico o rumbo
dapi08 2:82fdfeec5799 99 volatile int retardoActualizacion; // para no visualizar estado durante este tiempo en ms
dapi08 2:82fdfeec5799 100 volatile bool act_display; // flag para actualizar display
dapi08 2:82fdfeec5799 101 volatile bool trama_estado_valida;
dapi08 2:82fdfeec5799 102 volatile bool trama_posicion_valida;
dapi08 1:da6afd392792 103
dapi08 4:7564d0ffd595 104
dapi08 2:82fdfeec5799 105 // VARIABLE CONTROL SERVO
dapi08 2:82fdfeec5799 106 enEstadoServo estado_servo;
dapi08 2:82fdfeec5799 107 int16_t velocidad_servo;
dapi08 2:82fdfeec5799 108 int16_t torque_servo;
dapi08 2:82fdfeec5799 109 long desviacion_servo;
dapi08 2:82fdfeec5799 110 volatile int pos_actual_servo = 0;
dapi08 2:82fdfeec5799 111 volatile int pos_objetivo_servo = 0;
dapi08 2:82fdfeec5799 112 volatile unsigned long tempo_espera = 0;
dapi08 2:82fdfeec5799 113
dapi08 2:82fdfeec5799 114 int torque_max; // torque máximo calculado
dapi08 2:82fdfeec5799 115 int out_torque = TORQUE_MINIMO; // nivel de torque de TORQUE_MINIMO a TORQUE_MAXIMO
dapi08 2:82fdfeec5799 116
dapi08 2:82fdfeec5799 117 int cons_timon_babor = 0; // consigna % de timon babor bajado
dapi08 2:82fdfeec5799 118 int cons_timon_estribor = 0; // consigna % de timon estribor bajado
dapi08 2:82fdfeec5799 119
dapi08 2:82fdfeec5799 120 float diferencia_rumbo = 0.0; // Diferencia al Rumbo deseado
dapi08 1:da6afd392792 121
dapi08 2:82fdfeec5799 122 st_datos_servo datos_servo; // ESTRUCTURA DE VARIABLES DE ESTADO DEL SERVO DE DIRECCION
dapi08 2:82fdfeec5799 123 st_datos_GPS datos_GPS; // ESTRUCTURA DE DATOS DE ESTADO DEL GPS
dapi08 6:c21017e969af 124 volatile st_datos_servo datos_otro_servo; // ESTRUCTURA DE VARIABLES DE ESTADO DEL OTRO SERVO DE DIRECCION
dapi08 7:60d157ef1134 125 volatile bool f_actualiza_tim_trans=false; // Pone flag para actualizar timones y transmision
dapi08 2:82fdfeec5799 126
dapi08 2:82fdfeec5799 127
dapi08 4:7564d0ffd595 128
dapi08 2:82fdfeec5799 129 // VARIABLES CONFIGURACION DESDE ORDENADOR
dapi08 2:82fdfeec5799 130 int cfg_hora = 7200; // Suma dos horas a UTC
dapi08 2:82fdfeec5799 131
dapi08 2:82fdfeec5799 132
dapi08 2:82fdfeec5799 133 // VARIABLES GENERALES
dapi08 2:82fdfeec5799 134 float ana_velocidad = 0.0;
dapi08 2:82fdfeec5799 135 float ana_torque = 0.0;
dapi08 2:82fdfeec5799 136 bool newtime = false;
dapi08 2:82fdfeec5799 137 ntp_packet in_data; // para recibir fecha y hora del servidor
dapi08 2:82fdfeec5799 138
dapi08 2:82fdfeec5799 139 // VARIABLES DE COMUNICACION RS485
dapi08 3:55227f9877e0 140 volatile bool f_rs485_busy = false; // comunicacion ocupada
dapi08 3:55227f9877e0 141 volatile bool recibidoEstadoServo = false; //Si ha recibido estado del otro servo se pone a true para sincronizacion
dapi08 3:55227f9877e0 142
dapi08 4:7564d0ffd595 143 // VARIABLES RECIBIDAS DESDE MANDO
dapi08 4:7564d0ffd595 144 int estado_mando = 0;
dapi08 4:7564d0ffd595 145 byte pulsadores = 0; // Estado pulsadores del mando
dapi08 4:7564d0ffd595 146
dapi08 4:7564d0ffd595 147 byte estado_old = DIR_REPOSO; // ultimo estado enviado
dapi08 4:7564d0ffd595 148
dapi08 2:82fdfeec5799 149
dapi08 2:82fdfeec5799 150 // TIMERS
dapi08 2:82fdfeec5799 151 Timer timer;
dapi08 4:7564d0ffd595 152 Ticker tick_millis; // Interrupcion para cuenta de milisegundos
dapi08 3:55227f9877e0 153 Ticker tickRS485us; // Interrupcion de 100us para com rs485
dapi08 3:55227f9877e0 154 Timer timerRS485; // Timer para sincronizacion por rs485 de los mandos
dapi08 2:82fdfeec5799 155
dapi08 2:82fdfeec5799 156
dapi08 2:82fdfeec5799 157
dapi08 2:82fdfeec5799 158
dapi08 2:82fdfeec5799 159
dapi08 2:82fdfeec5799 160
dapi08 2:82fdfeec5799 161 // RUTINAS
dapi08 2:82fdfeec5799 162
dapi08 2:82fdfeec5799 163 // CUENTA MILISEGUNDOS DESDE RESET
dapi08 2:82fdfeec5799 164 void cuenta_ms() {
dapi08 2:82fdfeec5799 165 tiempo_ms++;
dapi08 2:82fdfeec5799 166 posicion_servo(); // AJUSTA POSICION SERVO SI ES NECESARIO
dapi08 2:82fdfeec5799 167 f_ms = true; // se pone en cada interrupcion de ms
dapi08 1:da6afd392792 168 }
dapi08 1:da6afd392792 169
dapi08 1:da6afd392792 170
dapi08 4:7564d0ffd595 171 // devuelve cuenta de tiempo en ms
dapi08 4:7564d0ffd595 172 unsigned long millis(void){ // devuelve cuenta de tiempo en ms
dapi08 4:7564d0ffd595 173 return tiempo_ms;
dapi08 4:7564d0ffd595 174 }
dapi08 4:7564d0ffd595 175
dapi08 4:7564d0ffd595 176
dapi08 1:da6afd392792 177
dapi08 1:da6afd392792 178
dapi08 1:da6afd392792 179 /*
dapi08 1:da6afd392792 180 /////////////////////////////////////////////////////////
dapi08 1:da6afd392792 181 // PROCESA RECEPCION DESDE SERIAL SERVO
dapi08 1:da6afd392792 182 void onSerialServo(void){ // Procesa recepcion SERVO
dapi08 1:da6afd392792 183
dapi08 1:da6afd392792 184 while (serSERVO.readable()) {
dapi08 1:da6afd392792 185 char c = serSERVO.getc();
dapi08 2:82fdfeec5799 186 // pc.putc(c);
dapi08 2:82fdfeec5799 187 }
dapi08 2:82fdfeec5799 188 }
dapi08 2:82fdfeec5799 189 */
dapi08 2:82fdfeec5799 190
dapi08 2:82fdfeec5799 191
dapi08 2:82fdfeec5799 192
dapi08 2:82fdfeec5799 193
dapi08 2:82fdfeec5799 194
dapi08 2:82fdfeec5799 195 #define FACTOR_INC_RUMBO_BABOR 0.015
dapi08 2:82fdfeec5799 196 #define FACTOR_INC_RUMBO_ESTRIBOR 0.015
dapi08 2:82fdfeec5799 197
dapi08 2:82fdfeec5799 198 // PARA SIMULACION
dapi08 2:82fdfeec5799 199 // Pocesa rumbo_GPS, llamada cada 250ms
dapi08 2:82fdfeec5799 200 void procesa_rumbo_GPS(void){
dapi08 2:82fdfeec5799 201
dapi08 2:82fdfeec5799 202 datos_servo.rumbo -= datos_servo.timon_babor * FACTOR_INC_RUMBO_BABOR;
dapi08 2:82fdfeec5799 203 datos_servo.rumbo += datos_servo.timon_estribor * FACTOR_INC_RUMBO_ESTRIBOR;
dapi08 2:82fdfeec5799 204 // datos_servo.rumbo = flimit_decimales(datos_servo.rumbo, 2);
dapi08 2:82fdfeec5799 205 if(datos_servo.rumbo>360.0){
dapi08 2:82fdfeec5799 206 datos_servo.rumbo -= 360.0;
dapi08 2:82fdfeec5799 207 }
dapi08 2:82fdfeec5799 208 else if(datos_servo.rumbo<0.0){
dapi08 2:82fdfeec5799 209 datos_servo.rumbo += 360.0;
dapi08 1:da6afd392792 210 }
dapi08 1:da6afd392792 211 }
dapi08 1:da6afd392792 212
dapi08 1:da6afd392792 213
dapi08 1:da6afd392792 214
dapi08 1:da6afd392792 215 /////////////////////////////////////////////////////////
dapi08 2:82fdfeec5799 216 // PROGRAMA PRINCIPAL
dapi08 2:82fdfeec5799 217 int main() {
dapi08 2:82fdfeec5799 218 int nc=0;
dapi08 2:82fdfeec5799 219 char out_buffer_2[50];
dapi08 2:82fdfeec5799 220 static int posicion_mando_old = 0;
dapi08 6:c21017e969af 221 static bool f_envia_trama_modbus = false;
dapi08 1:da6afd392792 222
deepikabhavnani 0:cf516d904427 223 // Bring up the ethernet interface
dapi08 2:82fdfeec5799 224 pc.printf("\r\n\r\n");
dapi08 2:82fdfeec5799 225 pc.printf("RESET\r\n\r\n");
dapi08 1:da6afd392792 226 pc.printf("PROGRAMA CONTROL SERVO V0.1\r\n");
dapi08 3:55227f9877e0 227 pc.printf("CONTROL DE SERVO DE ");
dapi08 4:7564d0ffd595 228 babor_estribor = SEL0.read();
dapi08 4:7564d0ffd595 229 if(babor_estribor == 0) // Si es servo de babor = 0
dapi08 3:55227f9877e0 230 pc.printf("BABOR\r\n");
dapi08 3:55227f9877e0 231 else
dapi08 3:55227f9877e0 232 pc.printf("ESTRIBOR\r\n");
dapi08 3:55227f9877e0 233
dapi08 3:55227f9877e0 234
dapi08 1:da6afd392792 235
dapi08 2:82fdfeec5799 236 // Open Ethernet connection
dapi08 2:82fdfeec5799 237 int c=0;
dapi08 2:82fdfeec5799 238 do{
dapi08 2:82fdfeec5799 239 net.disconnect();
dapi08 2:82fdfeec5799 240 wait(2.0); // espera de 1s
dapi08 2:82fdfeec5799 241 int j = net.set_network(IP, MASK, GATEWAY);
dapi08 2:82fdfeec5799 242 printf("set IP status: %i\r\n", j);
dapi08 1:da6afd392792 243
dapi08 2:82fdfeec5799 244 c=net.connect();
dapi08 2:82fdfeec5799 245 if(0 != c) {
dapi08 2:82fdfeec5799 246 printf("Error connecting\r\n");
dapi08 2:82fdfeec5799 247 // return -1;
dapi08 2:82fdfeec5799 248 }
dapi08 2:82fdfeec5799 249 }while(c != 0);
deepikabhavnani 0:cf516d904427 250
deepikabhavnani 0:cf516d904427 251 // Show the network address
deepikabhavnani 0:cf516d904427 252 const char *ip = net.get_ip_address();
dapi08 1:da6afd392792 253 printf("IP address is: %s\r\n", ip ? ip : "No IP");
dapi08 2:82fdfeec5799 254
dapi08 2:82fdfeec5799 255 Thread t; // can pass in the stack size here if you run out of memory
dapi08 2:82fdfeec5799 256 t.start(&udp_main);
dapi08 2:82fdfeec5799 257
dapi08 2:82fdfeec5799 258
dapi08 2:82fdfeec5799 259 // Inicializa datos_servo ESTRUCTURA DE VARIABLES DE ESTADO DEL SERVO DE DIRECCION
dapi08 4:7564d0ffd595 260 datos_servo.n_servo = babor_estribor;
dapi08 2:82fdfeec5799 261 datos_servo.estado = 0;
dapi08 2:82fdfeec5799 262 datos_servo.latitud = 0.0;
dapi08 2:82fdfeec5799 263 datos_servo.longitud = 0.0;
dapi08 2:82fdfeec5799 264 datos_servo.fecha_hora = 0;
dapi08 2:82fdfeec5799 265 datos_servo.velocidad = 0.0; // MNPH
dapi08 2:82fdfeec5799 266 datos_servo.rumbo = 0.0;
dapi08 2:82fdfeec5799 267 datos_servo.ch_radio = 0;
dapi08 2:82fdfeec5799 268 datos_servo.trim_timon_bab = 0;
dapi08 2:82fdfeec5799 269 datos_servo.trim_trans_bab = 6;
dapi08 2:82fdfeec5799 270 datos_servo.trim_timon_estr = 0;
dapi08 2:82fdfeec5799 271 datos_servo.trim_trans_estr = 6;
dapi08 2:82fdfeec5799 272 datos_servo.timon_babor = 0; // %de timon babor bajado, para 100% 5s, 1% = 50ms
dapi08 2:82fdfeec5799 273 datos_servo.timon_estribor = 0; // %de timon estribor bajado, para 100% 5s
dapi08 2:82fdfeec5799 274 datos_servo.rumbo_fijado = 0.0; // Rumbo fijado al pulsar botón para RUMBO FIJO
dapi08 2:82fdfeec5799 275 datos_servo.rumbo_piloto_auto = 0.0; // Rumbo fijado por piloto automático
dapi08 2:82fdfeec5799 276
dapi08 2:82fdfeec5799 277 // Set handler to be called when UDP socket event occurrs.
dapi08 2:82fdfeec5799 278 // sock.attach(&onUDPSocketEvent);
dapi08 1:da6afd392792 279
dapi08 1:da6afd392792 280 timer.start();
dapi08 4:7564d0ffd595 281 tick_millis.attach(&cuenta_ms, 0.001); // the address of the function to be attached (cuenta_ms) and the interval (1 miliseconds)
dapi08 3:55227f9877e0 282 tickRS485us.attach(&SerialTxRS485, 0.0001); // Interrupcion de 100us para comunicaciones por RS485 con mando
dapi08 3:55227f9877e0 283 timerRS485.start(); // Timer para sincronizacion desde recepcion del otro control servo
dapi08 3:55227f9877e0 284
dapi08 2:82fdfeec5799 285 led1 = led2 = led3 =1;
dapi08 1:da6afd392792 286 wait(0.5);
dapi08 2:82fdfeec5799 287 led1 = led2 = led3 =0;
dapi08 2:82fdfeec5799 288
dapi08 2:82fdfeec5799 289 // serSERVO.attach(&onSerialServo); // Interrupcion de serial Servo
dapi08 4:7564d0ffd595 290 serRS485.attach(&onSerialRxRS485, Serial::RxIrq); // Interrupcion de RX serial RS485 (MANDOS)
dapi08 2:82fdfeec5799 291 // serRS485.attach(&onSerialTxRS485, Serial::TxIrq); // Interrupcion de TX serial RS485 (MANDOS)
dapi08 2:82fdfeec5799 292
dapi08 2:82fdfeec5799 293 serGPS.attach(&onSerialGPS); // Interrupcion de serial GPS
dapi08 1:da6afd392792 294
dapi08 1:da6afd392792 295 RS485_OE = 1; // habilita conversor 3.3V a 5V para RS485
dapi08 1:da6afd392792 296
dapi08 2:82fdfeec5799 297 /*
dapi08 1:da6afd392792 298 SRV_C_TRQ_P.period(0.001f); // 1 kHz or 1 ms
dapi08 1:da6afd392792 299 SRV_C_TRQ_N.period(0.001f); // 1 kHz or 1 ms
dapi08 1:da6afd392792 300 SRV_C_TRQ_P=0;
dapi08 1:da6afd392792 301 SRV_C_TRQ_N=0;
dapi08 2:82fdfeec5799 302 */
dapi08 2:82fdfeec5799 303
dapi08 2:82fdfeec5799 304 SERVO_ON = 1; // Pone servo en ON
dapi08 2:82fdfeec5799 305
dapi08 2:82fdfeec5799 306 // uint8_t i=0;
dapi08 2:82fdfeec5799 307
dapi08 2:82fdfeec5799 308 // SetDateTime(2020, 4, 25, 10, 00, 0); // Actualiza hora del RTC
dapi08 2:82fdfeec5799 309 // ShowDateTime();
deepikabhavnani 0:cf516d904427 310
dapi08 1:da6afd392792 311 while(1){
dapi08 1:da6afd392792 312
dapi08 2:82fdfeec5799 313 while(f_ms!=true); // espera interrupcion de 1 ms
dapi08 2:82fdfeec5799 314 f_ms = false;
dapi08 2:82fdfeec5799 315
dapi08 2:82fdfeec5799 316 procesaGPS(); // Comprueba trama pendiente de GPS y la procesa
dapi08 1:da6afd392792 317
dapi08 2:82fdfeec5799 318 if(datos_servo.estado == DIR_MASTER || datos_servo.estado == DIR_REPOSO)
dapi08 2:82fdfeec5799 319 datos_servo.posicion_mando = desviacion_servo;
dapi08 2:82fdfeec5799 320 else
dapi08 2:82fdfeec5799 321 datos_servo.posicion_mando = pos_objetivo_servo; // PARA PILOTO AUTOMATICO
dapi08 2:82fdfeec5799 322
dapi08 2:82fdfeec5799 323 if(datos_servo.posicion_mando != posicion_mando_old){ // Si cambia la posicion del mando envia trama
dapi08 6:c21017e969af 324 if(abs(desviacion_servo) >= 50 && datos_servo.estado == DIR_REPOSO && datos_otro_servo.estado == DIR_REPOSO){
dapi08 2:82fdfeec5799 325 datos_servo.estado = DIR_MASTER;
dapi08 1:da6afd392792 326 }
dapi08 2:82fdfeec5799 327
dapi08 2:82fdfeec5799 328 if(datos_servo.estado == DIR_MASTER || datos_servo.estado == DIR_RUMBO || datos_servo.estado == DIR_PILOTO)
dapi08 2:82fdfeec5799 329 envia_posicion_mando(datos_servo.posicion_mando);
dapi08 2:82fdfeec5799 330 posicion_mando_old = datos_servo.posicion_mando;
dapi08 2:82fdfeec5799 331 }
dapi08 2:82fdfeec5799 332
dapi08 2:82fdfeec5799 333 // PROCESA CADA SEGUNDO
dapi08 2:82fdfeec5799 334 if(((tiempo_ms+500)%1000)==0){ // Envia cada segundo
dapi08 2:82fdfeec5799 335
dapi08 6:c21017e969af 336 // test_modbus(); // TEST PARA TRAMA MODBUS
dapi08 2:82fdfeec5799 337 // getTime();
dapi08 2:82fdfeec5799 338
dapi08 2:82fdfeec5799 339 // SetDateTime(2020, 4, 25, 10, 00, 0); // Actualiza hora del RTC
dapi08 2:82fdfeec5799 340 // ShowDateTime();
dapi08 2:82fdfeec5799 341 // pc.printf("tiempo_ms:%d\r\n", tiempo_ms);
dapi08 2:82fdfeec5799 342
dapi08 2:82fdfeec5799 343 // 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());
dapi08 2:82fdfeec5799 344 // pc.printf("velocidad:%3.3f torque:%3.3f fix:%d satelites:%s\r\n", ana_velocidad, ana_torque, gps.have_fix, gps.NmeaGpsData.NmeaSatelliteTracked);
dapi08 2:82fdfeec5799 345
dapi08 2:82fdfeec5799 346 // procesa_servo(); // lee estado del servo, procesa torque y actualiza
dapi08 2:82fdfeec5799 347 // envia_posicion_mando(datos_servo.posicion_mando);
dapi08 2:82fdfeec5799 348
dapi08 4:7564d0ffd595 349 // printf("tiempo_ms=%d pos_objetivo_servo=%d SEL0=%d ESTADO=%d\r\n", tiempo_ms, pos_objetivo_servo, babor_estribor, datos_servo.estado);
dapi08 2:82fdfeec5799 350 }
dapi08 1:da6afd392792 351
dapi08 2:82fdfeec5799 352 /*
dapi08 2:82fdfeec5799 353 //PROCESO CADA 10ms
dapi08 2:82fdfeec5799 354 if((tiempo_ms%10) == 0){
dapi08 2:82fdfeec5799 355 ana_velocidad = SRV_VEL.read(); // 1.0=VCC, lee entradas analogicas
dapi08 2:82fdfeec5799 356 ana_torque = SRV_TRQ.read();
dapi08 2:82fdfeec5799 357 SRV_C_TRQ_P.pulsewidth(ana_velocidad*0.001f); // Ajusta PWM de salida DAC
dapi08 2:82fdfeec5799 358 SRV_C_TRQ_N.pulsewidth(ana_torque*0.001f);
dapi08 2:82fdfeec5799 359 }
dapi08 2:82fdfeec5799 360 */
dapi08 2:82fdfeec5799 361 //PROCESO CADA 100ms
dapi08 2:82fdfeec5799 362 if((tiempo_ms%100) == 0){
dapi08 2:82fdfeec5799 363 // procesa_rumbo_GPS(); // SIMULA RUMBO SEGUN TIMON PARA GPS
dapi08 2:82fdfeec5799 364 // procesa_posicion_timones(); // Pocesa activación y posición timones, llamada cada 100ms
dapi08 2:82fdfeec5799 365 }
dapi08 1:da6afd392792 366
dapi08 2:82fdfeec5799 367 //PROCESO CADA 250ms
dapi08 2:82fdfeec5799 368 if((tiempo_ms%250) == 0){
dapi08 2:82fdfeec5799 369 procesa_servo(); // lee estado del servo, procesa torque y actualiza
dapi08 4:7564d0ffd595 370 procesa_estado(); // PROCESA CAMBIO DE ESTADO
dapi08 4:7564d0ffd595 371 procesa_piloto_automatico(); // procesa piloto automático
dapi08 2:82fdfeec5799 372 procesa_posicion_timones(); // Pocesa activación y posición timones, llamada cada 250ms
dapi08 2:82fdfeec5799 373 actualiza_timones(); // ACTUALIZA TIMONES
dapi08 2:82fdfeec5799 374 procesa_rumbo_GPS(); // SIMULA RUMBO SEGUN TIMON PARA GPS
dapi08 5:420cb56093d3 375 sincroniza_servos(); // SINCRONIZA LOS DOS SERVOS SEGUN SU ESTADO
dapi08 2:82fdfeec5799 376 }
dapi08 1:da6afd392792 377
dapi08 3:55227f9877e0 378 if(recibidoEstadoServo==true){ //Si ha recibido estado del otro servo está a true para sincronizacion
dapi08 6:c21017e969af 379 if(timerRS485.read_ms()>60 && f_envia_trama_modbus == true){
dapi08 6:c21017e969af 380 envia_trama_modbus(); // ENVIA TRAMA MODBUS RS485
dapi08 6:c21017e969af 381 f_envia_trama_modbus = false;
dapi08 6:c21017e969af 382 }
dapi08 5:420cb56093d3 383 if(timerRS485.read_ms()>=120){
dapi08 3:55227f9877e0 384 envia_estado_mando();
dapi08 3:55227f9877e0 385 recibidoEstadoServo = false;
dapi08 6:c21017e969af 386 f_envia_trama_modbus = true;
dapi08 3:55227f9877e0 387 timerRS485.reset();
dapi08 3:55227f9877e0 388 }
dapi08 3:55227f9877e0 389 }
dapi08 3:55227f9877e0 390 else{
dapi08 4:7564d0ffd595 391 int espera = (babor_estribor==0)? 240 :260;
dapi08 6:c21017e969af 392 if(timerRS485.read_ms()>espera){ // Si es servo de babor = 240ms, estribor = 260ms, evita colision
dapi08 3:55227f9877e0 393 envia_estado_mando();
dapi08 5:420cb56093d3 394 recibidoEstadoServo = false;
dapi08 6:c21017e969af 395 f_envia_trama_modbus = true;
dapi08 3:55227f9877e0 396 timerRS485.reset();
dapi08 3:55227f9877e0 397 }
dapi08 6:c21017e969af 398 else{
dapi08 6:c21017e969af 399 if(timerRS485.read_ms()>(espera-60) && f_envia_trama_modbus == true){
dapi08 6:c21017e969af 400 envia_trama_modbus(); // ENVIA TRAMA MODBUS RS485
dapi08 6:c21017e969af 401 f_envia_trama_modbus = false;
dapi08 6:c21017e969af 402 }
dapi08 6:c21017e969af 403 }
dapi08 3:55227f9877e0 404 }
dapi08 3:55227f9877e0 405
dapi08 1:da6afd392792 406 // control_PWR_servo = (pulsador.read() == 1)?0:1;
dapi08 1:da6afd392792 407 control_PWR_servo = 0; // encendido
dapi08 2:82fdfeec5799 408
dapi08 2:82fdfeec5799 409 // PROCESO CADA 10 SEGUNDOS
dapi08 2:82fdfeec5799 410 if((tiempo_ms%10000)==0){
dapi08 1:da6afd392792 411
dapi08 2:82fdfeec5799 412 /*
dapi08 2:82fdfeec5799 413 if(pos_objetivo_servo == 0){
dapi08 2:82fdfeec5799 414 pos_objetivo_servo = 200;
dapi08 2:82fdfeec5799 415 escribe_parametro_servo(0, 13, TORQUE_MAXIMO);
dapi08 2:82fdfeec5799 416 }
dapi08 2:82fdfeec5799 417 else if(pos_objetivo_servo == 200){
dapi08 2:82fdfeec5799 418 pos_objetivo_servo = -200;
dapi08 2:82fdfeec5799 419 escribe_parametro_servo(0, 13, TORQUE_MAXIMO);
dapi08 2:82fdfeec5799 420 }
dapi08 2:82fdfeec5799 421 else if(pos_objetivo_servo == -200){
dapi08 2:82fdfeec5799 422 pos_objetivo_servo = 0;
dapi08 2:82fdfeec5799 423 escribe_parametro_servo(0, 13, TORQUE_MINIMO);
dapi08 2:82fdfeec5799 424 }
dapi08 2:82fdfeec5799 425 // printf("\r\n\r\npos_objetivo_servo=%d\r\n\r\n",pos_objetivo_servo);
dapi08 2:82fdfeec5799 426 */
dapi08 1:da6afd392792 427 }
dapi08 1:da6afd392792 428
dapi08 1:da6afd392792 429 // Thread::wait(10000);
dapi08 1:da6afd392792 430 }
dapi08 1:da6afd392792 431
deepikabhavnani 0:cf516d904427 432 return 0;
dapi08 1:da6afd392792 433 }