ros melodic chefbot encoders test

ros melodic chefbot encoders test

chefbot_encoders_test1

#include <PinChangeInterrupt.h>

#include <ros.h>
#include <std_msgs/Int32.h>

ros::NodeHandle nh;

std_msgs::Int32 int_encoder_msg1;
ros::Publisher encoder_data1("encl", &int_encoder_msg1);

std_msgs::Int32 int_encoder_msg2;
ros::Publisher encoder_data2("encr", &int_encoder_msg2);

///////////////////////////////////////////////////////////////
//Encoder pins definition

// Left encoder

#define Left_Encoder_PinA 2
#define Left_Encoder_PinB 22

volatile long Left_Encoder_Ticks = 0;
volatile bool LeftEncoderBSet;

//Right Encoder

#define Right_Encoder_PinA 3
#define Right_Encoder_PinB 23
volatile long Right_Encoder_Ticks = 0;
volatile bool RightEncoderBSet;


void setup() {
SetupEncoders();
nh.initNode();
nh.advertise(encoder_data1);
nh.advertise(encoder_data2);
}

void loop() {
Update_Encoders();
nh.spinOnce();
delay(1);
}

void SetupEncoders() {
// Quadrature encoders
// Left encoder
pinMode(Left_Encoder_PinA, INPUT); // sets pin A as input 
pinMode(Left_Encoder_PinB, INPUT); // sets pin B as input
//Attaching interrupt in Left_Enc_PinA.
attachInterrupt(digitalPinToInterrupt(Left_Encoder_PinA), do_Left_Encoder, RISING);

// Right encoder
pinMode(Right_Encoder_PinA, INPUT); // sets pin A as input
pinMode(Right_Encoder_PinB, INPUT); // sets pin B as input
//Attaching interrupt in Right_Enc_PinA.
attachInterrupt(digitalPinToInterrupt(Right_Encoder_PinA), do_Right_Encoder, RISING); 
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Update both encoder value through serial port
void Update_Encoders() {
Serial.print("e");
Serial.print("\t");
Serial.print(Left_Encoder_Ticks);
Serial.print("\t");
Serial.print(Right_Encoder_Ticks);
Serial.print("\n");


int_encoder_msg1.data = Left_Encoder_Ticks;
encoder_data1.publish(&int_encoder_msg1);

int_encoder_msg2.data = Right_Encoder_Ticks;
encoder_data2.publish(&int_encoder_msg2); 
}


//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//do_Left_Encoder() Definitions
void do_Left_Encoder() {
// Test transition; since the interrupt will only fire on 'rising' we don't need to read pin A
LeftEncoderBSet = digitalRead(Left_Encoder_PinB); // read the input pin
Left_Encoder_Ticks -= LeftEncoderBSet ? -1 : +1;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//do_Right_Encoder() Definitions

void do_Right_Encoder() {
RightEncoderBSet = digitalRead(Right_Encoder_PinB); // read the input pin
Right_Encoder_Ticks += RightEncoderBSet ? -1 : +1; 
}

terminal 1

roscore

terminal2

rosrun rosserial_python serial_node.py /dev/ttyUSB0

terminal3

rostopic echo encl

rostopic echo encr

rotate the each wheels by hand