here's the set-up portion of code
#include <Wire.h>
#include <LiquidCrystal.h>
#include <math.h>
#define M_PI 3.141592653589793238462643
// gyroscope
#define CTRL_REG1 0x20
#define CTRL_REG2 0x21
#define CTRL_REG3 0x22
#define CTRL_REG4 0x23
#define CTRL_REG5 0x24
int L3G4200D_Address = 105; //I2C address of the L3G4200D
int Gyrox, Gyroy, Gyroz;
double Gyroxzero, Gyroyzero, Gyrozzero;
// Memsic2125
const int xPin = 6; // X output of the accelerometer
const int yPin = 10; // Y output of the accelerometer
int pulseX, pulseY;
double accX, accY;
double Xangle, Yangle;
double acczeroX, acczeroY;
double Xzero, Yzero;
double anglelast;
double angle;
#define ANGLE_KILL 35 // kill motor above
#define ACC_FILTER 0.01 // lowpass filter for acc
#define ACC_GYRO_RATIO 0.20 // acc to gyro ratio
float p = 0;
float i = 0;
float d = 0;
float Kp, Kd, Ki;
float balancea, balanceg;
int pidOutput;
const int PWM_A = 3;
const int DIR_A = 12;
const int BRAKE_A = 9;
const int PWM_B = 11;
const int DIR_B = 13;
const int BRAKE_B = 8;
int currentPin = A0;
int currentPin2 = A1;
int switchPin = 2;
// constants
float volt_per_amp = 1.65; // resolution according to hardware page
// variables
float currentRaw; // the raw analogRead ranging from 0-1023
float currentVolts; // raw reading changed to Volts
float currentAmps; // Voltage reading changed to Amps
float currentRaw2; // the raw analogRead ranging from 0-1023
float currentVolts2; // raw reading changed to Volts
float currentAmps2; // Voltage reading changed to Amps
int STD_LOOP_TIME = 9;
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;
void setup() {
float p = 0;
float i = 0;
float d = 0;
float Kp = 0;
float Kd = 0;
float Ki = 0;
angle = 0.0;
anglelast = 0.0;
balancea = 0;
balanceg = 0;
Wire.begin();
// initialize serial communications:
Serial.begin(9600);
writeI2C(CTRL_REG1, 0x1F); // Turn on all axes, disable power down
writeI2C(CTRL_REG3, 0x08); // Enable control ready signal
writeI2C(CTRL_REG4, 0b00000000); // Set scale (250 deg/sec)
// initialize the pins connected to the accelerometer
pinMode(xPin, INPUT);
pinMode(yPin, INPUT);
pinMode(xPin, INPUT);
pinMode(yPin, INPUT);
//Setup Channel A
pinMode(DIR_A, OUTPUT); //Initiates Motor Channel A pin
pinMode(BRAKE_A, OUTPUT); //Initiates Brake Channel A pin
//Setup Channel B
pinMode(DIR_B, OUTPUT); //Initiates Motor Channel A pin
pinMode(BRAKE_B, OUTPUT); //Initiates Brake Channel A pin
Serial.println("Motor shield Setup:\n");
pinMode(1,OUTPUT); // output for LCD
delay(100);
pulseX = pulseIn(xPin,HIGH);
pulseY = pulseIn(yPin,HIGH);
pulseX = pulseIn(xPin,HIGH);
pulseY = pulseIn(yPin,HIGH);
// earth's gravity is 1000 milli-g's, or 1g.
acczeroX = ((pulseX / 10) - 500) * 8;
acczeroY = ((pulseY / 10) - 500) * 8;
Xangle = asin(acczeroX / 1000.0);
Yangle = asin(acczeroY / 1000.0);
Xangle = Xangle * (360 / (2M_PI));
Yangle = Yangle * (360 / (2M_PI));
getGyroValues();
Gyroxzero = Gyrox * 0.00875;
Gyroyzero = Gyroy * 0.00875;
Gyrozzero = Gyroz * 0.00875;
float dt = (millis() - loopStartTime)/1000.0;
balanceg = (Gyrox-Gyroxzero) * 0.00875 * dt;
balancea = Yangle;
angle = ((1-ACC_GYRO_RATIO) * (angle + balanceg)) + ((ACC_GYRO_RATIO)*(balancea));
}
