#include <Wire.h> //include wire library for I2C communication
#include <L3G.h> //include library for gyro
#include <LSM303.h> //include library for accelerometer and magnometer
#include <Math.h> //include the Math library
double anglefilter = 0;
double derivfilter = 0;
double gyrovalue = 0;
//define the gyrovalue variblea
double x_acc = 0;
//define the x_acc variable
double dt;
//define the dt variable
double tiempo=0;
double acc_calibrate = 0; //define the acc_calibrate variable
double gyro_calibrate = 0; //define the gyro_calibrate variable
double angle = 0;
//define the angle variable
double prev_angle = 0;
double pr = 1.7;
double dr = 0.022;
double ir = 0.06;
double c = 1000;
double integ = 0;
double prop = 0;
double deriv = 0;
double torque = 0;
int m2in1 = 15;
int m2in2 = 14;
int m2PWM = 9;
int m1in1 = 11;
int m1in2 = 12;
int m1PWM = 10;
L3G gyro;
//declaring a gyro class
LSM303 compass; //declaring a acclerometer/magnometer class
void setup() {
pinMode(m2in1,OUTPUT);
pinMode(m2in2,OUTPUT);
pinMode(m2PWM,OUTPUT);
pinMode(m1in1,OUTPUT);
pinMode(m1in2,OUTPUT);
pinMode(m1PWM,OUTPUT);
pinMode(13, OUTPUT); //setting pin 13 to output
digitalWrite(13, HIGH); //turning on the led on the pin
delay(1000);
//waiting 3 sec
digitalWrite(13, LOW); //turning off the led on the pin
Serial.begin(9600); //beginning serial communication and declaring a baud rate
Wire.begin();
//initiating the wire library for I2C
compass.init();
//initiating the accl and magn
compass.enableDefault();//defaulting the accl and magn
gyro.init();
//initiating the gyro
gyro.enableDefault();
//defaulting the gyro
dt = micros();
//setting the dt variable equal to the number of
microseconds since the program started
for(int i = 0; i<1000; i++){ //for loop to count from 0 to 999
compass.read();
//reads the accel and mgn chip
gyro.read();
//reads the gyro chip
gyro_calibrate += gyro.g.x; //sets the gyro calibration variable equal to the gyro
value for the x axis plus itself
acc_calibrate += compass.a.z; //sets the accel calibration variable equal to the
accel value for the z axis plus itself
}
acc_calibrate = acc_calibrate/1000-50; // divides the 100 values added up in the
for loop by 100 to get an average
gyro_calibrate = gyro_calibrate/1000; // divides the 100 values added up in the
for loop by 100 to get an average
}
digitalWrite(13, HIGH); //turns the led pin on
delay(1000);
//delay 1 second
digitalWrite(13, LOW); //turns the led pin off
void loop() {
gyro.read();
// read gyro
compass.read();
// read accel and magn
//Serial.println(compass.a.z);
//print accel z axis reading
//Serial.print(" , ");
//print a ,
x_acc = compass.a.z/acc_calibrate;
//set the x_acc equal to the accel z
value divided by the calibration variable
//Serial.print(x_acc);
//print x_acc value
//Serial.print(" , ");
//print a ,
if(x_acc>1)
//if statement saying that if x_acc is larger than 1
x_acc = 1;
//then set x_acc equal to 2-x_acc
x_acc = 1.414*sqrt(1-x_acc);
//approximation for a cosine of x_acc
//Serial.print(x_acc);
//print the new x_acc value
//Serial.print(" , ");
//print a ,
gyrovalue = ((int)gyro.g.x - gyro_calibrate)*0.00015; // This 0.000002 scales the
gyro to radians/s using the scale factor of 8.75mdps/digit
//Serial.print(gyrovalue);
//print the scaled gyro x value
//Serial.print(" , ");
//print a ,
dt = micros()-tiempo;
//set dt equal to the number of
microseconds since the program started minus the old value for dt
//Serial.print(dt);
//Serial.print(" ");
tiempo = micros();
//Serial.print(dt);
//print the new values for dt
if(compass.a.y<0)
// if statement saying that if the y axis
reading from the accelerometer is negative
x_acc = -x_acc;
//set x_acc to -x_acc
angle = 0.99*(angle + gyrovalue * dt*0.0000001) + (0.01)*(x_acc);
//complementary filter for finding the angle of the robot
//Serial.print(" ");
/////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
prop = angle;
deriv = ((1000000*angle)-(prev_angle*1000000))/(dt);
/*Serial.print(angle);
Serial.print(" ");
Serial.print(prev_angle);
Serial.print(" ");
Serial.print(dt);
Serial.print(" ");
Serial.println(deriv);*/
//if(angle<0)
// deriv = 0-deriv;
integ = integ+(angle)*(dt/1000000);
torque = (pr*prop)+(dr*deriv)+(ir*integ);
//Serial.print(angle*57.2958);
//Serial.print(" ");
//Serial.print(micros());
Serial.print(" ");
Serial.print(gyrovalue);
Serial.print(" ");
Serial.print(x_acc);
Serial.print(" ");
Serial.print(angle);
Serial.print(" ");
Serial.println(c*torque);
if(angle>0){
digitalWrite(m1in1,HIGH);
// if angle is positive, drive one direction
digitalWrite(m1in2,LOW);
digitalWrite(m2in1,HIGH);
digitalWrite(m2in2,LOW);
}
else{
//if not, drive the other direction
digitalWrite(m1in1,LOW);
digitalWrite(m1in2,HIGH);
digitalWrite(m2in1,LOW);
digitalWrite(m2in2,HIGH);
}
analogWrite(m1PWM,255-abs(c*torque));
analogWrite(m2PWM,255-abs(c*torque));
prev_angle = angle;
}