next topic »

[on:]

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.   

[Reply #1 on:]

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

[Reply #2 on:]

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) * ;
   accY = (((pulseY / 10) - 500) * ;
   
   accX = accX - acczeroX;
   accY = accY - acczeroY;
   
   Xangle = asin(accX / 1000.0);
   Yangle = asin(accY / 1000.0);
   
   Xangle = Xangle * (360 / (2*M_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 << | LSB);

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

  MSB = readI2C(0x2D);
  LSB = readI2C(0x2C);
  Gyroz = ((MSB << | 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();
}

[Reply #3 on:]

happy faces should be an 8  sorry about that

[Reply #4 on:]

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.

[Reply #5 on:]

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 #.

[Reply #6 on:]

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!