I2C and servo conflict?

topherhunter · December 31, 2021, 9:36pm

I have a robot with four servos, one stepper, and six DC motors. Everything works great from a Mega board, until I try to add a secondary Nano board to run some lights and sound. It seems that when I add in the I2C function, the servos start picking up random noise or erroneous commands, as they start twitching erratically.

I've tried going through the libraries themselves to see if I2C is getting called at some point, hence creating a conflict, and can't find anything. But perhaps I'm missing something?

I'm using the following libraries:

Servo.h, ServoEasing.h, IBusBM.h, AccelStepper.h, Wire.h

UKHeliBob · December 31, 2021, 9:39pm

Are you sure that the pins you are using for I2C are not already used for some other purpose in the sketch ?

topherhunter · December 31, 2021, 9:41pm

Yes, I've checked. The SDA and SCL pins are free on both the boards.

anon73444976 · December 31, 2021, 9:43pm

We certainly are.

groundFungus · January 2, 2022, 4:25pm

It would make things easier if you would post your code here. Read the forum guidelines to see how to properly post code and some hints on how to get the most from this forum.
Use the IDE autoformat tool (ctrl-t or Tools, Auto format) before posting code in code tags.

Post a schematic showing how the project should be wired. Show the power supplies.

anon73444976 · January 3, 2022, 1:41pm

Well, that didn't last long.

topherhunter · January 3, 2022, 1:48pm

#include <Servo.h>
#include <ServoEasing.h>
#include <IBusBM.h>
#include <AccelStepper.h>
#include <Wire.h>

// pins 2-13 control the DC motors

#define motorW1_IN1 6
#define motorW1_IN2 7
#define motorW2_IN1 4
#define motorW2_IN2 5
#define motorW3_IN1 2
#define motorW3_IN2 3
#define motorW4_IN1 13
#define motorW4_IN2 10
#define motorW5_IN1 8
#define motorW5_IN2 9
#define motorW6_IN1 11
#define motorW6_IN2 12

// Attach SDA and SCL to pins 20 and 21

ServoEasing servoW1;
ServoEasing servoW3;
ServoEasing servoW4;
ServoEasing servoW6;
ServoEasing servoCamTilt;

// pins 45-46 control the pan stepper

AccelStepper camPanStepper(1, 46, 45);  //(Type:driver, STEP, DIR) - Stepper1

IBusBM IBus;
IBusBM IBusSensor;

//int angle = 0;   // servo position in degrees
int ch0, ch1, ch2, ch3, ch4, ch5, ch6 = 0;
int servo1Angle = 90;
int servo3Angle = 90;
int servo4Angle = 90;
int servo6Angle = 90;
int s = 0; // rover speed
int r = 0; // turning radius
//int m1, m2, m3, m4, m5, m6;
int camTilt = 90;
int camPan = 0;
float speed1, speed2, speed3 = 0;
float speed1PWM, speed2PWM, speed3PWM = 0;
float thetaInnerFront, thetaInnerBack, thetaOuterFront, thetaOuterBack = 0;

float d1 = 271; // distance in mm
float d2 = 278;
float d3 = 301;
float d4 = 304;

byte x = 0;

int MODE = 3;
int OLDMODE = MODE;

void setup() {

  Wire.begin();

  /*
     Use this if you need to change the frequency of the PWM signals
    TCCR4B = TCCR4B & B11111000 | B00000101;     // D6,D7,D8 PWM frequency of 30.64 Hz
    TCCR2B = TCCR2B & B11111000 | B00000111;   // D9, D10 PWM frequency of 30.64 Hz
    TCCR1B = TCCR1B & B11111000 | B00000101;   // D11, D12  PWM frequency of 30.64 Hz
    TCCR5B = TCCR5B & B11111000 | B00000101; // D4, D13 PWM frequency of 30.64 Hz
    TCCR3B = TCCR3B & B11111000 | B00000101;    // D2, D3, D5 PWM frequency of 30.64 Hz
  */

  Serial.begin(115200);
  IBus.begin(Serial1, IBUSBM_NOTIMER); // Servo iBUS
  IBusSensor.begin(Serial2, IBUSBM_NOTIMER); // Sensor iBUS

  IBusSensor.addSensor(IBUSS_INTV); // add voltage sensor

  // pins 22-26 control the servos

  servoW1.attach(22);
  servoW3.attach(23);
  servoW4.attach(24);
  servoW6.attach(25);
  servoCamTilt.attach(26);

  servoW1.write(90);
  servoW3.write(90);
  servoW4.write(90);
  servoW6.write(90);
  servoCamTilt.write(90);

  servoW1.setSpeed(550);
  servoW3.setSpeed(550);
  servoW4.setSpeed(550);
  servoW6.setSpeed(550);
  servoCamTilt.setSpeed(200);

  camPanStepper.setMaxSpeed(1000);
  camPan = 0;
  camTilt = 90;

  // DC Motors
  // Motor Wheel 1 - Left Front
  digitalWrite(motorW1_IN1, LOW);   // PWM value
  digitalWrite(motorW1_IN2, LOW); // Forward
  // Motor Wheel 2 - Left Middle
  digitalWrite(motorW2_IN1, LOW);
  digitalWrite(motorW2_IN2, LOW);
  // Motor Wheel 3 - Left Back
  digitalWrite(motorW3_IN1, LOW);
  digitalWrite(motorW3_IN2, LOW);
  // right side motors move in opposite direction
  // Motor Wheel 4 - Right Front
  digitalWrite(motorW4_IN1, LOW);
  digitalWrite(motorW4_IN2, LOW);
  // Motor Wheel 5 - Right Middle
  digitalWrite(motorW5_IN1, LOW);
  digitalWrite(motorW5_IN2, LOW);
  // Motor Wheel 6 - Right Back
  digitalWrite(motorW6_IN1, LOW);
  digitalWrite(motorW6_IN2, LOW);

}

void loop() {
  // Reading the data comming from the RC Transmitter
  // remember the IBus routine decrements the channel number by one, so sticks are channels 1-4
  IBus.loop();
  ch0 = IBus.readChannel(0);  // left-right steering
  ch1 = IBus.readChannel(1);  // camera tilt
  ch2 = IBus.readChannel(2);  // throttle
  ch3 = IBus.readChannel(3);  // camera pan
  ch4 = IBus.readChannel(4);  // fwd / rvs gear (SWA)
  ch5 = IBus.readChannel(9);  // lights on/off (SWD)
  ch6 = IBus.readChannel(8);  // light setting a/b/c (SWC)

  // at present, SWB, VRA, and VRB are unused
  // send color and light mode to AUX board; ch5 is power, ch6 selects the animation

  if (ch5 > 1500) {
    if (ch6 < 1300) {
      MODE = 0;
    }
    else if (ch6 < 1700) {
      MODE = 1;
    }
    else if (ch6 > 1700) {
      MODE = 2;
    }
  }
  else {
    MODE = 3;
  }

  // As long as ch5 and ch6 are unchanged, do nothing. If they've changed, send the new mode.

  if (MODE != OLDMODE) {
    Wire.beginTransmission(0x14);
    Wire.write(MODE);
    Wire.endTransmission();

    OLDMODE = MODE;
  }


  // Converting the incoming data
  // Steering right
  if (ch0 > 1515) {
    r = map(ch0, 1515, 2000, 1400, 600); // turning radius from 1400mm to 600mm
  }
  // Steering left
  else if (ch0 < 1485) {
    r = map(ch0, 1485, 1000, 1400, 600); // turning radius from 600mm to 1400mm
  }
  // Rover speed in % from 0 to 100
  s = map(ch2, 1000, 2000, 0, 100); // rover speed from 0% to 100%

  // Camera head steering
  if (ch1 < 1485 ) {
    if (camTilt >= 35) {
      camTilt--;
      delay(20);
    }
  }
  if (ch1 > 1515 ) {
    if (camTilt <= 165) {
      camTilt++;
      delay(20);
    }
  }
  servoCamTilt.startEaseTo(camTilt); // Camera tilt

  if (ch3 >= 1000 && ch3 < 1485) {
    camPan = map(ch3, 1000, 1485, 800, 0);
  }
  else if (ch3 > 1515 && ch3 <= 2000) {
    camPan = map(ch3, 1515, 2000, 0, -800);
  }
  else {
    camPan = 0;
  }
  camPanStepper.setSpeed(camPan);    // Camera pan
  camPanStepper.run();


  calculateMotorsSpeed();
  calculateServoAngle();

  // Steer right
  if (ch0 > 1515) {
    // Servo motors
    // Outer wheels
    servoW1.startEaseTo(97 + thetaInnerFront); // front wheel steer right
    servoW3.startEaseTo(97 - thetaInnerBack); // back wheel steer left for overall steering to the right of the rover
    // Inner wheels
    servoW4.startEaseTo(94 + thetaOuterFront);
    servoW6.startEaseTo(96 - thetaOuterBack);

    // DC Motors
    if (ch4 < 1500) { // Move forward
      // Motor Wheel 1 - Left Front
      analogWrite(motorW1_IN1, speed1PWM);   // Outer wheels running at speed1 - max speed
      digitalWrite(motorW1_IN2, LOW);
      // Motor Wheel 2 - Left Middle
      analogWrite(motorW2_IN1, speed1PWM);
      digitalWrite(motorW2_IN2, LOW);
      // Motor Wheel 3 - Left Back
      analogWrite(motorW3_IN1, speed1PWM);
      digitalWrite(motorW3_IN2, LOW);
      // right side motors move in opposite direction
      // Motor Wheel 4 - Right Front
      digitalWrite(motorW4_IN1, LOW);
      analogWrite(motorW4_IN2, speed2PWM); // Inner front wheel running at speed2 - lower speed
      // Motor Wheel 5 - Right Middle
      digitalWrite(motorW5_IN1, LOW);
      analogWrite(motorW5_IN2, speed3PWM); // Inner middle wheel running at speed3 - lowest speed
      // Motor Wheel 6 - Right Back
      digitalWrite(motorW6_IN1, LOW);
      analogWrite(motorW6_IN2, speed2PWM); // Inner back wheel running at speed2 - lower speed

    }
    else if (ch4 > 1500) {
      // Motor Wheel 1 - Left Front
      digitalWrite(motorW1_IN1, LOW);   // Outer wheels running at speed1 - max speed
      analogWrite(motorW1_IN2, speed1PWM);
      // Motor Wheel 2 - Left Middle
      digitalWrite(motorW2_IN1, LOW);
      analogWrite(motorW2_IN2, speed1PWM);
      // Motor Wheel 3 - Left Back
      digitalWrite(motorW3_IN1, LOW);
      analogWrite(motorW3_IN2, speed1PWM);
      // right side motors move in opposite direction
      // Motor Wheel 4 - Right Front
      analogWrite(motorW4_IN1, speed2PWM);
      digitalWrite(motorW4_IN2, LOW); // Inner front wheel running at speed2 - lower speed
      // Motor Wheel 5 - Right Middle
      analogWrite(motorW5_IN1, speed3PWM);
      digitalWrite(motorW5_IN2, LOW); // Inner middle wheel running at speed3 - lowest speed
      // Motor Wheel 6 - Right Back
      analogWrite(motorW6_IN1, speed2PWM);
      digitalWrite(motorW6_IN2, LOW); // Inner back wheel running at speed2 - lower speed

    }
  }

  // Steer left
  else if (ch0 < 1485) {
    // Servo motors
    servoW1.startEaseTo(97 - thetaOuterFront);
    servoW3.startEaseTo(97 + thetaOuterBack);
    servoW4.startEaseTo(94 - thetaInnerFront);
    servoW6.startEaseTo(96 + thetaInnerBack);

    // DC Motors
    if (ch4 < 1500) { // Move forward
      // Motor Wheel 1 - Left Front
      analogWrite(motorW1_IN1, speed2PWM);   // PWM value
      digitalWrite(motorW1_IN2, LOW); // Forward
      // Motor Wheel 2 - Left Middle
      analogWrite(motorW2_IN1, speed3PWM);
      digitalWrite(motorW2_IN2, LOW);
      // Motor Wheel 3 - Left Back
      analogWrite(motorW3_IN1, speed2PWM);
      digitalWrite(motorW3_IN2, LOW);
      // Motor Wheel 4 - Right Front
      // right side motors move in opposite direction
      digitalWrite(motorW4_IN1, LOW);
      analogWrite(motorW4_IN2, speed1PWM);
      // Motor Wheel 5 - Right Middle
      digitalWrite(motorW5_IN1, LOW);
      analogWrite(motorW5_IN2, speed1PWM);
      // Motor Wheel 6 - Right Back
      digitalWrite(motorW6_IN1, LOW);
      analogWrite(motorW6_IN2, speed1PWM);

    }
    else if (ch4 > 1500) { // Move backward
      // Motor Wheel 1 - Left Front
      digitalWrite(motorW1_IN1, LOW);   // PWM value
      analogWrite(motorW1_IN2, speed2PWM); // Forward
      // Motor Wheel 2 - Left Middle
      digitalWrite(motorW2_IN1, LOW);
      analogWrite(motorW2_IN2, speed3PWM);
      // Motor Wheel 3 - Left Back
      digitalWrite(motorW3_IN1, LOW);
      analogWrite(motorW3_IN2, speed2PWM);
      // Motor Wheel 4 - Right Front
      // right side motors move in opposite direction
      analogWrite(motorW4_IN1, speed1PWM);
      digitalWrite(motorW4_IN2, LOW);
      // Motor Wheel 5 - Right Middle
      analogWrite(motorW5_IN1, speed1PWM);
      digitalWrite(motorW5_IN2, LOW);
      // Motor Wheel 6 - Right Back
      analogWrite(motorW6_IN1, speed1PWM);
      digitalWrite(motorW6_IN2, LOW);

    }
  }

  // Move straight
  else {
    servoW1.startEaseTo(97);
    servoW3.startEaseTo(97);
    servoW4.startEaseTo(94);
    servoW6.startEaseTo(96);

    // DC Motors
    if (ch4 < 1500) {
      // Motor Wheel 1 - Left Front
      analogWrite(motorW1_IN1, speed1PWM);  // all wheels move at the same speed
      digitalWrite(motorW1_IN2, LOW); // Forward
      // Motor Wheel 2 - Left Middle
      analogWrite(motorW2_IN1, speed1PWM);
      digitalWrite(motorW2_IN2, LOW);
      // Motor Wheel 3 - Left Back
      analogWrite(motorW3_IN1, speed1PWM);
      digitalWrite(motorW3_IN2, LOW);
      // right side motors move in opposite direction
      // Motor Wheel 4 - Right Front
      digitalWrite(motorW4_IN1, LOW);
      analogWrite(motorW4_IN2, speed1PWM);
      // Motor Wheel 5 - Right Middle
      digitalWrite(motorW5_IN1, LOW);
      analogWrite(motorW5_IN2, speed1PWM);
      // Motor Wheel 6 - Right Back
      digitalWrite(motorW6_IN1, LOW);
      analogWrite(motorW6_IN2, speed1PWM);

    }
    else if (ch4 > 1500) {
      // Motor Wheel 1 - Left Front
      digitalWrite(motorW1_IN1, LOW);  // all wheels move at the same speed
      analogWrite(motorW1_IN2, speed1PWM); // Forward
      // Motor Wheel 2 - Left Middle
      digitalWrite(motorW2_IN1, LOW);
      analogWrite(motorW2_IN2, speed1PWM);
      // Motor Wheel 3 - Left Back
      digitalWrite(motorW3_IN1, LOW);
      analogWrite(motorW3_IN2, speed1PWM);
      // right side motors move in opposite direction
      // Motor Wheel 4 - Right Front
      analogWrite(motorW4_IN1, speed1PWM);
      digitalWrite(motorW4_IN2, LOW);
      // Motor Wheel 5 - Right Middle
      analogWrite(motorW5_IN1, speed1PWM);
      digitalWrite(motorW5_IN2, LOW);
      // Motor Wheel 6 - Right Back
      analogWrite(motorW6_IN1, speed1PWM);
      digitalWrite(motorW6_IN2, LOW);

    }
  }
  // Monitor the battery voltage
  int sensorValue = analogRead(A0);
  float voltage = sensorValue * (5.00 / 1023.00) * 3.02; // Convert the reading values from 5v to suitable 12V
  // Send battery voltage value to transmitter
  IBusSensor.loop();
  IBusSensor.setSensorMeasurement(1, voltage * 100);

}

void calculateMotorsSpeed() {
  // if no steering, all wheels speed is the same - straight move
  if (ch0 > 1485 && ch0 < 1515) {
    speed1 = speed2 = speed3 = s;
  }
  // when steering, wheels speed depend on the turning radius value
  else {
    // Outer wheels, furthest wheels from turning point, have max speed
    // Due to the rover geometry, all three outer wheels should rotate almost with the same speed. They differe only 1% so we asume they are the same.
    speed1 = s;
    // Inner front and back wheels are closer to the turing point and have lower speeds compared to the outer speeds
    speed2 = s * sqrt(pow(d3, 2) + pow((r - d1), 2)) / (r + d4);
    // Inner middle wheel is closest to the turning point, has the lowest speed
    speed3 = s * (r - d4) / (r + d4);
  }

  // speed value from 0 to 100% to PWM value from 0 to 255
  speed1PWM = map(round(speed1), 0, 100, 0, 255);
  speed2PWM = map(round(speed2), 0, 100, 0, 255);
  speed3PWM = map(round(speed3), 0, 100, 0, 255);
}

void calculateServoAngle() {
  // Calculate the angle for each servo for the input turning radius "r"
  thetaInnerFront = round((atan((d3 / (r + d1)))) * 180 / PI);
  thetaInnerBack = round((atan((d2 / (r + d1)))) * 180 / PI);
  thetaOuterFront = round((atan((d3 / (r - d1)))) * 180 / PI);
  thetaOuterBack = round((atan((d2 / (r - d1)))) * 180 / PI);

}

topherhunter · January 3, 2022, 1:48pm

Are you just here for the snark, or do you offer constructive advice?

anon73444976 · January 3, 2022, 2:03pm

I'd started to comment on the post you made earlier, and then you deleted it.

I was going to suggest meaningful names instead of "ch0", "ch1" etc.
Maybe some comments or named constants (instead of magic numbers) - don't make it difficult for people offering free help, particularly when it took three days to even see the code.

system · July 2, 2022, 2:03pm

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.

Topic		Replies	Views
Conflict problem between servo.h and LiquidCrystal_I2C.h Displays	11	1255	March 27, 2023
MegaServo and Wire(I2C)Library conflict Bugs & Suggestions	9	4464	May 6, 2021
servo jitter Motors, Mechanics, Power and CNC	6	849	May 6, 2021
Trouble with Wire I2C when connecting two Arduinos Programming Questions	2	1532	May 5, 2021
I2C slave receiving data is causing servo twitching Networking, Protocols, and Devices	3	1065	May 6, 2021

I2C and servo conflict?

Related topics