MegaServo and Wire(I2C)Library conflict

Forum 2005-2010 (read only) Software Bugs & Suggestions
1

I am using ubuntu 9.04 and Arduino 0017 and self made arduino clone board with atmega 328 chip
Native Servo library(renamed Megaservo) in 0017 and Wire library didn't work well with
Initialy I have been using two servos with ServoTimer1 library and Wire library for compass sensor
I wanted to add one more servo
so I changed it into native Servo library in 0017 from ServoTimer1

Then problem happens all servos are head banging and compass sensor value is set to 1304. You can see 1304 when you didn't declare Wire.begin() in setup.

and AFSoftSerial doesn't work with this code too. It makes board reset. NewSoftSerial works well with this code

Is there anyone who see the problem like this?

adding:
I tested the code in Arduino 0016 with MegaServo and Wire library
this time Wire works(compass senser shows right degree) and servos are still head banging(all servos ride same left and right zee~ zee~rythme )
Isn't it be a clue?

2

I am having the same problems here on a Roboduino board with mega168. I have 8 servos, 2 I2C sensors, one Ping sensor, one Sharp sensor. The servos are moving fantastic with the Servo library from Arduino 17, but if I enable the sensors, the servos start to move back and forth, all at once. The most weird effect is when I play a tune. The servos will move to different positions, depending on the note duration. Looks like the robot is dancing or better yet acting like it's having an epileptic episode. Everything work fine with the SoftwareServo library, but I hate that I need the refresh command and sometimes the servos don't actually go to the same angle they would if I use the new Servo library.

Question is, is it possible to fix the problem (Wire library interference)?

Thanks.

3

Do you get the problem if you use just the ping sensor? Neither the ping sensor or the Sharp IR sensor I have use I2C.

Can you post a small sketch that demonstrates the problem?

4

I wanted to add one more servo

How are you supplying power to the servos?

Lefty

5

Looks like it's not only the I2C that causes servos to twitch. Below is part of the sketch I'm using. I put everything in the same sketch for easier reading (I use several tabs and libs). If you look at the main loop, you'll see some functions that do some actions based on different sensors readings. If I comment out all functions, leaving active only the state LED and delay(1000), the servos stay still. If I activate any of the functions, the servos twitch. So even reading the analog sensor or the Ping sensor create twitching. At first I was testing only the temperature array when I observed the problem. If I use the SoftwareServo library, the servos stay still, but I have to make sure I don't use delays greater than 20 ms between ServoRefresh commands.

Will it be possible to use Timer2 to set up an interrupt to issue the ServoRefresh command every 20ms? If yes, how do I set the Timer2 for that?

Thanks for your help!

6 
// Includes
#include <SoftwareSerial.h>
#include <Servo.h>
#include <EEPROM.h>
#include <Wire.h>

// I2C slaves addresses
#define MotorController 60
#define Thermo          208>>1
#define Compass         42>>1

// I2C commands
#define Forward 1
#define Reverse 2
#define Left    3
#define Right   4
#define Stop    5

// LCD pins
#define txPin 2         // digital pin 2
#define rxPin 13        // digital pin 13, not used for the LCD

// Servo pins
#define TiltPin 3       // digital pin 3, PWM pin  (6V power)
#define RshoulderPin 5  // digital pin 5, PWM pin  (6V power)
#define LshoulderPin 6  // digital pin 6, PWM pin  (6V power)
#define ScanPin 7       // digital pin 7 (regulated 5V power)
#define PanPin 8        // digital pin 8 (regulated 5V power)
#define RarmPin 9       // digital pin  9, PWM pin (6V power)
#define LarmPin 10      // digital pin 10, PWM pin (6V power)
#define WaistPin 11     // digital pin 11, PWM pin (6V power)

// Media pins
#define speakerOut 12   // digital pin 12
#define ledPin 13       // digital pin 13                          

// Sensor pins
#define PingPin 4       // digital pin 4
#define BattSensor 0    // analog pin 0
#define TouchPin 1      // analog pin 1
#define IR_Pin  16      // digital pin 16 (analog pin 2)
#define SharpPin 3      // analog pin 3

// Moves
byte Stand_By = 0;
byte Rarm_Wave = 1;
byte Pick_Up_Object = 2;
byte Release = 3;

// Servos
Servo Scan;
Servo Waist;
Servo Lshoulder;
Servo Rshoulder;
Servo Larm;
Servo Rarm;
Servo Pan; 
Servo Tilt;

// Software serial
SoftwareSerial LcdSerial =  SoftwareSerial(rxPin, txPin);

// Sensor Variables
unsigned int PingDistance = 0;
byte IrDistance = 0;
byte TouchVal = 0;
int temperature[] = {0,0,0,0,0,0,0,0,0};                 // Array to hold temperature data

// servo variables
#define center  80
byte direction = 1;
byte object = 0;

//servos:    stepDeleay, Waist, Lshould, Rshould, Larm, Rarm, Tilt, Pan
byte oldPos[8] = {0,      70,     180,     180,    80,   90,   80,   80};

byte Speed = 0;
byte Distance = 0;
byte Degrees = 0;
byte Object_here = 3; // object is closer than 3
byte moveNumber = 0;
byte sv = 1;
boolean teachMode = 0;
byte pos = 80;
byte index = 1;
byte pos_delay = 0;
boolean move = 0;
boolean stored = 0;
boolean state = 0;

// Play Melody variables
byte names[] = {'c', 'd', 'e', 'f', 'g', 'a', 'b', 'C'};  
int tones[] = {1915, 1700, 1519, 1432, 1275, 1136, 1014, 956};
byte melody[] = "2d2a1f2c2d2a2d2c2f2d2a2c2d2a1f2c2d2a2a2g2p8p8p8p";
// count length: 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0
//                                10                  20                  30
int count = 0;
int count2 = 0;
int count3 = 0;
int MAX_COUNT = 24;


void setup() { 
  Wire.begin(); // join i2c bus (address optional for master)

  //hardware serial
  Serial.begin(9600);
  
  //software serial for LCD
  pinMode(rxPin, INPUT);
  pinMode(txPin, OUTPUT);
  LcdSerial.begin(9600);
  LCDposition(1, 1);
  LcdSerial.print("Mini Eric");
  LCDposition(2, 5);
  LcdSerial.print("robot");
  delay(1000);    //wait a second
  LCDclear();

  //media
  pinMode(ledPin, OUTPUT); 
  pinMode(speakerOut, OUTPUT);  

  //sensors
  pinMode(PingPin, OUTPUT); 
  digitalWrite(PingPin, LOW);
  
  //servos
  Scan.attach(ScanPin);
  Pan.attach(PanPin);
  Tilt.attach(TiltPin);
  Waist.attach(WaistPin);
  Lshoulder.attach(LshoulderPin);
  Rshoulder.attach(RshoulderPin);
  Larm.attach(LarmPin);
  Rarm.attach(RarmPin);

  Pan.write(center); // 0 - 80 - 170
  Tilt.write(center); // 0 - 80 - 180
  Scan.write(center); // 40 - 85 - 130 ; 40 = left, 130 = right
  Larm.write(center); // 0 - 80 - 180
  Rarm.write(center+10); // 0 - 80 - 180; 0 = 
  Lshoulder.write(center+100); // 0 - 80 - 180
  Rshoulder.write(center+100); // 0 - 80 - 180 ; 0 = Up, 180 = Down
  Waist.write(center-10);  // 70 = Up, 150 = Down

}

// Main Loop
void loop(){
    state = !state;
    digitalWrite(ledPin, state);
    Trigger_Wave_Hand();
    //Trigger_Music();
    Trigger_Pick_Object();
    //Trigger_Release();
    delay(1000);          // delay a second to see the values
    Find_Hot_Object();
    delay(1000);          // delay a second to see the values
}  

int Read_Ping_Sensor() {
  //trigger the sensor
  int Ping = 0;
  pinMode(PingPin, OUTPUT);
  digitalWrite(PingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(PingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(PingPin, LOW);
  //receive the echo
  pinMode(PingPin, INPUT);
  Ping = pulseIn(PingPin, HIGH);
  return Ping/29/2;                // convert to cm
}

int Read_Sharp_Sensor() {
  int value = 0;
  value = analogRead(SharpPin);
  return 509.88*pow(value,-.9154);   // convert to cm
}

int Read_Touch_Sensor() {
  int Touch=analogRead(TouchPin);
  return Touch;
}

void Read_Thermopile_Array(){
  for(int i = 0; i < 9; i++){                          // Loops and stores temperature data in array
    temperature[i] = getTemp(i+1);
  }
}

int getTemp(int reg){                                 // Function to receive one byte of data from TPA81
  Wire.beginTransmission(Thermo);                     // Begin communication with TPA81
  Wire.send(reg);                                     // Send reg to TPA81
  Wire.endTransmission();
  Wire.requestFrom(Thermo, 1);                        // Request 1 byte
  while(Wire.available() < 1);                        // Wait for byte to arrive
  int data = Wire.receive();                          // Get byte
  return(data);                                       // return byte
}

void Trigger_Wave_Hand() {
  PingDistance = Read_Ping_Sensor();
      LCDposition(1, 1);
      LcdSerial.print("Ping  Dist: ");
      LcdSerial.print(PingDistance, DEC);
      LcdSerial.print("  ");
  if (PingDistance < 40 && PingDistance > 30){
     Play_Move(Rarm_Wave);
  } 
}

void Trigger_Music() {
  PingDistance = Read_Ping_Sensor();
      LCDposition(1, 1);
      LcdSerial.print("Ping Dist: ");
      LcdSerial.print(PingDistance, DEC);
      LcdSerial.print("  ");
  if (PingDistance < 30 && PingDistance > 20)
  {
    Play_Melody(); 
  }
}

void Trigger_Pick_Object() {
    IrDistance = Read_Sharp_Sensor();
      LCDposition(2, 1);
      LcdSerial.print("Sharp Dist: ");
      LcdSerial.print(IrDistance, DEC);
      LcdSerial.print("  ");
    if (IrDistance < Object_here){
      Play_Move(Pick_Up_Object);
      object = 1;
    }
}

void Trigger_Release() {
    PingDistance = Read_Ping_Sensor();
      LCDposition(1, 1);
      LcdSerial.print("Ping Dist: ");
      LcdSerial.print(PingDistance, DEC);
      LcdSerial.print("  ");
    if (PingDistance < 20) {
      Play_Move(Release);
      object = 0;
    }
}

void Find_Hot_Object() {
  Read_Thermopile_Array();
  byte cluster = 0;
  int hotObject = temperature[0];  // ambient temperature
  for(byte i = 1; i < 9; i++){  
    if (temperature[i] > hotObject) {
      hotObject = temperature[i]; // store the highest temperature
      cluster = i; // store the location of the hottest spot
    }
  }
      LCDposition(1, 1);
      LcdSerial.print("Hot Object: ");
      LcdSerial.print(hotObject, DEC);
      LcdSerial.print("  ");
      LCDposition(2, 1);
      LcdSerial.print("in cluster ");
      LcdSerial.print(cluster, DEC);
      LcdSerial.print("   ");
}

void Play_Move(byte moveNumber) {
   LCDposition(1, 1);
   LcdSerial.print("Playing         ");
   LCDposition(2, 1);
   LcdSerial.print("Move ");
   LcdSerial.print(move, DEC);
   LcdSerial.print("          ");
  byte moveStart = EEPROM.read(490+(2*moveNumber));
  byte moveEnd = EEPROM.read(491+(2*moveNumber));
  for (byte i = moveStart; i <= moveEnd; i+=2){
    sv = EEPROM.read(i);
    pos = EEPROM.read(i+1); 
    switch (sv) {
      case 0:
        for (int u=1; u<=pos; u++){
          StepDelay();
        }
        break;
      case 1:
        Waist.write(pos);
        break;
      case 2:
        Lshoulder.write(pos);
        break;
      case 3:
        Rshoulder.write(pos);
        break;
      case 4:
        Larm.write(pos);
        break;
      case 5:
        Rarm.write(pos);
        break;
      case 6:
        Tilt.write(pos);
        break;
      case 7:
        Pan.write(pos);
        break;
    }
  }
}

void Play_Melody() {
  digitalWrite(speakerOut, LOW);     
  for (count = 0; count < MAX_COUNT; count++) {
    for (count3 = 0; count3 <= (melody[count*2] - 48) * 30; count3++) {
      for (count2=0;count2<8;count2++) {
        if (names[count2] == melody[count*2 + 1]) {       
          digitalWrite(speakerOut,HIGH);
          delayMicroseconds(tones[count2]);
          digitalWrite(speakerOut, LOW);
          delayMicroseconds(tones[count2]);
        } 
        if (melody[count*2 + 1] == 'p') {
          // make a pause of a certain size
          digitalWrite(speakerOut, 0);
          delayMicroseconds(500);
        }
      }
    }
  }
}

void StepDelay() {
    for (int t=0; t<=10; t++){
      delay(20);
    }
}

 void LCDposition (byte line, byte column) {
  LcdSerial.print(line, BYTE);       // LCD line 
  LcdSerial.print(column, BYTE);     // LCD column 
}

void LCDclear () {
  LcdSerial.print(5, BYTE);          // select LCD command mode 
  LcdSerial.print(1, BYTE);          // clear screen
}
7

The 0017 Servo library relies on interrupts to time the pulses and your sketch spends a lot of time with interrupts disabled. Calls to delayMicroseconds currently disables interrupts (although the Arduino team is considering modifying this in a future release) and your melody function and the timing functions from the sensor do turn interrupts off and this is probably what is disrupting the servos.

The earlier servo library will not be affected by interrupts but it is limited to two servos so that is not any help for your application. The software Servo library will be less susceptible to this problem and if you really need to run everything on a single board you may want to see if you can make your code work with softwareServo. Unfortunately, an application as complex as yours is not easy to implement on a single board, but perhaps that's part of the challenge that makes this hobby fun. If it was my project I would put the 8 servos on a cheap runtime board and connect it to the app running on your existing board using a ttl serial interface.

8

Thanks, I get the idea. But like you said, the fun is to have the servos on this board. I am working on a dual motor controller with I2C interface, perhaps later on I'll separate the servos too, but for now, they will have to work on the main board.

Will it be possible to use Timer2 to set up an interrupt to issue the ServoRefresh command every 20ms? If yes, how do I set the Timer2 for that?

Any advice on this part?

9

Refreshing the servos in a interrupt will freeze out all your other code that will be trying to time things during the two milliseconds or so that the servo pulses are generated. So although the servos may be more stable, the readings from the sensors and the output from the melody routine will be disrupted.