> > 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
[email protected]
https://lists.sourceforge.net/lists/listinfo/emc-developers
