Go Down

Topic: Simple Balancer (Read 340 times) previous topic - next topic

GunnerCAF

This is a simple balancing robot using just an accelerometer and a small simple sketch.  I built the robot with the center of gravity (CG) only about an inch above the pivot (axle) and put the accelerometer as close to the CG as possible.  By putting the accelerometer at the CG reduces affect of the motor acceleration during balancing.  I put the x axis of the accelerometer in the direction of travel parallel to the ground to detect the direction of tipping.   

Here is a short video:

https://www.youtube.com/watch?v=pFOl2eFPzMs

Here is the sketch with more notes:

Code: [Select]
/*
Simple Balancing Robot
by Dave Gundlach
8/5/2014

This is a simple balancer using a robot with a short distance to center of gravity (CG) above the wheel centers.
This uses on an accelerometer for sensor.  The tilt is calculated by reading the value and direction of an
axis parallel to the ground.  The accelerometer is located at the robot CG to minimize motor acceleration.
In addition, the motors are shut down with a small delay before reading the accelerometer to minimize motor acceleration.

Robot:  RadioShack Make Robotics Starter Kit + Motor Addon
        Robot with 2 Black Tip motors, 2 (4 AA) battery packs
        Battery Pack (9V) for Arduino Power
        Arduino Uno
        Adafruit V1.2 motor controller
        Make Prototype Shield with mini breadboard
        Adafruit ADXL335 Accelerometer
       
Wiring: 2 (4 AA) battery packs connected in parallel to Motor Power connection (jumper off)
        motors connected to each of the motor connections on the motor controller, black wire to inner terminal.
        Accelerometer connected to 5V, Ground, A0 to Xout., and Aref to 3Vo (See Adafruit Tutorial for more info).
       
Libraries:
  Adafruit Motor shield library
    copyright Adafruit Industries LLC, 2009
    this code is public domain, enjoy!
 
*****WARNING - SEE ARDUINO REFERENCE PAGE FOR IMPORTANT INFO ON EXTERNAL REFERENCE USAGE******
WARNING - Failure to not follow external REFERENCE instruction may result in damage to your Arduino

*/
// Motor Shield Setup //
#include <AFMotor.h>

AF_DCMotor motor1(1, MOTOR12_64KHZ);    // Instance for each motor
AF_DCMotor motor2(2, MOTOR12_64KHZ);

// Accelerometer Setup //

#define xPin A0      // x axis pin
//#define yPin A1      // y axis pin
//#define zPin A2      // z axis pin
#define xZero 509    // x zero G value              // calibration values (need to use values for your sensor)
//#define yZero 510    // y zero G value
//#define zZero 523    // z zero G value
#define xInc 103.0   // x increment value (float)
//#define yInc 106.0   // y increment value (float)
//#define zInc 101.0   // z increment value (float)

void setup() {
  analogReference(EXTERNAL);  // SEE WARNING
  Serial.begin(9600);           // set up Serial library at 9600 bps
 
  // turn on motor
  motor1.setSpeed(250);
  motor2.setSpeed(250);

  motor1.run(RELEASE);
  motor2.run(RELEASE);
}

void loop() {
  motor1.run(RELEASE);                   // release motors to get reduce motor accel on sensor
  motor2.run(RELEASE);
  //delay(1);                             // short delay to let motors release
  int xRaw = analogRead(xPin);           // Read Raw accelerometer Values
  float xG = (xRaw - xZero)/xInc;        // Calc G values (note, xInc needs to be a float value)
  int del = 2;                          // Set delay to 2ms
  int xGh = xG*100;                     // Make xG an integer by multiplying by 100
  int Speed = (xGh) * 90;               // Calculate motor speed and multiply by a speed factor
  Speed = abs(Speed);                    // make it positive
  Speed = constrain(Speed, 0, 250);      // constrain to max of 250
  Serial.println (Speed);
  if (xGh < 0) {                         // if negative
    motor1.run(FORWARD);                 // Move forward
    motor2.run(FORWARD);
    motor1.setSpeed(Speed);
    motor2.setSpeed(Speed);
    delay(del);
    }
  else {                                // else positive
    motor1.run(BACKWARD);               // Move backward
    motor2.run(BACKWARD);
    motor1.setSpeed(Speed);
    motor2.setSpeed(Speed);
    delay(del);
  }
}


I made this to experiment with what I can do with an accelerometer.  It would be much better to use an accelerometer and gyro, but everyone has done that :)

Dave

Go Up