Jump to content
43oh

Getting stcuked in encoder interrupt


Recommended Posts

Hello,

I am building a differential drive robot based on ROS and Tivac tm4c129xl as the embedded controller and I am using rosserial_tivac package to communicate between ROS and tivac through serial communication every peiece of code is working fine beacuse I tested them all seperately

The problem is that I have two interrupts for each wheel to read the encoder ticks and send these ticks to ROS through Serial port what cause a problem that the wheels will continue rotating and the interrupt will be raised always and the program will execute the interrupt over and over again I read something about writing a timer interrupt but I didn't get the idea and did not found enough docs about it so how shall I modify my code to be able to get out of the interrupt and execute the whole commands

 

the code:

 

#include <ros.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Int16.h>
#include <std_msgs/Float32.h>

float left_motor_speed = 0;
float right_motor_speed = 0;

// ------------ ROS --------------- //

ros::NodeHandle nh;//ros node handle object
std_msgs::Int16 enLticks;
std_msgs::Int16 enRticks;



void lCb(const std_msgs::Float32& msg){
  left_motor_speed = msg.data;
}// end of the left motor callback function

void rCb(const std_msgs::Float32& msg){
  right_motor_speed = msg.data;
}// end of the right motor callback function

ros::Subscriber<std_msgs::Float32> sL("lmotor_cmd", &lCb);
ros::Subscriber<std_msgs::Float32> sR("rmotor_cmd", &rCb);

ros::Publisher pL("lwheel", &enLticks);
ros::Publisher pR("rwheel", &enLticks);

// ---------- Tiva C pins ------------- //


// ENCODERS
#define Left_Encoder_PinA PF_3
#define Left_Encoder_PinB PG_0
volatile long Left_Encoder_Ticks = 0;
volatile bool LeftEncoderBSet;

#define Right_Encoder_PinA PF_1
#define Right_Encoder_PinB PF_2
volatile long Right_Encoder_Ticks = 0;
volatile bool RightEncoderBSet;

//MOTORS
#define A_L PA_6
#define B_L PD_7
#define PWM_L PB_2

#define A_R PM_5
#define B_R PM_4
#define PWM_R PB_3


// *********************************************** //
void setup() {
  
  setupROS();

  setupEncoders();

  setupMotors();

}
// *********************************************** //

void setupROS(){
  
  nh.initNode();
  nh.subscribe(sL);
  nh.subscribe(sR);
  nh.advertise(pL);
  nh.advertise(pR);

}//end of the setup ROS function

void setupEncoders(){
  
  pinMode(Left_Encoder_PinA, INPUT_PULLUP); 
  pinMode(Left_Encoder_PinB, INPUT_PULLUP);
  //Attaching interrupt in Left_Enc_PinA.
  attachInterrupt(Left_Encoder_PinA, do_Left_Encoder, RISING);

  pinMode(Right_Encoder_PinA, INPUT_PULLUP);
  pinMode(Right_Encoder_PinB, INPUT_PULLUP);
  //Attaching interrupt in Right_Enc_PinA.
  attachInterrupt(Right_Encoder_PinA, do_Right_Encoder, RISING); 

}// end of the setup Encoders Function

void setupMotors(){

  pinMode(A_L,OUTPUT);
  pinMode(B_L,OUTPUT);
  pinMode(PWM_L,OUTPUT);
  
  pinMode(A_R,OUTPUT);
  pinMode(B_R,OUTPUT);
  pinMode(PWM_R,OUTPUT);

}// end of the setupMotors Function

// *********************************************** //
void loop() {
  nh.spinOnce();
  do_Left_Encoder();
  do_Right_Encoder();

  moveLeftMotor(left_motor_speed);
  moveRightMotor(right_motor_speed);
  
  nh.spinOnce();
  delay(10);
}// end of the main function
// ********************************************** //



//############################################################//
void moveLeftMotor(float PWM_val){
  if (PWM_val>0)
  {
       
 digitalWrite(A_L,HIGH);
 digitalWrite(B_L,LOW);
 analogWrite(PWM_L,PWM_val);
 nh.logdebug("l forward");
    
  }
  else if(PWM_val<0)
  {
 digitalWrite(A_L,LOW);
 digitalWrite(B_L,HIGH);
 analogWrite(PWM_L,abs(PWM_val));
 nh.logdebug("l back");
 
  }
  
  else if(PWM_val == 0)
  {
 digitalWrite(A_L,HIGH);
 digitalWrite(B_L,HIGH);
 nh.logdebug("l stop");
    
    
  }
  
}
//############################################################//



//############################################################//
void moveRightMotor(float PWM_val){
  
  if (PWM_val>0)
  {
       
 digitalWrite(A_R,HIGH);
 digitalWrite(B_R,LOW);
 analogWrite(PWM_R,PWM_val);
 nh.logdebug("r forward");
    
  }
  else if(PWM_val<0)
  {
 digitalWrite(A_R,LOW);
 digitalWrite(B_R,HIGH);
 analogWrite(PWM_R,abs(PWM_val));
 nh.logdebug("r back");
  }
  
  else if(PWM_val == 0)
  {
 digitalWrite(A_R,HIGH);
 digitalWrite(B_R,HIGH);
 nh.logdebug("stop");
    
  }


}// end of the move right motor function
//############################################################//

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;
  enLticks.data = Left_Encoder_Ticks;
  pL.publish(&enLticks);
   
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//do_Right_Encoder() Definitions

void do_Right_Encoder()
{
  
  RightEncoderBSet = digitalRead(Right_Encoder_PinB);   // read the input pin
  Right_Encoder_Ticks += RightEncoderBSet ? -1 : +1;
  enRticks.data = Right_Encoder_Ticks;
  pR.publish(&enRticks);
}

 

Link to post
Share on other sites

Join the conversation

You can post now and register later. If you have an account, sign in now to post with your account.

Guest
Reply to this topic...

×   Pasted as rich text.   Paste as plain text instead

  Only 75 emoji are allowed.

×   Your link has been automatically embedded.   Display as a link instead

×   Your previous content has been restored.   Clear editor

×   You cannot paste images directly. Upload or insert images from URL.

×
×
  • Create New...