radio shack balancing arduino help

I need some help with a project. I’m trying to build a balancing robot from Radio Shack parts, but I can’t get the motor controller to move both motors.

Arduino Uno REV 3
Arduino Motor Shield R3
RadioShack® Memsic 2125 Dual-Axis Accelerometer
Parallax 27911-RT 3-Axis Gyroscope Module
RadioShack® Super Speed 9-18VDC Hobby Motor
Digital Energy® 7.2V/2800mAh Ni-MH Combo Pack for RC

the first time i tried to build this, I used the smaller motors (1.5-3VDC Metal Gear Motors and 2 9volt batteries in series). I found the torque was low using the motor shield and I could only get 1 motor to work in one direction when I leaned the platform. I bought bigger motors and a RC battery to supply more power and hopefully create more torque.

Now, even with the bigger batteries and motors, I see 3/4 of the led lights above the motor terminals indicating the motor is working, but still only in 1 motor in 1 direction. I can't seem to figure out why the wheel that works doesn't turn in the other direction.

I tested both motors using one of the Digital Energy® 7.2V/2800mAh Ni-MH batteries, then reverse polarity, and found the motors work fine with high torque. I'm also getting a decent output on the pid program I'm using.

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 / (2
M_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));

}

this is the loop and functions part of the code

void loop() {
lastLoopUsefulTime = millis()-loopStartTime;
if(lastLoopUsefulTime<STD_LOOP_TIME) delay(STD_LOOP_TIME-lastLoopUsefulTime);
lastLoopTime = millis() - loopStartTime;
loopStartTime = millis();

anglelast = angle;

digitalWrite(8, HIGH);
digitalWrite(9, HIGH);
// read pulse from x- and y-axes:
pulseX = pulseIn(xPin,HIGH);
pulseY = pulseIn(yPin,HIGH);
pulseX = pulseIn(xPin,HIGH);
pulseY = pulseIn(yPin,HIGH);
// convert the pulse width into acceleration
// accelerationX and accelerationY are in milli-g's:
// earth's gravity is 1000 milli-g's, or 1g.
accX = (((pulseX / 10) - 500) * 8);
accY = (((pulseY / 10) - 500) * 8);

accX = accX - acczeroX;
accY = accY - acczeroY;

Xangle = asin(accX / 1000.0);
Yangle = asin(accY / 1000.0);

Xangle = Xangle * (360 / (2M_PI));
Yangle = Yangle * (360 / (2
M_PI));

getGyroValues();
Gyrox = Gyrox * 0.00875;
Gyroy = Gyroy * 0.00875;
Gyroz = Gyroz * 0.00875;

float dt = (millis() - lastLoopTime)/1000.0;
balanceg = (Gyroy) * 0.00875 * dt;
balancea = Yangle;

angle = ((1-ACC_GYRO_RATIO) * (angle + balanceg)) + ((ACC_GYRO_RATIO)*(balancea));

// like ANGLE_TORQUE_RATIO
Ki = 2.59; //analogRead(A0) / 1000.0; //0.49
Kp = 25.5; //analogRead(A1) / 10.0; //19.8
Kd = 54.0;//analogRead(A2) / 10.0; //44.0

p = (angle * Kp);
d = (angle - anglelast) * Kd;
i = i + (angle * Ki);
float pid = p + d + i;
pidOutput = pid;
pidOutput = constrain(pid, -255, 255);

if (abs(angle) >= ANGLE_KILL) {
pidOutput = 0; }

Drive_Motor(pidOutput);

currentRaw = analogRead(currentPin);
currentVolts = currentRaw *(5.0/1024.0);
currentAmps = currentVolts/volt_per_amp;
currentRaw2 = analogRead(currentPin2);
currentVolts2 = currentRaw2 *(5.0/1024.0);
currentAmps2 = currentVolts2/volt_per_amp;
Serial.print("angle: ");
Serial.print(angle);
Serial.print(", ");
Serial.print("pidOutput: ");
Serial.print(pidOutput);
Serial.print(", ");
Serial.print("currentAmps: ");
Serial.print(currentAmps);
Serial.print(", ");
Serial.print("currentAmps2: ");
Serial.print(currentAmps2);
Serial.println();
}

void Drive_Motor(int drive) {
if (drive < 0) { // drive motors forward
digitalWrite(BRAKE_A, LOW); //Disengage the Brake for Channel A
digitalWrite(DIR_A, HIGH);
digitalWrite(BRAKE_B, LOW); //Disengage the Brake for Channel A
digitalWrite(DIR_A, HIGH);
analogWrite(PWM_A, abs(drive));
analogWrite(PWM_B, abs(drive)); // motor are not built equal.
} else { // drive motors backward
digitalWrite(BRAKE_A, LOW); //Disengage the Brake for Channel A
digitalWrite(DIR_A, LOW);
digitalWrite(BRAKE_B, LOW); //Disengage the Brake for Channel A
digitalWrite(DIR_B, LOW);
analogWrite(PWM_A, abs(drive));
analogWrite(PWM_B, abs(drive));}
}
void getGyroValues(){
byte MSB, LSB;

MSB = readI2C(0x29);
LSB = readI2C(0x28);
Gyrox = ((MSB << 8) | LSB);

MSB = readI2C(0x2B);
LSB = readI2C(0x2A);
Gyroy = ((MSB << 8) | LSB);

MSB = readI2C(0x2D);
LSB = readI2C(0x2C);
Gyroz = ((MSB << 8) | LSB);
}
int readI2C (byte regAddr) {
Wire.beginTransmission(L3G4200D_Address);
Wire.write(regAddr); // Register address to read
Wire.endTransmission(); // Terminate request
Wire.requestFrom(L3G4200D_Address, 1); // Read a byte
while(!Wire.available()) { }; // Wait for receipt
return(Wire.read()); // Get result
}
void writeI2C (byte regAddr, byte val) {
Wire.beginTransmission(L3G4200D_Address);
Wire.write(regAddr);
Wire.write(val);
Wire.endTransmission();
}

happy faces should be an 8 sorry about that

but I can’t get the motor controller to move both motors.

Then, why are you bothering to read the gyro and accelerometer? Create a sketch that JUST moves the motors one way for one second, and then the other way for one second.

Show that sketch and your wiring.

happy faces should be an 8 sorry about that

Use the code box feature to put your code in. It is the # on the post tool bar. Highlight your code and click the #.

ok. Thanks PaulS and zoomkat. I'll create the wiring diagram and upload today. Now, both motors work (I had a short in one wire), and I can get both wheels working in both directions.

PaulS - I started with the gyroscope and accelerometer and they were working fine before I added the motors. Now motors working ok, but I can't get the gyroscope and accelerometer to function as before.

Thanks - all comments are appreciated!