Go Down

Topic: radio shack balancing arduino help (Read 1 time) previous topic - next topic

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


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 / (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 << 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

PaulS

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

zoomkat

Quote
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 #.
Google forum search: Use Google Advanced Search and use Http://forum.arduino.cc/index in the "site or domain:" box.

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!

Go Up