Error while running rosserial node for motor controller
Below is my code to control DC motor of differential drive robot to communicate between ROS and Arduino.
//Library to communicate with I2C devices
#include "Wire.h"
#include <Messenger.h>
//Contain definition of maximum limits of various data type
#include <limits.h>
//Messenger object
Messenger Messenger_Handler = Messenger();
//////////////////////////////////////////////////////////////////////////////////////
//Motor Pin definition
//Left Motor
#define USE_USBCOM
#define INA_1 7
#define INB_1 12
//PWM 1 pin
#define PWM_1 5
//Right Motor
#define INA_2 11
#define INB_2 10
//PWM 2 pin
#define PWM_2 6
#define RESET_PIN 4
///////////////////////////////////////////////////////////////////////////////////////
//Motor speed from PC
//Motor left and right speed
float motor_left_speed = 0;
float motor_right_speed = 0;
/////////////////////////////////////////////////////////////////
//Setup serial, motors and Reset functions
void setup()
{
//Init Serial port with 115200 baud rate
Serial.begin(57600);
//Setup Motors
SetupMotors();
SetupReset();
//Set up Messenger
Messenger_Handler.attach(OnMssageCompleted);
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Setup Motors() function
void SetupMotors()
{
//Left motor
pinMode(INA_1,OUTPUT);
pinMode(INB_1,OUTPUT);
//Right Motor
pinMode(INA_2,OUTPUT);
pinMode(INB_2,OUTPUT);
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Setup Reset() function
void SetupReset()
{
pinMode(RESET_PIN,OUTPUT);
///Conenect RESET Pins to the RESET pin of launchpad,its the 16th PIN
digitalWrite(RESET_PIN,HIGH);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//MAIN LOOP
void loop()
{
//Read from Serial port
Read_From_Serial();
//Update motor values with corresponding speed and send speed values through serial port
Update_Motors();
delay(1000);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Read from Serial Function
void Read_From_Serial()
{
while(Serial.available() > 0)
{
int data = Serial.read();
Messenger_Handler.process(data);
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//OnMssg Complete function definition
void OnMssageCompleted()
{
char reset[] = "r";
char set_speed[] = "s";
if(Messenger_Handler.checkString(reset))
{
Reset();
}
if(Messenger_Handler.checkString(set_speed))
{
//This will set the speed
Set_Speed();
return;
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Set speed
void Set_Speed()
{
motor_left_speed = Messenger_Handler.readLong();
motor_right_speed = Messenger_Handler.readLong();
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Reset function
void Reset()
{
delay(1000);
digitalWrite(RESET_PIN,LOW);
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Will update both motors
void Update_Motors()
{
moveRightMotor(motor_right_speed);
moveLeftMotor(motor_left_speed);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Motor running function
void moveRightMotor(float rightServoValue)
{
if (rightServoValue>0)
{
digitalWrite(INA_1,HIGH);
digitalWrite(INB_1,LOW);
analogWrite(PWM_1,rightServoValue);
}
else if(rightServoValue<0)
{
digitalWrite(INA_1,LOW);
digitalWrite(INB_1,HIGH);
analogWrite(PWM_1,abs(rightServoValue));
}
else if(rightServoValue == 0)
{
digitalWrite(INA_1,HIGH);
digitalWrite(INB_1,HIGH);
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void moveLeftMotor(float leftServoValue)
{
if (leftServoValue > 0)
{
digitalWrite(INA_2,LOW);
digitalWrite(INB_2,HIGH);
analogWrite(PWM_2,leftServoValue);
}
else if(leftServoValue < 0)
{
digitalWrite(INA_2,HIGH);
digitalWrite(INB_2,LOW);
analogWrite(PWM_2,abs(leftServoValue));
}
else if(leftServoValue == 0)
{
digitalWrite(INA_2,HIGH);
digitalWrite(INB_2,HIGH);
}
}
When i start the rosserial node it throws the below error. I tried with same baud rate and adding delay in loop nothing worked. The issue is only with this sketch other sketches works perfectly.
[ERROR] [WallTime: 1475787988.134165] Unable to sync with device; possible link problem or link software version mismatch such as hydro rosserial_python with groovy Arduino
add a comment
