I have mounted the Arduino and the sensors onto the Vex prototype to start working on a main program. I interfaced the Vex motors to the Arduino and added a 7.2V NiMH battery for power to the Arduino and the Vex Motors. We started on a program that will face the robot in the correct heading based on what values it is given. It works fine for headings up to 359 degrees. It has trouble pointing North because it is on the threshold of 359 to 0 degrees and cannot compensate for that. I have been trying to work on it so that it can work with that threshold of values.
Compass Program
#include <Wire.h>
#include <LSM303.h>
#include <Servo.h>
LSM303 compass;
Servo LeftMotor;
Servo RightMotor;
Servo BallMotor;
int stopp = 100;
int forward = 70;
int backward = 130;
void setup()
{
Serial.begin(9600);
Wire.begin();
compass.init();
compass.enableDefault();
LeftMotor.attach(3);
RightMotor.attach(2);
BallMotor.attach(4);
compass.m_min = (LSM303::vector<int16_t>){-32767, -32767, -32767};
compass.m_max = (LSM303::vector<int16_t>){+32767, +32767, +32767};
}
void loop()
{
int dir = compass.heading();
Serial.println(dir);
face(180);
}
void face (int val)
{
int dir = 0;
while(true)
{
compass.read();
dir = compass.heading();
Serial.println(dir);
if(dir > (val+5))
{
turnleft();
}
else if(dir < (val-5))
{
turnright();
}
else
{
nospeed();
delay(50);
break;
}
}
}
//motor movement functions
void goforward()
{
RightMotor.write(backward);
LeftMotor.write(forward);
delay(1);
}
void gobackward()
{
RightMotor.write(forward);
LeftMotor.write(backward);
delay (1);
}
void nospeed()
{
RightMotor.write(stopp);
LeftMotor.write(stopp);
delay (1);
}
void turnleft()
{
RightMotor.write(backward);
LeftMotor.write(backward);
delay (1);
}
void turnright()
{
RightMotor.write(forward);
LeftMotor.write(forward);
delay(1);
}
void ballmotor()
{
BallMotor.write(forward);
delay(1);
}