pawlass's profile - activity

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 ...