> > I use the STM32F407 with the hostmot2 driver. I had some glitches at > > startup but today I added a small delay someone here suggested end then > > startup works perfect every time with two cards connected. > > > > I have been running two axes with DC servo motors and ordinary encoders on > > one of the cards although both cards should be used to get 4 axis. I ran > > servo loop at 1kHz but expect somewhat faster will also work. > > Hi, currently I am working on a similar project involving a ethernet based > board to control my own servo drives: https://github.com/rene-dev/stmbl > Is anyone of you willing to share some code? I already started implementing a > server for the hm2_eth protocol, but maybe we can work together? > my idea is to use rs485 to control the drives, as it is much better than > step/direction. > the stmbl hardware also supports smart serial, but there is a lot of software > to do. > is there any smartserial client code available? > > Rene
Attached code communicate with the 7i80 driver via ethernet. I also spent some time on RS-485 and Profibus and found someone who wrote Profibus before http://freecode.com/projects/profibus but have not had time yet to look at it. I spent the day to look for cheap shielded connectors for cables.
/* * servo.c * * Created on: 11 sep 2014 * Author: karlnick */ #include "servo.h" #include "lwip/pbuf.h" #include "lwip/udp.h" #include "lwip/tcp.h" #include "main.h" #include "inverter1.h" #include "inverter2.h" #include "encoder1.h" #include "encoder2.h" #include "string.h" #include <stdint.h> #include "debugUdp.h" #include "io.h" #include "stm32f4xx.h" static void udp_receive_callback(void *arg, struct udp_pcb *upcb, struct pbuf *p, struct ip_addr *addr, uint16_t port); static void send(struct udp_pcb* pcbSend, const void* reply, int len); void SysTick_Handler(void); typedef union{ struct{ uint16_t N:7; // Transfer count in units of the selected size uint16_t I:1; // If this is '1' the address pointer is incremented by the element transfer size (in bytes) after every transfer uint16_t S:2; // transfer element size specifier (00b = 8 bits, 01b = 16 bits 10b = 32 bits and 11b = 64 bits) uint16_t M:3; // 3 bit memory space specifier 000b through 111b uint16_t C:1; // Indicates if memory space itself (C=â0') or associated info area for the memory will be accessed (C= â1') uint16_t A:1; // If this is '1' the command is followed by a 16 bit address and the address pointer is loaded with this address. uint16_t W:1; // Write bit (1 means write, 0 means read) }; uint16_t raw; } cmd_type; typedef struct { uint32_t idrom_type; uint32_t offset_to_modules; uint32_t offset_to_pin_desc; uint8_t board_name[8]; // ascii string, but not NULL terminated! uint32_t fpga_size; uint32_t fpga_pins; uint32_t io_ports; uint32_t io_width; uint32_t port_width; uint32_t clock_low; uint32_t clock_high; uint32_t instance_stride_0; uint32_t instance_stride_1; uint32_t register_stride_0; uint32_t register_stride_1; } hm2_idrom_t; typedef struct{ uint16_t cookie; union{ struct{ uint16_t a:4; uint16_t :4; uint16_t t:7; uint16_t w:1; }; uint16_t memsizes; }; union{ struct{ uint16_t s:6; uint16_t p:5; uint16_t e:5; }; uint16_t memranges; }; uint16_t address_pointer; uint16_t spacename0_1; uint16_t spacename2_3; uint16_t spacename4_5; uint16_t spacename6_7; } infoArea_type; static struct udp_pcb *pcbReceive = NULL; static err_t err = ERR_BUF; static int32_t d[4] = {0, 0, 0}; static uint32_t pwm[4]; static volatile int PwmWatchdog = 0; void servoInit(void){ assert_param(!SysTick_Config(SystemCoreClock/1000)); NVIC_EnableIRQ(SysTick_IRQn); pcbReceive = udp_new(); if (pcbReceive != NULL){ err = udp_bind(pcbReceive, IP_ADDR_ANY, UDP_SERVER_PORT); if(err == ERR_OK){ /* Set a receive callback for the upcb */ udp_recv(pcbReceive, udp_receive_callback, NULL); } } } /** * @brief This function is called when an UDP datagram has been received on the port UDP_PORT. * @param arg user supplied argument (udp_pcb.recv_arg) * @param pcb the udp_pcb which received data * @param p the packet buffer that was received * @param addr the remote IP address from which the packet was received * @param port the remote port from which the packet was received * @retval None */ static void udp_receive_callback(void *arg, struct udp_pcb *upcb, struct pbuf *p, struct ip_addr *addr, uint16_t port){ (void)arg; int left = p->len; const void* data = &((uint16_t*)p->payload)[0]; int dataSendPos = 0; uint8_t dataSend[512]; while(p != NULL && left > 0){ cmd_type cmd = {.raw = ((uint16_t*)data)[0]}; const uint16_t address = ((uint16_t*)data)[1]; data = &((uint16_t*)data)[2]; switch(cmd.M){ case 0 : // Memory space 0: Hostmot 2 registers switch(address){ case 0x0100 : if(cmd.W){ // Write ? } else{ // Read if(cmd.C == 0){ // Info area ? const uint32_t cookie = 0x55AACAFE; memcpy(&dataSend[dataSendPos], &cookie, 4); } else{ // Info area const uint16_t info = 23; memcpy(&dataSend[dataSendPos], &info, 2); } } break; case 0x0104 : if(cmd.C == 0){ // Memory space ? if(cmd.W){ // Write ? } else{ // Read const char cookie[] = "HOSTMOT2"; memcpy(&dataSend[dataSendPos], cookie, 8); } } break; case 0x010C : if(cmd.C == 0){ // Memory space ? if(cmd.W){ // Write ? } else{ // Read const uint32_t idromOffset = 0x1000; memcpy(&dataSend[dataSendPos], &idromOffset, 4); } } break; case 0x1000 : if(cmd.C == 0){ // Memory space ? if(cmd.W){ // Write ? } else{ // Read const hm2_idrom_t hm2_idrom = { .idrom_type = 2, .offset_to_modules = 0x1000, .offset_to_pin_desc = 0x2000, .board_name = {'S','T','M','3','2','F','4','x'}, // ascii string, but not NULL terminated! .fpga_size = 0, .fpga_pins = 0, .io_ports = 4, .io_width = 68, .port_width = 17, .clock_low = 168000000, .clock_high = 168000000, .instance_stride_0 = 4, .instance_stride_1 = 0, .register_stride_0 = 0, .register_stride_1 = 0 }; memcpy(&dataSend[dataSendPos], &hm2_idrom, 4*cmd.N); } } else{ const uint16_t idromType = 2; memcpy(&dataSend[dataSendPos], &idromType, 2); } break; case 0x2000 ... 0x223F : // Module descriptors if(cmd.C == 0){ // Memory space ? if(cmd.W){ // Write ? } else{ // Read const int pos = (address - 0x2000)/12; const uint32_t descriptorDefault[3] = { // gtag: 2=watchdog, 3=IO, 4=encoder, 6=PWM (3<<24) | (1<<16) | (0<<8) | (0<<0), // instances<<24, clock_tag<<16, version<<8, gtag<<0 (0<<28) | (0<<24) | (5<<16) | (0x5000<<0), // instance_stride<<28, register_stride<<24, num_registers<<16, base_address<<0 0x1F}; // Multiple registers const uint32_t descriptor[4][3] = { {(4<<24) | (1<<16) | (3<<8) | (4<<0), // instances<<24, clock_tag<<16, version<<8, gtag<<0 (0<<28) | (0<<24) | (5<<16) | (0x5500<<0), // instance_stride<<28, register_stride<<24, num_registers<<16, base_address<<0 3}, // Multiple registers {(3<<24) | (1<<16) | (0<<8) | (2<<0), // instances<<24, clock_tag<<16, version<<8, gtag<<0 (0<<28) | (0<<24) | (3<<16) | (0x5200<<0), // instance_stride<<28, register_stride<<24, num_registers<<16, base_address<<0 0}, // Multiple registers {(4<<24) | (1<<16) | (0<<8) | (3<<0), // instances<<24, clock_tag<<16, version<<8, gtag<<0 (0<<28) | (0<<24) | (5<<16) | (0x5300<<0), // instance_stride<<28, register_stride<<24, num_registers<<16, base_address<<0 0x1F}, // Multiple registers {(4<<24) | (1<<16) | (0<<8) | (6<<0), // instances<<24, clock_tag<<16, version<<8, gtag<<0 (0<<28) | (0<<24) | (5<<16) | (0x5400<<0), // instance_stride<<28, register_stride<<24, num_registers<<16, base_address<<0 3} // Multiple registers }; if(pos < 4){ memcpy(&dataSend[dataSendPos], &descriptor[pos], 12); } else{ memcpy(&dataSend[dataSendPos], &descriptorDefault, 12); } } } break; case 0x3000 ... 0x310F : if(cmd.C == 0){ // Memory space ? if(cmd.W){ // Write ? } else{ // Read const int pos = (address - 0x3000) >> 2; const uint32_t idromOffset[68] = { [0] = (3<<24) | (0<<16) | (0<<8) | (2<<0), // primary_tag<<24, sec_unit<<16, sec_tag<<8, sec_pin<<0 [1] = (3<<24) | (0<<16) | (0<<8) | (2<<0), // primary_tag<<24, sec_unit<<16, sec_tag<<8, sec_pin<<0 [2 ... 24] = (3<<24) | (0<<16) | (0<<8) | (2<<0), [25] = (3<<24) | (0<<16) | (0<<8) | (2<<0), [26 ... 67] = (3<<24) | (0<<16) | (0<<8) | (2<<0) }; memcpy(&dataSend[dataSendPos], &idromOffset[pos], 4); } } break; case 0x5200 ... 0x52FF : // Watch dog if(cmd.C == 0){ // Memory space ? static uint32_t watchdog = 0; if(cmd.W){ watchdog = ((uint32_t*)data)[0]; } else{ watchdog = 0; memcpy(&dataSend[dataSendPos], &watchdog, 4); } } else{ // Info area static uint32_t watchdog = 0; memcpy(&dataSend[dataSendPos], &watchdog, 4); } break; case 0x5300 ... 0x53FF : // IO ports if(cmd.C == 0){ // Memory space ? static uint8_t ioPort[4*3] = {[0 ... 11] = 0}; if(0&&IO_getE3()){ ioPort[4] |= (1<<0); } else{ ioPort[4] &= ~(1<<0); } if(0&&IO_getE4()){ ioPort[4] |= (1<<1); } else{ ioPort[4] &= ~(1<<1); } if(0&&IO_getE5()){ ioPort[4] |= (1<<2); } else{ ioPort[4] &= ~(1<<2); } if(cmd.W){ if(((uint8_t*)data)[4] & (1<<3)){ //IO_Brake_On(); } else{ //IO_Brake_Off(); } } else{ memcpy(&dataSend[dataSendPos], &ioPort, 12); } } break; case 0x5400 ... 0x54FF : // PWM if(cmd.C == 0){ // Memory space ? if(cmd.W){ // Write ? int n; int len; if(cmd.N > 4){ len = 4; } else{ len = cmd.N; } for(n = 0; n < len; n++){ pwm[n] = ((uint32_t*)data)[n]; } { uint32_t value = (((pwm[0] >> 16) &0x0FFF)*MAX1) >> 12; if(pwm[0] & (1<<31)){ inverter1SetU(value); } else{ inverter1SetU(-value); } } { uint32_t value = (((pwm[1] >> 16) &0x0FFF)*MAX2) >> 12; if(pwm[1] & (1<<31)){ inverter2SetU(value); } else{ inverter2SetU(-value); } #if 0 { char msg[40]; snprintf(msg, 40, "pwm[1] = 0x%X value = 0x%X\n", pwm[1] >> 16, (uint16_t)value); int len = strlen(msg); if(len > 40){ len = 40; } udp_debug_send(msg, len); } #endif } NVIC_DisableIRQ(SysTick_IRQn); if(PwmWatchdog < 490){ PwmWatchdog += 2; } NVIC_EnableIRQ(SysTick_IRQn); } else{ // Read } } break; case 0x5500 ... 0x55FF : // Encoders if(cmd.C == 0){ // Memory space ? if(cmd.W){ //encoder[0] = ((uint32_t*)data)[0]; //encoder[1] = ((uint32_t*)data)[1]; //encoder[2] = ((uint32_t*)data)[2]; } else{ static uint16_t t[4] = {0, 0, 0, 0}; static uint32_t encoder[4] = {0<<16, 0<<16, 0<<16, 0<<16}; int n; for(n = 0; n < 4; n++){ int value = (pwm[n] >> 16) &0x7FFF; if(n == 2){ value = -value; } t[n]++; if(pwm[n] & (1<<31)){ d[n] += value; } else{ d[n] -= value; } encoder[n] = (t[n] << 16) | (d[n] & 0xFFFF); } encoder[0] = (t[2] << 16) | (encoder1GetAngle() & 0xFFFF); encoder[1] = (t[2] << 16) | (encoder2GetAngle() & 0xFFFF); memcpy(&dataSend[dataSendPos], encoder, 16); } } break; } break; case 2 : // Memory space 2: ETHERNET EEPROM CHIP ACCESS switch(address){ case 0x0000 : // Reserved RO break; case 0x0002 : // MAC address LS Word RO if(cmd.N == 3){ uint8_t mac[6] = {MAC_ADDR5, MAC_ADDR4, MAC_ADDR3, MAC_ADDR2, MAC_ADDR1, MAC_ADDR0}; memcpy(&dataSend[dataSendPos], &mac, 6); } break; case 0x0004 : // MAC address Mid Word RO break; case 0x0006 : // MAC address MS Word RO break; case 0x0008 : // Reserved RO break; case 0x000A : // Reserved RO break; case 0x000C : // Reserved RO break; case 0x000E : // Unused RO break; } break; case 7 : // Memory space 7: LBP READ ONLY AREA switch(address){ case 0x0000 : // CardNameChar-0,1 { char CardName[16] = "7I80DB-25"; memcpy(&dataSend[dataSendPos], &CardName, 16); } break; case 0x0002 : // CardNameChar-2,3 break; case 0x0004 : // CardNameChar-4,5 break; case 0x0006 : // CardNameChar-6,7 break; case 0x0008 : // CardNameChar-8,9 break; case 0x000A : // CardNameChar-10,11 break; case 0x000C : // CardNameChar-12.13 break; case 0x000E : // CardNameChar-14,15 break; case 0x0010 : // LBPVersion break; case 0x0012 : // FirmwareVersion break; case 0x0014 : // Option Jumpers break; case 0x0016 : // Reserved break; case 0x0018 : // RecvStartTS 1 uSec timestamps break; case 0x001A : // RecvDoneTS For performance monitoring break; case 0x001C : // SendStartTS Send timestamps are break; case 0x001E : // SendDoneTS from previous packet break; } break; default : break; } { const int size = (1<<cmd.S)*cmd.N; if(cmd.W){ left = left - 4 - size; data = &((uint8_t*)data)[size]; } else{ left = left - 4; dataSendPos += size; } } } if(dataSendPos > 0){ err = udp_connect(upcb, addr, port); send(upcb, &dataSend, dataSendPos); } /* free the UDP connection, so we can accept new clients */ udp_disconnect(upcb); /* Free the p buffer */ pbuf_free(p); } static void send(struct udp_pcb* pcbSend, const void* reply, int len){ if (pcbSend != NULL){ if (err == ERR_OK){ struct pbuf *p; /* allocate pbuf from pool*/ p = pbuf_alloc(PBUF_TRANSPORT,len, PBUF_POOL); if (p != NULL){ /* copy data to pbuf */ pbuf_take(p, reply, len); /* send udp data */ udp_send(pcbSend, p); /* free pbuf */ pbuf_free(p); } } } } void SysTick_Handler(void){ static int cnt = 0; cnt++; if(PwmWatchdog > 0){ PwmWatchdog--; } else{ inverter1SetU(0); } }
------------------------------------------------------------------------------
_______________________________________________ Emc-developers mailing list Emc-developers@lists.sourceforge.net https://lists.sourceforge.net/lists/listinfo/emc-developers