Hi everyone,

I would like to control the motor/servo motor using ros_control. I did courses at robotingiteacademy, but I can't understand practically what I should do now. I need information how to read and transmit data from the encoder to ROS and how to send data to the engine to be able to control it. I would be grateful for also example. I put my code.

#define ROSSERIAL_ARDUINO_TCP
#include <Timers.h>
#include <ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Float64.h>

byte mac[]= {
   0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED};

IPAddress ip(10,3,2,25);
IPAddress server(10,3,2,51);
// Set the rosserial socket server port
const uint16_t serverPort = 11411;

void ros_set_current_limit( const std_msgs::Float64& cmd_msg); void ros_set_temp_limit( const std_msgs::Float64& 
cmd_msg); void ros_set_joint_min_position( const std_msgs::Float64& cmd_msg); void ros_set_joint_max_position( 
const std_msgs::Float64& cmd_msg); void ros_set_joint_position( const std_msgs::Float64& cmd_msg); float 
read_ADC(int pin, int ilosc_odczytow=1, bool odrzuc_brzegowe=0);

// CONSTANTS

const byte pin_TE   = A1; 

// VARIABLES
// ROS

ros::NodeHandle nh;
std_msgs::Float64 adc_msg;

Timer timer_adc_cs;

float odczytADC_CS  = 0.0; // aktualna wartosc czujnika pradu float odczytADC_TE  = 0.0;
temperatury float odczytADC_EN  = 0.0;

// Wartosci brzegowe
float max_current   =  10.0;

bool wystapila_komunikacja = 0;
- SECURITY

// Publishers
ros::Publisher publish_te("/sensors/temp/get",    &adc_msg); ros::Publisher publish_cs("/sensors/current/get", 
&adc_msg); ros::Publisher publish_en("/sensors/encoder/get", &adc_msg);

// Subscribers
ros::Subscriber <std_msgs::Float64>
subscriber_set_current_limit("/sensors/current/limit/set",
ros_set_current_limit);
ros::Subscriber <std_msgs::Float64>
subscriber_set_temp_limit("/sensors/temp/limit/set", ros_set_temp_limit); ros::Subscriber <std_msgs::Float64> 
subscriber_set_joint_position("/joint_position/set",
ros_set_joint_position);
ros::Subscriber <std_msgs::Float64>
subscriber_set_joint_min_position("/joint_position/min/set",
ros_set_joint_min_position);
ros::Subscriber <std_msgs::Float64>
subscriber_set_joint_max_position("/joint_position/max/set",
ros_set_joint_max_position);

// SETUP

void setup() {
   Serial.begin(115200);

   Ethernet.begin(mac, ip);

   Serial.println("aa");
   // DC Motor D9 (16bit), D4, D7
   // analogWriteResolution(12);

   pinMode(pin_PWM, OUTPUT);
   pinMode(pin_DCL, OUTPUT);
   pinMode(pin_DCR, OUTPUT);
   // // ADC A0, A1, A2
   pinMode(pin_CS, INPUT);
   pinMode(pin_TE, INPUT);
   pinMode(pin_EN, INPUT);

   nh.getHardware()->setConnection(server, serverPort);
   nh.initNode();

   delay(1000);
   nh.advertise(publish_cs);
   nh.advertise(publish_te);
   nh.advertise(publish_en);

   nh.subscribe(subscriber_set_temp_limit);
   nh.subscribe(subscriber_set_current_limit);
   nh.subscribe(subscriber_set_joint_position);
   nh.subscribe(subscriber_set_joint_min_position);
   nh.subscribe(subscriber_set_joint_max_position);

   delay(1000);

   timer_adc_cs.begin(FREQ(czestotliwosc_ADC_CS));
   timer_adc_te.begin(FREQ(czestotliwosc_ADC_TE));
   timer_adc_en.begin(FREQ(czestotliwosc_ADC_EN));

   timer_sec.begin(FREQ(czestotliwosc_SEC));
}

// LOOP
void loop() {
// ADC READ - temperature sensor

    if (timer_adc_te.available())ty
    {
      // int bledy = timer_adc_cs.errors();
      odczytADC_TE = read_ADC(pin_TE, 10, 1);
      // Wyslanie komunikatu ROS
      adc_msg.data = odczytADC_TE;
      publish_te.publish(&adc_msg);
      nh.spinOnce();

  //    timer_adc.resetErrors();
    }

  // ADC READ - current sensor
    if (timer_adc_cs.available())
    {
      // int bledy = timer_adc_cs.errors();
      odczytADC_CS = read_ADC(pin_CS, 10, 1);
      adc_msg.data = odczytADC_CS;
      // Wyslanie komunikatu ROS
      publish_cs.publish(&adc_msg);
      nh.spinOnce();

  //    timer_adc.resetErrors();
    }

    // ADC READ - motor encoder
    if (timer_adc_en.available())
    {
      // int bledy = timer_adc_cs.errors();
      odczytADC_EN = read_ADC(pin_EN, 10, 1);
      // Konwersja z ADC na wartosc odchylenia
      // adc_msg.data = 1.456;
      adc_msg.data = (odczytADC_EN / 1024) * 100;
      // Wyslanie komunikatu ROS
      publish_en.publish(&adc_msg);
      nh.spinOnce();
  //    timer_adc.resetErrors();
    }

   // SECURITY
   if (timer_sec.available()) // Gdy przekroczono time odczytu ADC, uruchom odczyty
   {
   }
}

// FUNKCJE
void helper_sortowanie(int a[], int size) {
   for ( int i = 0 ; i < ( size - 1 ) ; i++)
   {
     for ( int o = ( i + 1 ) ; o < size ; o++ )
     {
       if(a[o] > a[o+1])
       {
         int t = a[o];
         a[o] = a[o+1];
         a[o+1] = t;
       }
     }
   }
}

float read_ADC(int pin, int ilosc_odczytow, bool odrzuc_brzegowe) {
   int odczyty_ADC[ilosc_odczytow];
   float srednia = 0.0;

   // odczytanie n wynikow
   for (int i = 0; i < ilosc_odczytow; i = i + 1)
   {
     odczyty_ADC[i] = analogRead(pin);
   }

   if (odrzuc_brzegowe == 1 && ilosc_odczytow > 2)
   {
     // posortowanie
     helper_sortowanie(odczyty_ADC, ilosc_odczytow);

     // odrzucenie skrajnych i usrednienie
     for (int i = 1; i < ilosc_odczytow - 1; i = i + 1)
     {
       srednia = srednia + odczyty_ADC[i];
     }

     return srednia / (ilosc_odczytow - 2);
   }
   else
   {
     // przelicz srednia dla wszystkich
     for (int i = 0; i < ilosc_odczytow; i = i + 1)
     {
       srednia = srednia + odczyty_ADC[i];
     }
     return  srednia / ilosc_odczytow;
   }

   return 0;
}

void ros_set_joint_min_position( const std_msgs::Float64& cmd_msg) {
   min_position = cmd_msg.data;
}

void ros_set_joint_max_position( const std_msgs::Float64& cmd_msg) {
   max_position = cmd_msg.data;
}

void ros_set_temp_limit( const std_msgs::Float64& cmd_msg) {
   max_temp = cmd_msg.data;
}

void ros_set_current_limit( const std_msgs::Float64& cmd_msg) {
   max_current = cmd_msg.data;
}

void ros_set_joint_position( const std_msgs::Float64& cmd_msg) {
     // analogWrite(pin_PWM, 2550);
     // digitalWrite(pin_DCL, HIGH);
     // digitalWrite(pin_DCR, LOW);

   if (cmd_msg.data > 0) {
     // pwmWrite(pin_PWM, cmd_msg.data);
     analogWrite(pin_PWM, cmd_msg.data);
     digitalWrite(pin_DCL, HIGH);
     digitalWrite(pin_DCR, LOW);

   } else if (cmd_msg.data < 0) {
     // pwmWrite(pin_PWM, cmd_msg.data * -1);
     analogWrite(pin_PWM, cmd_msg.data * -1);
     digitalWrite(pin_DCL, LOW);
     digitalWrite(pin_DCR, HIGH);

   } else {
     // pwmWrite(pin_PWM, 0);
     analogWrite(pin_PWM, 0);
     digitalWrite(pin_DCL, HIGH);
     digitalWrite(pin_DCR, HIGH);

   }

   // adc_msg.data = cmd_msg.data;
   // publish_en.publish(&adc_msg);
   // nh.spinOnce();
}