Pedro Campos / Mbed OS SERVOS_V0_3

test

main.cpp@1:da6afd392792, 2020-04-20 (annotated)

Committer:
dapi08
Date:
Mon Apr 20 11:42:55 2020 +0000
Revision:
1:da6afd392792
Parent:
0:cf516d904427
Child:
2:82fdfeec5799
Final pruebas de hardware, falta CANbus

Who changed what in which revision?

UserRevisionLine numberNew contents of line
deepikabhavnani 0:cf516d904427 1 #include "mbed.h"
deepikabhavnani 0:cf516d904427 2 #include "EthernetInterface.h"
dapi08 1:da6afd392792 3
dapi08 1:da6afd392792 4
dapi08 1:da6afd392792 5 #define IP "192.168.1.227"
dapi08 1:da6afd392792 6 #define GATEWAY "192.168.1.1"
dapi08 1:da6afd392792 7 #define MASK "255.255.255.0"
dapi08 1:da6afd392792 8
deepikabhavnani 0:cf516d904427 9
deepikabhavnani 0:cf516d904427 10 // Network interface
deepikabhavnani 0:cf516d904427 11 EthernetInterface net;
dapi08 1:da6afd392792 12
dapi08 1:da6afd392792 13 // CANALES DE COMUNICACION
dapi08 1:da6afd392792 14 Serial pc(SERIAL_TX, SERIAL_RX, 115200);
dapi08 1:da6afd392792 15 Serial serGPS(PC_10, PC_11, 9600);
dapi08 1:da6afd392792 16 Serial serSERVO(PC_12, PD_2, 115200);
dapi08 1:da6afd392792 17 Serial serRS485(PD_5, PD_6, 115200);
dapi08 1:da6afd392792 18
dapi08 1:da6afd392792 19 // SALIDAS PARA LEDS
dapi08 1:da6afd392792 20 DigitalOut led1(LED1, 0); //LED1
dapi08 1:da6afd392792 21 DigitalOut led2(LED2, 0);
dapi08 1:da6afd392792 22 DigitalOut led3(LED3, 0);
dapi08 1:da6afd392792 23
dapi08 1:da6afd392792 24 // SALIDAS DE CONTROL COMUNICACIONES
dapi08 1:da6afd392792 25 DigitalOut RS485_DIR(PE_3, 0); //DIR RS485 a RX
dapi08 1:da6afd392792 26 DigitalOut RS485_OE(PF_7, 0); //OE RS485 a tri-state
dapi08 1:da6afd392792 27
dapi08 1:da6afd392792 28 // SALIDAS PARA CONTROL DEL SERVO
dapi08 1:da6afd392792 29 DigitalOut control_PWR_servo(PF_9, 1); // Alimentacion del inverter para 220V del servo
dapi08 1:da6afd392792 30 DigitalOut SERVO_ON(PG_1, 0); // Salida de control ON del servo (J7-2)
dapi08 1:da6afd392792 31 DigitalOut SERVO_CLR_COUNT(PG_0, 0); // Salida de control CLR_COUNT del servo (J7-1)
dapi08 1:da6afd392792 32 DigitalOut SERVO_ALARM_CLR(PF_0, 0); // Salida de control ALARM_CLR del servo (J7-4)
dapi08 1:da6afd392792 33 DigitalOut SERVO_CONTROL_MODE(PC_6, 0); // Salida de control CONTROL_MODE del servo (J7-3)
dapi08 1:da6afd392792 34 DigitalOut SERVO_INC_POS(PB_10, 0); // Salida de control INC_POS del servo (J1-2)
dapi08 1:da6afd392792 35 DigitalOut SERVO_DEC_POS(PB_11, 0); // Salida de control DEC_POS del servo (J1-3)
dapi08 1:da6afd392792 36 DigitalOut SERVO_AUX_OUT(PA_15, 0); // Salida de control AUX_OUT del servo (J7-10)
dapi08 1:da6afd392792 37
dapi08 1:da6afd392792 38 // ENTRADAS DE ESTADO DEL SERVO
dapi08 1:da6afd392792 39 DigitalIn SERVO_RDY_IN(PF_13); //Entrada desde SERVO_RDY, 1=activa;
dapi08 1:da6afd392792 40 DigitalIn SERVO_AL_IN(PE_9); //Entrada desde SERVO_AL, 1=activa;
dapi08 1:da6afd392792 41 DigitalIn SERVO_POS_IN(PE_11); //Entrada desde SERVO_POS, 1=activa;
dapi08 1:da6afd392792 42 DigitalIn SERVO_TORQ_L_IN(PF_14); //Entrada desde SERVO_TORQ_L, 1=activa;
dapi08 1:da6afd392792 43 DigitalIn SERVO_Z_SPEED_IN(PE_13); //Entrada desde SERVO_Z_SPEED, 1=activa;
dapi08 1:da6afd392792 44 DigitalIn SRV_D_VEL(PF_2); //Entrada desde SERVO signo nivel de velocidad 1=positivo
dapi08 1:da6afd392792 45 DigitalIn SRV_D_TRQ(PF_1); //Entrada desde SERVO signo nivel de torque 1=positivo
dapi08 1:da6afd392792 46
dapi08 1:da6afd392792 47 // SALIDAS PWM
dapi08 1:da6afd392792 48 PwmOut SRV_C_TRQ_P(PD_13); // Salida PWM para control del torque POSITIVO del SERVO
dapi08 1:da6afd392792 49 PwmOut SRV_C_TRQ_N(PA_0); // Salida PWM para control del torque NEGATIVO del SERVO
dapi08 1:da6afd392792 50
dapi08 1:da6afd392792 51 // ENTRADAS ANALOGICAS
dapi08 1:da6afd392792 52 AnalogIn SRV_VEL(PA_3); // Entrada analogica de VELOCIDAD del servo
dapi08 1:da6afd392792 53 AnalogIn SRV_TRQ(PC_0); // Entrada analogica de TORQUE del servo
dapi08 1:da6afd392792 54
dapi08 1:da6afd392792 55
dapi08 1:da6afd392792 56 // ENTRADAS GENERALES
dapi08 1:da6afd392792 57 DigitalIn pulsador(PC_13); //Pulsador de usuario, 1=pulsado;
dapi08 1:da6afd392792 58
dapi08 1:da6afd392792 59
dapi08 1:da6afd392792 60
dapi08 1:da6afd392792 61
dapi08 1:da6afd392792 62 long tiempo_ms = 0;
dapi08 1:da6afd392792 63 long tiempo = 0;
dapi08 1:da6afd392792 64
dapi08 1:da6afd392792 65 float velocidad = 0.0;
dapi08 1:da6afd392792 66 float torque = 0.0;
dapi08 1:da6afd392792 67
dapi08 1:da6afd392792 68
dapi08 1:da6afd392792 69 Timer timer;
dapi08 1:da6afd392792 70
dapi08 1:da6afd392792 71 Ticker flipper;
dapi08 1:da6afd392792 72
deepikabhavnani 0:cf516d904427 73
dapi08 1:da6afd392792 74 void flip() {
dapi08 1:da6afd392792 75 led2 = !led2;
dapi08 1:da6afd392792 76 led3 = !led3;
dapi08 1:da6afd392792 77 }
dapi08 1:da6afd392792 78
dapi08 1:da6afd392792 79
deepikabhavnani 0:cf516d904427 80 // Time protocol implementation : Address: time.nist.gov UDPPort: 37
deepikabhavnani 0:cf516d904427 81
deepikabhavnani 0:cf516d904427 82 typedef struct {
deepikabhavnani 0:cf516d904427 83 uint32_t secs; // Transmit Time-stamp seconds.
deepikabhavnani 0:cf516d904427 84 }ntp_packet;
dapi08 1:da6afd392792 85
dapi08 1:da6afd392792 86
dapi08 1:da6afd392792 87 /////////////////////////////////////////////////////////
dapi08 1:da6afd392792 88 // PROCESA RECEPCION DESDE GPS
dapi08 1:da6afd392792 89 void procesa_recepcion(void){ // Procesa recepcion GPS
dapi08 1:da6afd392792 90 /*
dapi08 1:da6afd392792 91 while (serGPS.readable()) {
dapi08 1:da6afd392792 92 char c = serGPS.getc();
dapi08 1:da6afd392792 93 pc.putc(c);
dapi08 1:da6afd392792 94 }
dapi08 1:da6afd392792 95
dapi08 1:da6afd392792 96 while (serSERVO.readable()) {
dapi08 1:da6afd392792 97 char c = serSERVO.getc();
dapi08 1:da6afd392792 98 pc.putc(c);
dapi08 1:da6afd392792 99 }
dapi08 1:da6afd392792 100 */
dapi08 1:da6afd392792 101 }
dapi08 1:da6afd392792 102
dapi08 1:da6afd392792 103
dapi08 1:da6afd392792 104 /////////////////////////////////////////////////////////
dapi08 1:da6afd392792 105 // PROCESA RECEPCION DESDE SERIAL SERVO
dapi08 1:da6afd392792 106 void onSerialServo(void){ // Procesa recepcion SERVO
dapi08 1:da6afd392792 107
dapi08 1:da6afd392792 108 while (serSERVO.readable()) {
dapi08 1:da6afd392792 109 char c = serSERVO.getc();
dapi08 1:da6afd392792 110 pc.putc(c);
dapi08 1:da6afd392792 111 }
dapi08 1:da6afd392792 112 }
dapi08 1:da6afd392792 113
dapi08 1:da6afd392792 114
dapi08 1:da6afd392792 115
dapi08 1:da6afd392792 116 /////////////////////////////////////////////////////////
dapi08 1:da6afd392792 117 // PROCESA RECEPCION DESDE SERIAL RS485
dapi08 1:da6afd392792 118 void onSerialRS485(void){ // Procesa recepcion RS485
dapi08 1:da6afd392792 119
dapi08 1:da6afd392792 120 while (serRS485.readable()) {
dapi08 1:da6afd392792 121 char c = serRS485.getc();
dapi08 1:da6afd392792 122 pc.putc(c);
dapi08 1:da6afd392792 123 }
dapi08 1:da6afd392792 124 }
dapi08 1:da6afd392792 125
dapi08 1:da6afd392792 126
deepikabhavnani 0:cf516d904427 127
deepikabhavnani 0:cf516d904427 128 int main() {
dapi08 1:da6afd392792 129
deepikabhavnani 0:cf516d904427 130 // Bring up the ethernet interface
dapi08 1:da6afd392792 131 pc.printf("PROGRAMA CONTROL SERVO V0.1\r\n");
dapi08 1:da6afd392792 132 pc.printf("PRUEBAS DE COMUNICACION POR UDP\r\n");
dapi08 1:da6afd392792 133
dapi08 1:da6afd392792 134 net.disconnect();
dapi08 1:da6afd392792 135 int j = net.set_network(IP,MASK,GATEWAY);
dapi08 1:da6afd392792 136 printf("set IP status: %i \r\n",j);
dapi08 1:da6afd392792 137
deepikabhavnani 0:cf516d904427 138 if(0 != net.connect()) {
dapi08 1:da6afd392792 139 printf("Error connecting\r\n");
deepikabhavnani 0:cf516d904427 140 return -1;
deepikabhavnani 0:cf516d904427 141 }
deepikabhavnani 0:cf516d904427 142
deepikabhavnani 0:cf516d904427 143 // Show the network address
deepikabhavnani 0:cf516d904427 144 const char *ip = net.get_ip_address();
dapi08 1:da6afd392792 145 printf("IP address is: %s\r\n", ip ? ip : "No IP");
deepikabhavnani 0:cf516d904427 146
deepikabhavnani 0:cf516d904427 147 UDPSocket sock(&net);
deepikabhavnani 0:cf516d904427 148 SocketAddress sockAddr;
dapi08 1:da6afd392792 149
dapi08 1:da6afd392792 150 timer.start();
dapi08 1:da6afd392792 151
dapi08 1:da6afd392792 152 led2 = 1;
dapi08 1:da6afd392792 153 led3 = 0;
dapi08 1:da6afd392792 154 flipper.attach(&flip, 1.0); // the address of the function to be attached (flip) and the interval (2 seconds)
dapi08 1:da6afd392792 155 wait(0.5);
dapi08 1:da6afd392792 156
dapi08 1:da6afd392792 157 serSERVO.attach(&onSerialServo); // Interrupcion de serial Servo
dapi08 1:da6afd392792 158 serRS485.attach(&onSerialRS485); // Interrupcion de serial RS485 (MANDOS)
dapi08 1:da6afd392792 159
dapi08 1:da6afd392792 160 RS485_OE = 1; // habilita conversor 3.3V a 5V para RS485
dapi08 1:da6afd392792 161
dapi08 1:da6afd392792 162 SRV_C_TRQ_P.period(0.001f); // 1 kHz or 1 ms
dapi08 1:da6afd392792 163 SRV_C_TRQ_N.period(0.001f); // 1 kHz or 1 ms
dapi08 1:da6afd392792 164 SRV_C_TRQ_P=0;
dapi08 1:da6afd392792 165 SRV_C_TRQ_N=0;
dapi08 1:da6afd392792 166
deepikabhavnani 0:cf516d904427 167
dapi08 1:da6afd392792 168 // uint8_t i=0;
dapi08 1:da6afd392792 169 while(1){
dapi08 1:da6afd392792 170
dapi08 1:da6afd392792 171 if(timer.read_us()>=250){
dapi08 1:da6afd392792 172 procesa_recepcion(); // Procesa recepcion GPS
dapi08 1:da6afd392792 173 tiempo++;
dapi08 1:da6afd392792 174 timer.reset();
dapi08 1:da6afd392792 175 if(tiempo%4000==0){ // Envia cada segundo
dapi08 1:da6afd392792 176 // serSERVO.printf("PRUEBA DE COMUNICACION CON EL SERVO\r\n");
dapi08 1:da6afd392792 177 RS485_DIR = 1; // pone en TX
dapi08 1:da6afd392792 178 wait(0.1);
dapi08 1:da6afd392792 179 serRS485.printf("PRUEBA DE COMUNICACION POR RS485\r\n");
dapi08 1:da6afd392792 180 wait(0.1);
dapi08 1:da6afd392792 181 RS485_DIR = 0; // pone en RX
dapi08 1:da6afd392792 182
dapi08 1:da6afd392792 183 // 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 1:da6afd392792 184 pc.printf("velocidad:%3.3f torque:%3.3f\r\n", velocidad, torque);
dapi08 1:da6afd392792 185
dapi08 1:da6afd392792 186 }
dapi08 1:da6afd392792 187 if(tiempo%40 == 0){
dapi08 1:da6afd392792 188 velocidad = SRV_VEL.read(); // 1.0=VCC
dapi08 1:da6afd392792 189 torque = SRV_TRQ.read();
dapi08 1:da6afd392792 190 SRV_C_TRQ_P.pulsewidth(velocidad*0.001f);
dapi08 1:da6afd392792 191 SRV_C_TRQ_N.pulsewidth(torque*0.001f);
dapi08 1:da6afd392792 192 }
dapi08 1:da6afd392792 193 }
dapi08 1:da6afd392792 194
dapi08 1:da6afd392792 195 led1 = ((tiempo/4000)%2 == 0)?1:0;
dapi08 1:da6afd392792 196
dapi08 1:da6afd392792 197 SERVO_INC_POS = led2;
dapi08 1:da6afd392792 198 SERVO_DEC_POS = led3;
dapi08 1:da6afd392792 199
dapi08 1:da6afd392792 200 SERVO_ON = led1; // Salida de control ON del servo (J7-2)
dapi08 1:da6afd392792 201 SERVO_CLR_COUNT = led2; // Salida de control CLR_COUNT del servo (J7-1)
dapi08 1:da6afd392792 202 SERVO_ALARM_CLR = led3; // Salida de control ALARM_CLR del servo (J7-4)
dapi08 1:da6afd392792 203 SERVO_CONTROL_MODE = led1; // Salida de control CONTROL_MODE del servo (J7-3)
dapi08 1:da6afd392792 204 SERVO_AUX_OUT = led2; // Salida de control AUX_OUT del servo (J7-10)
dapi08 1:da6afd392792 205
dapi08 1:da6afd392792 206
dapi08 1:da6afd392792 207 // control_PWR_servo = (pulsador.read() == 1)?0:1;
dapi08 1:da6afd392792 208 control_PWR_servo = 0; // encendido
dapi08 1:da6afd392792 209
dapi08 1:da6afd392792 210 if(tiempo >= 40000){
dapi08 1:da6afd392792 211 tiempo = 0;
dapi08 1:da6afd392792 212
dapi08 1:da6afd392792 213 char out_buffer[] = "time";
dapi08 1:da6afd392792 214 char out_buffer_2[50];
dapi08 1:da6afd392792 215 int nc=0;
dapi08 1:da6afd392792 216 if(0 > sock.sendto("time.nist.gov", 37, out_buffer, sizeof(out_buffer))) {
dapi08 1:da6afd392792 217 printf("Error sending data\r\n");
dapi08 1:da6afd392792 218 return -1;
dapi08 1:da6afd392792 219 }
dapi08 1:da6afd392792 220
dapi08 1:da6afd392792 221 ntp_packet in_data;
dapi08 1:da6afd392792 222 int n = sock.recvfrom(&sockAddr, &in_data, sizeof(ntp_packet));
dapi08 1:da6afd392792 223 in_data.secs = ntohl( in_data.secs ) - 2208988800; // 1900-1970
dapi08 1:da6afd392792 224 printf("\r\n\r\nTime Received %lu seconds since 1/01/1900 00:00 GMT\r\n",
dapi08 1:da6afd392792 225 (uint32_t)in_data.secs);
dapi08 1:da6afd392792 226 nc = sprintf(out_buffer_2, "%s", ctime(( const time_t* )&in_data.secs) );
dapi08 1:da6afd392792 227 printf("Time = %s\r\n", out_buffer_2);
dapi08 1:da6afd392792 228
dapi08 1:da6afd392792 229 printf("Time Server Address: %s Port: %d\r\n\r\n",
dapi08 1:da6afd392792 230 sockAddr.get_ip_address(), sockAddr.get_port());
deepikabhavnani 0:cf516d904427 231
dapi08 1:da6afd392792 232 if(0 > sock.sendto("192.168.1.37", 6000, out_buffer_2, nc)) {
dapi08 1:da6afd392792 233 printf("\r\nError sending data 2\r\n\r\n");
dapi08 1:da6afd392792 234 return -1;
dapi08 1:da6afd392792 235 }
dapi08 1:da6afd392792 236 }
dapi08 1:da6afd392792 237
dapi08 1:da6afd392792 238 // Thread::wait(10000);
dapi08 1:da6afd392792 239 }
dapi08 1:da6afd392792 240
deepikabhavnani 0:cf516d904427 241 // Close the socket and bring down the network interface
deepikabhavnani 0:cf516d904427 242 sock.close();
deepikabhavnani 0:cf516d904427 243 net.disconnect();
deepikabhavnani 0:cf516d904427 244 return 0;
dapi08 1:da6afd392792 245 }