Pedro Campos
test
Diff: main.cpp
- Revision:
- 1:da6afd392792
- Parent:
- 0:cf516d904427
- Child:
- 2:82fdfeec5799
diff -r cf516d904427 -r da6afd392792 main.cpp
--- a/main.cpp Mon Oct 23 21:26:36 2017 +0000
+++ b/main.cpp Mon Apr 20 11:42:55 2020 +0000
@@ -1,48 +1,245 @@
#include "mbed.h"
#include "EthernetInterface.h"
+
+
+#define IP "192.168.1.227"
+#define GATEWAY "192.168.1.1"
+#define MASK "255.255.255.0"
+
// Network interface
EthernetInterface net;
+
+// CANALES DE COMUNICACION
+Serial pc(SERIAL_TX, SERIAL_RX, 115200);
+Serial serGPS(PC_10, PC_11, 9600);
+Serial serSERVO(PC_12, PD_2, 115200);
+Serial serRS485(PD_5, PD_6, 115200);
+
+// SALIDAS PARA LEDS
+DigitalOut led1(LED1, 0); //LED1
+DigitalOut led2(LED2, 0);
+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
+
+// SALIDAS PARA CONTROL DEL SERVO
+DigitalOut control_PWR_servo(PF_9, 1); // Alimentacion del inverter para 220V del servo
+DigitalOut SERVO_ON(PG_1, 0); // Salida de control ON del servo (J7-2)
+DigitalOut SERVO_CLR_COUNT(PG_0, 0); // Salida de control CLR_COUNT del servo (J7-1)
+DigitalOut SERVO_ALARM_CLR(PF_0, 0); // Salida de control ALARM_CLR del servo (J7-4)
+DigitalOut SERVO_CONTROL_MODE(PC_6, 0); // Salida de control CONTROL_MODE del servo (J7-3)
+DigitalOut SERVO_INC_POS(PB_10, 0); // Salida de control INC_POS del servo (J1-2)
+DigitalOut SERVO_DEC_POS(PB_11, 0); // Salida de control DEC_POS del servo (J1-3)
+DigitalOut SERVO_AUX_OUT(PA_15, 0); // Salida de control AUX_OUT del servo (J7-10)
+
+// ENTRADAS DE ESTADO DEL SERVO
+DigitalIn SERVO_RDY_IN(PF_13); //Entrada desde SERVO_RDY, 1=activa;
+DigitalIn SERVO_AL_IN(PE_9); //Entrada desde SERVO_AL, 1=activa;
+DigitalIn SERVO_POS_IN(PE_11); //Entrada desde SERVO_POS, 1=activa;
+DigitalIn SERVO_TORQ_L_IN(PF_14); //Entrada desde SERVO_TORQ_L, 1=activa;
+DigitalIn SERVO_Z_SPEED_IN(PE_13); //Entrada desde SERVO_Z_SPEED, 1=activa;
+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
+
+// 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
+
+// ENTRADAS ANALOGICAS
+AnalogIn SRV_VEL(PA_3); // Entrada analogica de VELOCIDAD del servo
+AnalogIn SRV_TRQ(PC_0); // Entrada analogica de TORQUE del servo
+
+
+// ENTRADAS GENERALES
+DigitalIn pulsador(PC_13); //Pulsador de usuario, 1=pulsado;
+
+
+
+
+long tiempo_ms = 0;
+long tiempo = 0;
+
+float velocidad = 0.0;
+float torque = 0.0;
+
+
+Timer timer;
+
+Ticker flipper;
+
+void flip() {
+ led2 = !led2;
+ led3 = !led3;
+}
+
+
// 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);
+ }
+}
+
+
+
+/////////////////////////////////////////////////////////
+// PROCESA RECEPCION DESDE SERIAL RS485
+void onSerialRS485(void){ // Procesa recepcion RS485
+
+ while (serRS485.readable()) {
+ char c = serRS485.getc();
+ pc.putc(c);
+ }
+}
+
+
int main() {
+
// Bring up the ethernet interface
- printf("UDP Socket example\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);
+
if(0 != net.connect()) {
- printf("Error connecting\n");
+ printf("Error connecting\r\n");
return -1;
}
// Show the network address
const char *ip = net.get_ip_address();
- printf("IP address is: %s\n", ip ? ip : "No IP");
+ printf("IP address is: %s\r\n", ip ? ip : "No IP");
UDPSocket sock(&net);
SocketAddress sockAddr;
-
- char out_buffer[] = "time";
- if(0 > sock.sendto("time.nist.gov", 37, out_buffer, sizeof(out_buffer))) {
- printf("Error sending data\n");
- return -1;
- }
+
+ 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)
+ wait(0.5);
+
+ serSERVO.attach(&onSerialServo); // Interrupcion de serial Servo
+ serRS485.attach(&onSerialRS485); // Interrupcion de serial RS485 (MANDOS)
+
+ 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;
+
- 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("Time Received %lu seconds since 1/01/1900 00:00 GMT\n",
- (uint32_t)in_data.secs);
- printf("Time = %s", ctime(( const time_t* )&in_data.secs));
+// 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
+
+// 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(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;
+
+ SERVO_INC_POS = led2;
+ SERVO_DEC_POS = led3;
+
+ 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)
+
+
+// control_PWR_servo = (pulsador.read() == 1)?0:1;
+ control_PWR_servo = 0; // encendido
+
+ if(tiempo >= 40000){
+ tiempo = 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());
- printf("Time Server Address: %s Port: %d\n\r",
- 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;
+ }
+ }
+
+// Thread::wait(10000);
+ }
+
// Close the socket and bring down the network interface
sock.close();
net.disconnect();
return 0;
-}
\ No newline at end of file
+}