linuxcnc-esp32 Software Stepping over Ethernet Using ESP32

sudo tcpdump -i any -n udp port 5000
...snip...
22:00:41.020871 IP 192.168.2.178.40206 > 192.168.2.177.5000: UDP, length 40
22:00:41.021237 IP 192.168.2.177.5000 > 192.168.2.178.40206: UDP, length 32
22:00:41.021882 IP 192.168.2.178.59440 > 192.168.2.177.5000: UDP, length 40
22:00:41.022238 IP 192.168.2.177.5000 > 192.168.2.178.59440: UDP, length 32
22:00:41.022888 IP 192.168.2.178.57804 > 192.168.2.177.5000: UDP, length 40
22:00:41.023241 IP 192.168.2.177.5000 > 192.168.2.178.57804: UDP, length 32
^C
3272 packets captured
3290 packets received by filter
0 packets dropped by kernel

char *ipaddress;
RTAPI_MP_STRING(ipaddress,"ESP IP Address");


// Driver code 
FUNCTION(update)
{
    
    
    

    motor_pos.pos0=posx*xscale;
    motor_pos.pos1=posy*yscale;
    motor_pos.pos2=posz*zscale;
    motor_pos.pos3=posu*uscale;

param rw float scale=0.0 """scale: number of step pulses per unit of measure""";

setp udp2.0.scale 133.3333333

component udp "Compute the derivative of the input function";
pin in float posx;
pin in float posy;
pin in float posz;
pin in float posu;


// Scale as parameter, default is 100
param rw float xscale=100;
param rw float yscale=100;
param rw float zscale=100;
param rw float uscale=100;


pin out float test;

pin out float fdbx;
pin out float fdby;
pin out float fdbz;
pin out float fdbu;
pin out float enc0;

///variable double old;

pin out bit in_00;
pin out bit in_01;
pin out bit in_02;
pin out bit in_03;
pin out bit in_04;
pin out bit in_05;
pin out bit in_06;
pin out bit in_07;
pin out bit in_08;
pin out bit in_09;
pin out bit in_10;
pin out bit in_11;
pin out bit in_12;
pin out bit in_13;
pin out bit in_14;
pin out bit in_15;
pin out bit in_16;
pin out bit in_17;
pin out bit in_18;
pin out bit in_19;
pin out bit in_20;
pin out bit in_21;
pin out bit in_22;
pin out bit in_23;
pin out bit in_24;
pin out bit in_25;
pin out bit in_26;
pin out bit in_27;
pin out bit in_28;
pin out bit in_29;
pin out bit in_30;
pin out bit in_31;

pin in bit out_00;
pin in bit out_01;
pin in bit out_02;
pin in bit out_03;
pin in bit out_04;
pin in bit out_05;
pin in bit out_06;
pin in bit out_07;
pin in bit out_08;
pin in bit out_09;
pin in bit out_10;
pin in bit out_11;
pin in bit out_12;
pin in bit out_13;
pin in bit out_14;
pin in bit out_15;
pin in bit out_16;
pin in bit out_17;
pin in bit out_18;
pin in bit out_19;
pin in bit out_20;
pin in bit out_21;
pin in bit out_22;
pin in bit out_23;
pin in bit out_24;
pin in bit out_25;
pin in bit out_26;
pin in bit out_27;
pin in bit out_28;
pin in bit out_29;
pin in bit out_30;
pin in bit out_31;

option extra_setup yes;

function update;
license "GPL"; // indicates GPL v2 or later
;;

// Server side implementation of UDP client-server model 
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h> 
#include <linux/kernel.h>
#include <unistd.h>

struct axes  {
 hal_s32_t fb0;
 hal_s32_t fb1;
 hal_s32_t fb2;
 hal_s32_t fb3;
 hal_u32_t  io;
 int64_t enc0v;
}fb;       

struct pos  {
 hal_s32_t pos0;
 hal_s32_t pos1;
 hal_s32_t pos2;
 hal_s32_t pos3;
 hal_u32_t   io;
 int64_t   analog;
}motor_pos;       







#define BUFSIZE 512
//#define UDP_PORT 27181
#define SEND_TIMEOUT_US 100
#define RECV_TIMEOUT_US 100
#define READ_PCK_DELAY_NS 1000


int sockfd, portno, n;
int serverlen;
struct sockaddr_in serveraddr;
struct hostent *server;
char *hostname;
char buf[BUFSIZE];
char buf1[512];
float axes[9];
char buffer[512] ;
float scale=100.0;

char *ipaddress;
RTAPI_MP_STRING(ipaddress,"ESP IP Address");


// Driver code 
FUNCTION(update)
{
    
    
    

    motor_pos.pos0=posx*xscale;
    motor_pos.pos1=posy*yscale;
    motor_pos.pos2=posz*zscale;
    motor_pos.pos3=posu*uscale;
   
    hostname = "192.168.1.222";
    portno = 5000;

    /* socket: create the socket */
    sockfd = socket(AF_INET, SOCK_DGRAM, 0);
    

    /* gethostbyname: get the server's DNS entry */
    server = gethostbyname(hostname);
    
    /* build the server's Internet address */
    bzero((char *) &serveraddr, sizeof(serveraddr));
    serveraddr.sin_family = AF_INET;
    bcopy((char *)server->h_addr, 
	  (char *)&serveraddr.sin_addr.s_addr, server->h_length);
    serveraddr.sin_port = htons(portno);
    



memcpy(&buf,&motor_pos,sizeof(motor_pos));

    /* send the message to the server */
    serverlen = sizeof(serveraddr);
    int ret;
    struct timeval timeout;
    timeout.tv_sec = 0;
    timeout.tv_usec = RECV_TIMEOUT_US;

    ret = setsockopt(sockfd, SOL_SOCKET, SO_RCVTIMEO, (char *)&timeout, sizeof(timeout));
    timeout.tv_usec = SEND_TIMEOUT_US;
    setsockopt(sockfd, SOL_SOCKET, SO_SNDTIMEO, (char *)&timeout, sizeof(timeout));
    sendto(sockfd, buf,sizeof(motor_pos), 0, &serveraddr, serverlen);
    //if (n < 0) {writes=writes+1;}
    recv(sockfd, buf1 ,sizeof(motor_pos), 0);
    //if(n < 0) {
    //reads=reads+1;
    //rtapi_delay(READ_PCK_DELAY_NS);
    //}
    close(sockfd);
    //*****************************************************************//
  
    

//memcpy(buffer, buf, 1024);
memcpy(&fb, buf1, sizeof(fb));    
fdbx=(float)fb.fb0/xscale;
fdby=(float)fb.fb1/yscale;
fdbz=(float)fb.fb2/zscale;
fdbu=(float)fb.fb3/uscale;
enc0=(float)fb.enc0v;
in_00=(fb.io >> 0) & 1;
in_01=(fb.io >> 1) & 1;
in_02=(fb.io >> 2) & 1;
in_03=(fb.io >> 3) & 1;
in_04=(fb.io >> 4) & 1;
in_05=(fb.io >> 5) & 1;
in_06=(fb.io >> 6) & 1;
in_07=(fb.io >> 7) & 1;
in_08=(fb.io >> 8) & 1;
in_09=(fb.io >> 9) & 1;
in_10=(fb.io >> 10) & 1;
in_11=(fb.io >> 11) & 1;
in_12=(fb.io >> 12) & 1;
in_13=(fb.io >> 13) & 1;
in_14=(fb.io >> 14) & 1;
in_15=(fb.io >> 15) & 1;
in_16=(fb.io >> 16) & 1;
in_17=(fb.io >> 17) & 1;
in_18=(fb.io >> 18) & 1;
in_19=(fb.io >> 19) & 1;



}


EXTRA_SETUP(){

//We test that we have an address on the command line

if (ipaddress == 0) {
	        rtapi_print_msg(RTAPI_MSG_ERR,"Message string for ESP32 IP Address missing\n");
        return -EINVAL;
    }
    rtapi_print_msg(RTAPI_MSG_ERR,"Message string for ESP32 IP Address %s\n",ipaddress);
	return(0);
}

//Should socket be created here so that it is not recreated on every invocation

halcmd: loadrt udp "ipaddress=192.168.1.10"
Note: Using POSIX realtime
Message string for ESP32 IP Address 192.168.1.10
halcmd: 

/*  tjp 9jun2021
 *   what pins does he use?
 *    for step                          [b]actually the orig code used X gpio2 Y gpio16 Z gpio16, i tried to untangle b4[/b]
 *     axis 0 X gpio16
 *     axis 1 Y gpio2
 *     axis 3 Z gpio16     [b]!!! WTF  try use 13  9jun2021[/b]     OK got ols litany to work and can scope Z steps on gpio13
 *    for dir
 *     axis 0 X gpio4
 *     axis 1 Y gpio12
 *     axis 2 Z gpio14 ( NB he uses old school arduino pin out statements for these BUT
 *                          i don tthink it maters ( untested )
 *    for ENC
 *     gpio15
 *     gpio17          I dont know which he wanted for phase a or b   doesnt matter right now
 *    for CS
 *     d5 gpio5
 *    for SPI
 *     SCK     d18 gpio18
 *     MISO    d19 gpio19     
 *     MOSI    d23 gpio23
 *  ... snip....

/*
 UDPSendReceiveString:
 This sketch receives UDP message strings, prints them to the serial port
 and sends an "acknowledge" string back to the sender

 A Processing sketch is included at the end of file that can be used to send
 and received messages for testing with a computer.

 created 21 Aug 2010
 by Michael Margolis

 This code is in the public domain.
 */



#include <stdlib.h>
#include <driver/gpio.h>
#include <Arduino.h>
#include <Ethernet.h>
#include <EthernetUdp.h>
///#include <mcpwm.h>
///#include <ledc.h>
#include <esp_timer.h>
#include <driver/rmt.h>
#include <string.h>
#include <ESP32Encoder.h>


ESP32Encoder encoder;

// Enter a MAC address and IP address for your controller below.
// The IP address will be dependent on your local network:
 
/*###########################Ethernet definitions ######################################*/
byte mac[] = {
  0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED
};
IPAddress ip(192, 168, 1, 177);
unsigned int localPort = 5000;      // local port to listen on
// buffers for receiving and sending data
char packetBuffer[1024];  // buffer to hold incoming packet,
EthernetUDP Udp;
char bufferin[1024] ;
char bufferout[1024] ;
/*##################################i nterupt function ################################*/
unsigned long timeus;
volatile int interrupts;
int totalInterrupts;//char packetBuffer[UDP_TX_PACKET_MAX_SIZE]; 
hw_timer_t * timer = NULL;

portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;

/*######################### STEPPER variables ################################*/

int16_t steps_0_dir;
int16_t steps_0;
int16_t steps_1_dir;
int16_t steps_1;
int16_t steps_2_dir;
int16_t steps_2;
int16_t steps_3_dir;
int16_t steps_3;
int16_t steps_4_dir;
int16_t steps_4;


int8_t steps_0_executed;
int8_t steps_1_executed;
int8_t steps_2_executed;
int8_t steps_3_executed;
int8_t steps_4_executed;
int8_t steps_5_executed;

static int fdb0;
static int fdb1;
static int fdb2;
static int fdb3;
static int fdb4;
static int fdb5;

 
struct old {
  
int old_pos_0;
int old_pos_1;
int old_pos_2;
int old_pos_3;
int old_pos_4;
int old_pos_5;
}olds;






int8_t dir0=1;
int8_t dir1=1;
int8_t dir2=1;
int8_t dir3=1;
int8_t dir4=1;
int8_t dir5=1;


struct axes  {
 int fb0;
 int fb1;
 int fb2;
 int fb3;
 unsigned int io;
int64_t enc0;

};       

axes fdbrt;
//struct axes mayaxes ;

struct pos  {
 int pos0;
 int pos1;
 int pos2;
 int pos3;
 uint32_t io;
int32_t analog;

}cmd;       

axes fdb={0, 0, 0, 0, 0, 0};

/*######################### RMT configuration function  ################################*/
void initRMT() {
    rmt_item32_t rmtItem[2];
    rmt_config_t rmtConfig;
    rmtConfig.rmt_mode = RMT_MODE_TX;
    rmtConfig.clk_div = 20;
    rmtConfig.mem_block_num = 2;
    rmtConfig.tx_config.loop_en = false;
    rmtConfig.tx_config.carrier_en = 0;
    rmtConfig.tx_config.carrier_freq_hz = 0;
    rmtConfig.tx_config.carrier_duty_percent = 50;
    rmtConfig.tx_config.carrier_level = RMT_CARRIER_LEVEL_LOW;
    rmtConfig.tx_config.idle_output_en = true;


    rmtConfig.mem_block_num = 2;
    rmtConfig.tx_config.loop_en = false;
    rmtConfig.tx_config.carrier_en = 0;
    rmtConfig.tx_config.carrier_freq_hz = 0;

//#ifdef STEP_PULSE_DELAY
  // rmtItem[0].duration0 = 4 * 2;
//#else
    rmtItem[0].duration0 = 2;
//#endif
    rmtItem[0].duration1 = 4 * 2;
    rmtItem[1].duration0 = 0;
    rmtItem[1].duration1 = 0;
//AXIS 0 Config
   
    rmt_set_source_clk(RMT_CHANNEL_0, RMT_BASECLK_MAX);
    rmtConfig.channel = RMT_CHANNEL_0;
    rmtConfig.tx_config.idle_level =  RMT_IDLE_LEVEL_LOW;
    rmtConfig.gpio_num = GPIO_NUM_2;						//Step pin AXIS 0
    rmtItem[0].level0 = rmtConfig.tx_config.idle_level;
    rmtItem[0].level1 = !rmtConfig.tx_config.idle_level;
    rmt_config(&rmtConfig);
    rmt_fill_tx_items(rmtConfig.channel, &rmtItem[0], rmtConfig.mem_block_num, 0);


//AXIS 1 Config

    rmt_set_source_clk(RMT_CHANNEL_1, RMT_BASECLK_MAX);
    rmtConfig.channel = RMT_CHANNEL_1;
    rmtConfig.tx_config.idle_level =  RMT_IDLE_LEVEL_LOW;
    rmtConfig.gpio_num = GPIO_NUM_16;						//Step pin AXIS 1
    rmtItem[0].level0 = rmtConfig.tx_config.idle_level;
    rmtItem[0].level1 = !rmtConfig.tx_config.idle_level;
    rmt_config(&rmtConfig);
    rmt_fill_tx_items(rmtConfig.channel, &rmtItem[0], rmtConfig.mem_block_num, 0);
/*

//AXIS 2 Config

    rmt_set_source_clk(RMT_CHANNEL_2, RMT_BASECLK_MAX);
    rmtConfig.channel = RMT_CHANNEL_2;
    rmtConfig.tx_config.idle_level =  RMT_IDLE_LEVEL_LOW;
    rmtConfig.gpio_num = GPIO_NUM_16;						//Step pin AXIS 2
    rmtItem[0].level0 = rmtConfig.tx_config.idle_level;
    rmtItem[0].level1 = !rmtConfig.tx_config.idle_level;
    rmt_config(&rmtConfig);
    rmt_fill_tx_items(rmtConfig.channel, &rmtItem[0], rmtConfig.mem_block_num, 0);

//AXIS 3 Config

    rmt_set_source_clk(RMT_CHANNEL_3, RMT_BASECLK_MAX);
    rmtConfig.channel = RMT_CHANNEL_3;
    rmtConfig.tx_config.idle_level =  RMT_IDLE_LEVEL_LOW;
    rmtConfig.gpio_num = GPIO_NUM_16;						//Step pin AXIS 3
    rmtItem[0].level0 = rmtConfig.tx_config.idle_level;
    rmtItem[0].level1 = !rmtConfig.tx_config.idle_level;
    rmt_config(&rmtConfig);
    rmt_fill_tx_items(rmtConfig.channel, &rmtItem[0], rmtConfig.mem_block_num, 0);

//AXIS 4 Config

    rmt_set_source_clk(RMT_CHANNEL_4, RMT_BASECLK_MAX);
    rmtConfig.channel = RMT_CHANNEL_4;
    rmtConfig.tx_config.idle_level =  RMT_IDLE_LEVEL_LOW;
    rmtConfig.gpio_num = GPIO_NUM_16;						//Step pin AXIS 4
    rmtItem[0].level0 = rmtConfig.tx_config.idle_level;
    rmtItem[0].level1 = !rmtConfig.tx_config.idle_level;
    rmt_config(&rmtConfig);
    rmt_fill_tx_items(rmtConfig.channel, &rmtItem[0], rmtConfig.mem_block_num, 0);

//AXIS 5 Config

    rmt_set_source_clk(RMT_CHANNEL_5, RMT_BASECLK_MAX);
    rmtConfig.channel = RMT_CHANNEL_5;
    rmtConfig.tx_config.idle_level =  RMT_IDLE_LEVEL_LOW;
    rmtConfig.gpio_num = GPIO_NUM_16;						//Step pin AXIS 5
    rmtItem[0].level0 = rmtConfig.tx_config.idle_level;
    rmtItem[0].level1 = !rmtConfig.tx_config.idle_level;
    rmt_config(&rmtConfig);
    rmt_fill_tx_items(rmtConfig.channel, &rmtItem[0], rmtConfig.mem_block_num, 0);
*/
}




//IRAM_ATTR static void onTime()(){
static void IRAM_ATTR onTime() {

 
 //portENTER_CRITICAL_ISR(&timerMux);
if (steps_0_executed < steps_0) {
 RMT.conf_ch[RMT_CHANNEL_0].conf1.mem_rd_rst = 1;
 RMT.conf_ch[RMT_CHANNEL_0].conf1.tx_start = 1;
steps_0_executed++;
fdbrt.fb0=fdbrt.fb0+dir0;
}


if (steps_1_executed < steps_1) {
 RMT.conf_ch[RMT_CHANNEL_1].conf1.mem_rd_rst = 1;
 RMT.conf_ch[RMT_CHANNEL_1].conf1.tx_start = 1;
steps_1_executed++;
fdbrt.fb1=fdbrt.fb1+dir1;

}
if (steps_2_executed < steps_2) {
 RMT.conf_ch[RMT_CHANNEL_2].conf1.mem_rd_rst = 1;
 RMT.conf_ch[RMT_CHANNEL_2].conf1.tx_start = 1;
steps_2_executed++;
fdbrt.fb2=fdbrt.fb2+dir2;

}
if (steps_3_executed < steps_3) {
 RMT.conf_ch[RMT_CHANNEL_3].conf1.mem_rd_rst = 1;
 RMT.conf_ch[RMT_CHANNEL_3].conf1.tx_start = 1;
steps_3_executed++;
fdbrt.fb3=fdbrt.fb3+dir3;
}


///*

if (steps_0_executed==steps_0 && steps_1_executed==steps_1 
    && steps_2_executed==steps_2 && steps_3_executed==steps_3)
    { 
     //fdbrt.fb0=fdbrt.fb0+steps_0_dir;
     
     // timerAlarmDisable(timer);
      //timerEnd(timer);
    //timer = NULL;
    }
//*/
//portEXIT_CRITICAL_ISR(&timerMux);
}









void setup() {

 
 // AXIS DIR PINS
 pinMode(4,OUTPUT);
 pinMode(12,OUTPUT);
 pinMode(14,OUTPUT);
 
 //REG_WRITE(GPIO_ENABLE_REG, BIT4);//Define o GPIO2 como saída
 //REG_WRITE(GPIO_ENABLE_REG, BIT15);//Define o GPIO2 como saída
 //REG_WRITE(GPIO_ENABLE_REG, BIT17);//Define o GPIO2 como saída
 
///////////////////////   encoder ///////////////////////////////

// Enable the weak pull up resistors
  ESP32Encoder::useInternalWeakPullResistors=UP;

  // Attache pins for use as encoder pins
  encoder.attachHalfQuad(15, 17);


///////////////////////////////////////////////////////////////
 
 
 
 initRMT();
 
 /***************************************** Main timer stepper *****************/ 
 
hw_timer_t * timer = NULL;
//portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;


  // Configure Prescaler to 80, as our timer runs @ 80Mhz
  // Giving an output of 80,000,000 / 80 = 1,000,000 ticks / second
  timer = timerBegin(0, 10, true);                
  timerAttachInterrupt(timer, &onTime, true);    
  // Fire Interrupt every 1m ticks, so 1s
  timerAlarmWrite(timer, 45, true);      
  timerAlarmEnable(timer);



// You can use Ethernet.init(pin) to configure the CS pin
  
  Ethernet.init(5);   // MKR ETH shield
  
//pinMode(15, OUTPUT);
//pinMode(2, OUTPUT);
  // start the Ethernet
  Ethernet.begin(mac, ip);

  // Open serial communications and wait for port to open:
 Serial.begin(230400);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }

  // Check for Ethernet hardware present
  if (Ethernet.hardwareStatus() == EthernetNoHardware) {
    Serial.println("Ethernet shield was not found.  Sorry, can't run without hardware. :(");
    while (true) {
      delay(1); // do nothing, no point running without Ethernet hardware
    }
  }
  if (Ethernet.linkStatus() == LinkOFF) {
    Serial.println("Ethernet cable is not connected.");
  }

  // start UDP
  Udp.begin(localPort);
}

IPAddress remote = Udp.remoteIP();
int packetSize;


//////////////// LOOP////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
long enc0=0;
long enc1;
long enc2;
long enc3;
void loop() {
// if there's data available, read a packet
packetSize = Udp.parsePacket();
if (packetSize) {
 
   
    // send a reply to the IP address and port that sent us the packet we received
    
    Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
   
    
    Udp.write(bufferin,sizeof(fdb));
    Udp.endPacket();
    //Udp.read(packetBuffer,50);
    Udp.read(packetBuffer,sizeof(cmd));
    memcpy(&cmd,packetBuffer,sizeof(cmd));
  
//Serial.println (cmd.pos5);
fdb={fdbrt.fb0,fdbrt.fb1,fdbrt.fb2,fdbrt.fb3,100,enc0};
//fdb={cmd.pos0,cmd.pos1,cmd.pos2,cmd.pos3,3};
memcpy(&bufferin, &fdb, sizeof(fdb));


//Serial.println (fdbrt.fb2);


//Serial.println (testch1);

////////////////////////////////////////////////////////
steps_0_executed=0;
steps_0_dir=cmd.pos0 - fdbrt.fb0;
if(steps_0_dir > 0 && dir0 == -1 ){
   REG_WRITE(GPIO_OUT_W1TC_REG, BIT4);	//GPIO4 LOW (clear) Dir Axis 0;
   dir0=1;
   steps_0_dir=0;
   
 }
 if(steps_0_dir < 0 && dir0 == 1 ){
  
   REG_WRITE(GPIO_OUT_W1TS_REG, BIT4);//GPIO4 HIGH (clear) Dir Axis 0;
   dir0=-1;
   steps_0_dir=0;
  
 }
steps_0=abs(steps_0_dir);
/////////////////////////////////////////////////////
steps_1_executed=0;
steps_1_dir=cmd.pos1 - fdbrt.fb1;
if(steps_1_dir > 0 && dir1 != 1 ){
   REG_WRITE(GPIO_OUT_W1TC_REG, BIT12);//GPIO4 LOW (clear)  Dir Axis 1;
    steps_1_dir=0;
   dir1=1;
 }
 if(steps_1_dir < 0 && dir1 != -1 ){
  
   REG_WRITE(GPIO_OUT_W1TS_REG, BIT12);//GPIO4 HIGH (clear)  Dir Axis 1;
    steps_1_dir=0;
   dir1=-1;
 }
steps_1=abs(steps_1_dir);
/////////////////////////////////////////////////////
steps_2_executed=0;
steps_2_dir=cmd.pos2 - fdbrt.fb2;
if(steps_2_dir > 0 && dir2 != 1 ){
    steps_2_dir=0;
   REG_WRITE(GPIO_OUT_W1TC_REG, BIT14);//GPIO2 LOW (clear)  Dir Axis 2;
  dir2=1;
 }
 if(steps_2_dir < 0 && dir2 != -1 ){
   steps_2_dir=0;
   REG_WRITE(GPIO_OUT_W1TS_REG, BIT14);//GPIO2 LOW (clear)  Dir Axis 2;
  dir2=-1;
 }
steps_2=abs(steps_2_dir);
////////////////////////////////////////////////////
steps_3_executed=0;
steps_3_dir=cmd.pos3 - fdbrt.fb3;
if(steps_3_dir > 0 && dir3 != 1 ){
   REG_WRITE(GPIO_OUT_W1TC_REG, BIT14);//GPIO4 LOW (clear)  Dir Axis 3;
    steps_3_dir=0;
   dir3=1;
 }
 if(steps_3_dir < 0 && dir3 != -1 ){
  
   REG_WRITE(GPIO_OUT_W1TS_REG, BIT14);//GPIO4 HIGH (clear)  Dir Axis 3;
    steps_3_dir=0;
   dir3=-1;
 }
steps_3=abs(steps_3_dir);






//Serial.println(enc0);
//Serial.println(steps_0);      
 //timerAlarmWrite(timer, 1000, true);      
 // timerAlarmEnable(timer);

 }

memcpy(&olds,&cmd, sizeof(cmd));
//fdb={888.8,444.4,111.1,0.555,3};
//delayMicroseconds(20);
enc0=encoder.getCount();
enc1=encoder.getCount();
enc2=encoder.getCount();
enc3=encoder.getCount();

}