Control De Motores I2c

#include <Wire.h>

#include <Servo.h>

Servo myservo1;

Servo myservo2;

#define i2cAddress 0x58 // i2c address

// MD25 Commands

#define speed1 0x00 // speed to first motor

#define speed2 0x01 // speed to second motor

#define encoderOne 0x02 // motor encoder 1 (first byte)

#define encoderTwo 0x06 // motor encoder 2 (first byte)

#define voltRead 0x0A // battery volts

#define current1 0x0B // motor 1 current

#define current2 0x0C // motor 2 current

#define softwareVer 0x0D // software version

#define accRate 0x0E // acceleration rate

#define OPmode 0x0F // mode of operation

#define cmdByte 0x10 // Command byte

int x=128, x2=128,pos1=90,pos2=90;

long x_t;

#define BUFFERSIZE 20

char buffer[BUFFERSIZE]; // incoming serial

char *parseptr;

char bufferidx;

void setup()

{

Serial.begin(115200);

pinMode(13, OUTPUT);

Wire.begin();

delay(200); // Wait for everything to power up

Serial.print("MD25 version:");

int softVer = getSoft(); // software version of board

Serial.println(softVer, DEC);

myservo1.attach(9);

myservo2.attach(10);

encodeReset(); // resets the encoder values to 0

Serial.flush();

}

void loop()

{

if (Serial.available()>0)

{

pos2=Serial.read();

delay(5);

pos1=Serial.read();

delay(5);

x2=Serial.read();

delay(5);

x=Serial.read();

delay(5);

}

digitalWrite(13,HIGH);

myservo1.write(pos1);

myservo2.write(pos2);

Wire.beginTransmission(i2cAddress); // Drive motor 1 at speed value stored in x

Wire.send(speed1);

Wire.send(x);

Wire.endTransmission();

Wire.beginTransmission(i2cAddress); // Drive motor 2 at speed value stored in x2

Wire.send(speed2);

Wire.send(x2);

Wire.endTransmission();

delay(10);

digitalWrite(13,LOW);

}

int getSoft(){ // Function that gets the software version

Wire.beginTransmission(i2cAddress);

...