And this is the code I'm running
#include <Servo.h>
Servo myservo;
int servoPos = 0; // variable to store the servo position and aimed position
int desirePos;
int servoDirection; //0 if 0 moving 0 to 180, if 1 moving from 180 to 0
int servoPin = 4;
int x, y, z; //triple axis data
int mine = 0;
#include <Wire.h> //I2C Arduino Library
#define addr 0x1E //I2C Address for The HMC5883
int trigPin1 = 3, echoPin1 = 4, trigPin2 = 5, echoPin2 = 6; //Ultrasonic
const int E1Pin = 6;
const int M1Pin = 12;
const int E2Pin = 11;
const int M2Pin = 13;
//motorshield setup
/inner definition for motorsield/
typedef struct {
byte enPin;
byte directionPin;
} MotorContrl;
const int M1 = 0;
const int M2 = 1;
const int MotorNum = 2;
const MotorContrl MotorPin[] = { {E1Pin, M1Pin}, {E2Pin, M2Pin} } ;
const int Forward = LOW;
const int Backward = HIGH;
//variables for ultrasonic sensor
float duration1, duration2, cm1, cm2, ratio1, ratio2, distance = 8;
void setup() {
Wire.begin();
Wire.beginTransmission(addr); //start talking
Wire.write(0x02); // Set the Register
Wire.write(0x00); // Tell the HMC5883 to Continuously Measure
Wire.endTransmission();
initMotor();
myservo.attach(2); //Set servo digital pin 2
//Ultrasonic Pinmode
pinMode(trigPin1, OUTPUT);
pinMode(echoPin1, INPUT);
pinMode(trigPin2, OUTPUT);
pinMode(echoPin2, INPUT);
}
void loop() {
ultraSonic ();
//calculate ratios for motor speed
motorRun ();
servoMove();
}
void servoMove() {
if (mine == 0) {
if (servoPos == 7) { //Are you at the end to left
servoDirection = 0; //If so, start moving right
}
if (servoPos == 180) { //Are you at end on right
servoDirection = 1; //Start Moving left
}
if (servoDirection == 0) { //Move right if direction is right
servoPos += 1;
}
if (servoDirection == 1) { //Move left if direction is left
servoPos -= 1;
}
myservo.write(servoPos);
delayMicroseconds(3000);
Wire.requestFrom(addr, 6);
if (6 <= Wire.available()) {
z = Wire.read() << 8; //MSB z
z |= Wire.read(); //LSB z
}
if (abs(z) == 4096) {
mine = 1;
}
}
}
/functions/
void initMotor( ) {
int i;
for ( i = 0; i < MotorNum; i++ ) {
digitalWrite(MotorPin*.enPin, LOW);*
_ pinMode(MotorPin*.enPin, OUTPUT);_
_ pinMode(MotorPin.directionPin, OUTPUT);
}
}
/** motorNumber: M1, M2_
direction: Forward, Backward **/
void setMotorDirection( int motorNumber, int direction ) {
digitalWrite( MotorPin[motorNumber].directionPin, direction);
_}
/** speed: 0-100 * /
inline void setMotorSpeed( int motorNumber, int speed ) {
analogWrite(MotorPin[motorNumber].enPin, 255.0 * (speed / 100.0) ); //PWM
}
void ultraSonic () {
//ultrasonic sensors ping both
digitalWrite(trigPin1, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin1, LOW);
duration1 = pulseIn(echoPin1, HIGH);
delay(20);
digitalWrite(trigPin2, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin2, LOW);
duration2 = (pulseIn(echoPin2, HIGH));
}
void motorRun () {
cm1 = duration1 / 58;
cm2 = duration2 / 58;
if (cm1 > 85) {
* cm1 = 85;
}
if (cm2 > 85) {
cm2 = 85;
}
distance += 0.01;
ratio2 = (cm1 - (distance)) / cm2;
ratio1 = (1 / ratio2);
//set motor speeds*
setMotorDirection( M1, Forward );
setMotorSpeed( M1, ratio1 * 20);
setMotorDirection( M2, Forward );
setMotorSpeed( M2, ratio2 * 25);
//delay for ultrasonic sensor
delay(20);
}_
