I've been trying to troubleshoot a strange problem for a few days now. I've got a ROS Kinetic node running on a Raspberry Pi 3 (Ubuntu Mate). I'm utilizing the rosserial_arduino package to handle communication between the Arduino and my master node. Current setup is basically an Arduino Mega 2560 with a SEED Studio Motor Shield powering 2 wheels, plus an ITG3205/ADXL345/HMC5883L 9-DOF chip. I currently have two publishers set up - one outputting twistStamped messages and one chatter topic outputting loop iterations and debug information. I've got one subscriber set up receiving an int16 message from a homespun teleop script - basically listening for keys 2,4,5,6,8.
Everything works perfectly if there is no power to the motor shield (I'm using an external 6v pack). I am able to echo out twist messages which are directly linked to the xyz axis on my gyro and accel. My chatter topic shows me my current loop iteration and when I send key commands I see them echoing out as well. Similarly, if the motor power is off, I still get LED signals on the shield showing the current motor states.
If I turn on power supply to the motors and send a signal other than "full stop" which is key 5, everything hangs up. it might take a few loops but it will hang within 1-4 seconds of the motor signal being sent and received. Resetting the board alleviates the issue and all my topics begin to send/receive again.
I have traced this problem down to the ITG3205 portion that performs the I2C communication (itgRead function) - specifically after the first call to Wire.endTransmission().
Any ideas? I've tried swapping out the arduino, the shield, and the 9-dof chip with no success. As long as i've got wheels are spinning, everything hangs on gyro read. I've included relevant code snippets below.
**Motor Shield code. Only included forward for brevity.**
void SeeedMotorV2::allStop()
{
digitalWrite(leftmotorspeed,0);
digitalWrite(rightmotorspeed,0);
digitalWrite(leftmotorForward,LOW);
digitalWrite(rightmotorBackward,LOW);
digitalWrite(leftmotorBackward,LOW);
digitalWrite(rightmotorForward,LOW);
}
void SeeedMotorV2::forward(int speed)
{
allStop();
analogWrite(leftmotorspeed,speed); //pin 10
analogWrite(rightmotorspeed,speed);//pin 9
digitalWrite(leftmotorBackward,LOW);//pin 13
digitalWrite(rightmotorBackward,LOW);//pin 11
digitalWrite(leftmotorForward,HIGH);//pin 12
digitalWrite(rightmotorForward,HIGH);//pin 8
}
**ITG3205 code snippet.**
unsigned char ITG3205::itgRead(char address, char registerAddress)
{
//This variable will hold the contents read from the i2c device.
unsigned char data=0;
//Send the register address to be read.
Wire.beginTransmission(address);
//Send the Register Address
Wire.write(registerAddress);
//End the communication sequence.
Wire.endTransmission();
//Ask the I2C device for data
Wire.beginTransmission(address);
Wire.requestFrom(address, 1);
//Wait for a response from the I2C device
if(Wire.available()){
//Save the data sent from the I2C device
data = Wire.read();
}
//End the communication sequence.
Wire.endTransmission();
//Return the data read during the operation
return data;
}
//itgAddress = 0x68
//GYRO_XOUT_H = 0x1D
//GYRO_YOUT_L = 0x1DE
int ITG3205::readX(void)
{
int data=0;
data = itgRead(itgAddress, GYRO_XOUT_H)<<8;
data |= itgRead(itgAddress, GYRO_XOUT_L);
return data;
}
**Main Sketch. Shortened to code in question.**
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
char base_link[] = "/base_link";
SeeedMotorV2 motors;
ITG3205 gyroscope;
unsigned long timer;
int cntr = 0;
char loops[25];
void cmd_int_vel(const std_msgs::Int16& message)
{
switch(message.data)
{
case 1:
sendMessage("1");
break;
case 2:
sendMessage("2");
motors.backward();
break;
case 3:
sendMessage("3");
break;
case 4:
sendMessage("4");
motors.left();
break;
case 5:
sendMessage("5");
motors.allStop();
break;
case 6:
sendMessage("6");
motors.right();
break;
case 7:
sendMessage("7");
break;
case 8:
sendMessage("8");
motors.forward();
break;
case 9:
sendMessage("9");
break;
default:
sendMessage("Unknown Velocity Command");
break;
}
}
ros::Subscriber int_movement("cmd_int_vel", &cmd_int_vel);
ros::NodeHandle nh;
std_msgs::String str_msg;
ros::Publisher chatter("chatter", &str_msg);
geometry_msgs::TwistStamped twistMsg;
ros::Publisher twists("twists", &twistMsg);
void setup()
{
nh.getHardware()->setBaud(115200);
nh.initNode();
nh.advertise(chatter);
nh.advertise(twists);
nh.subscribe(int_movement);
motors.begin();
timer = 0;
gyroscope.begin();
gyroscope.calibrate(100, 10);
delay(10);
nh.spinOnce();
}
void loop()
{
sprintf(loops, "%i", cntr);
cntr++;
sendMessage(loops);
Vector acc2 = accelerometer.readNormalize();
Vector gyr = gyroscope.readNormalize();
twistMsg.header.frame_id = base_link;
twistMsg.twist.linear.x = acc2.XAxis;
twistMsg.twist.linear.y = acc2.YAxis;
twistMsg.twist.linear.z = acc2.ZAxis;
twistMsg.twist.angular.x = gyr.XAxis;
twistMsg.twist.angular.y = gyr.YAxis;
twistMsg.twist.angular.z = gyr.ZAxis;
twistMsg.header.stamp = nh.now();
twists.publish(&twistMsg);
nh.spinOnce();
delay(10);
}
void sendMessage(char* message)
{
str_msg.data = message;
chatter.publish(&str_msg);
}
↧