Go Down

Topic: Segway-like self balancing robot problem (Read 4027 times) previous topic - next topic

deathcurl

Hi! This is my first time posting here in Arduino forums as we cannot solve a problem with our school robotics project. As the title suggests, we are currently building a Segway-like self balancing robot, and we have a problem. We have been troubleshooting our project for days and we just can't seem to find it. First, here are the components that we used for the project:

And this is the actual build of our project: (the components were not placed yet in the picture, but I'll just describe it. There is a platform plywood on top of its base then all electronic components are placed on it, below the handlebar side. It took almost 1/4 of the platform but there is still enough space for someone to step on it. Batteries are placed inline with the axle of the motors, below the plywood platform.)

Its code:
Code: [Select]
#include <SPI.h>
#include <PID_v1.h>
#include <Wire.h>
int CS=10;
char POWER_CTL = 0x2D;
char DATA_FORMAT = 0x31;
char DATAX0 = 0x32;
char DATAX1 = 0x33;
char DATAY0 = 0x34;
char DATAY1 = 0x35;
char DATAZ0 = 0x36;
char DATAZ1 = 0x37;
char values[10];
int x,y,z;
// gyro declarations
#define GYRO 0x68
#define G_SMPLRT_DIV 0x15
#define G_DLPF_FS 0x16
#define G_INT_CFG 0x17
#define G_PWR_MGM 0x3E
#define G_TO_READ 8
int g_offx = 120;
int g_offy = 20;
int g_offz = 93;
int hx, hy, hz, turetemp;
// own declarations
double kp = 10;
double ki = 1;
double kd = 2;
double input, output, setpoint;
PID segway(&input, &output, &setpoint, kp, ki, kd, REVERSE);
// from kas balancing robot
int x0, z0, gy0;
int xv, zv, gyv;
// complementary filter
float timeStep = 0.02;
float rollAccel = 0;
float rollPrediction = 0;
unsigned long timer;
void setup()
{
  SPI.begin();
  SPI.setDataMode(SPI_MODE3);
  Serial.begin(115200);
  pinMode(CS, OUTPUT);
  digitalWrite(CS, HIGH);
  writeRegister(DATA_FORMAT, 0x01);
  writeRegister(POWER_CTL, 0x08);
  // gyro initializations
  Wire.begin();
  initGyro();
  // own initializations
  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  setpoint = 0;
  // accelerometer X value to be balanced
  segway.SetMode(AUTOMATIC);  // turn PID controller on
  // kas balancing robot
  delay(300);
  calibrateSensors();
}

void loop()
{
  timer = millis();
  updateSensors(); 
  rollAccel = atan2(xv, zv) * 180/PI;
  rollPrediction = (0.98*(rollPrediction + gyv*0.01)) + (timeStep*rollAccel);
  // PID code
  input = abs((double)rollPrediction);
  if (rollPrediction < 0)                     // direction of motor
  {
    digitalWrite(3, LOW);
    digitalWrite(4, LOW);
  }
  else
  {
    digitalWrite(3, HIGH);
    digitalWrite(4, HIGH);
  }
  segway.Compute();
  analogWrite(5 ,output);    // speed of motor 1
  analogWrite(6 ,output);    // speed of motor 2
  Serial.print(rollPrediction);
  Serial.print("-");
  Serial.println(output);
  timer = millis() - timer;
  timer = (timeStep * 1000) - timer;
  delay(timer);
}
void calibrateSensors()
{
  byte addr;
  int gyro[4];
  long ax, az, gy;
  ax = 0, az = 0, gy = 0;
  for(int i = 0; i < 50; i++)
  {
    readRegister(DATAX0, 6, values);
    x = ((int)values[1]<<8)|(int)values[0];
    z = ((int)values[5]<<8)|(int)values[4];
    // gyro code
    getGyroscopeData(gyro);
    hy = gyro[1] / 14.375;
    ax += x;
    az += z;
    gy += hy;
  }
  x0 = ax/50;
  z0 = az/50;
  gy0 = gy/50;
  z0 -= 89;
}
void updateSensors()
{
  byte addr;
  int gyro[4];
  long ax, az, gy;
  ax = 0, az = 0, gy = 0;
  for (int i= 0; i < 5; i++)
  {
    readRegister(DATAX0, 6, values);
    x = ((int)values[1]<<8)|(int)values[0];
    z = ((int)values[5]<<8)|(int)values[4];
    // gyro code
    getGyroscopeData(gyro);
    hy = gyro[1] / 14.375;
    ax += x;
    az += z;
    gy += hy;
  }
  xv = ax/5 - x0;
  zv = az/5 - z0;
  gyv = gy/5 - gy0;
}
void writeRegister(char registerAddress, char value)
{
  digitalWrite(CS, LOW);
  SPI.transfer(registerAddress);
  SPI.transfer(value);
  digitalWrite(CS, HIGH);
}
void readRegister(char registerAddress, int numBytes, char * values)
{
  char address = 0x80 | registerAddress;
  if(numBytes > 1)address = address | 0x40;
  digitalWrite(CS, LOW);
  SPI.transfer(address);
  for(int i=0; i<numBytes; i++)
  {
    values[i] = SPI.transfer(0x00);
  }
  digitalWrite(CS, HIGH);
}
void initGyro()
{
writeTo(GYRO, G_PWR_MGM, 0x00);
writeTo(GYRO, G_SMPLRT_DIV, 0x07);
writeTo(GYRO, G_DLPF_FS, 0x1E);
writeTo(GYRO, G_INT_CFG, 0x00);
}
void getGyroscopeData(int * result)
{
int regAddress = 0x1B;
int temp, x, y, z;
byte buff[G_TO_READ];
readFrom(GYRO, regAddress, G_TO_READ, buff);
result[0] = ((buff[2] << 8) | buff[3]) + g_offx;
result[1] = ((buff[4] << 8) | buff[5]) + g_offy;
result[2] = ((buff[6] << 8) | buff[7]) + g_offz;
result[3] = (buff[0] << 8) | buff[1];
}
void writeTo(int DEVICE, byte address, byte val)
{
Wire.beginTransmission(DEVICE);
Wire.write(address);
Wire.write(val);
Wire.endTransmission();
}
void readFrom(int DEVICE, byte address, int num, byte buff[])
{
Wire.beginTransmission(DEVICE);
Wire.write(address);
Wire.endTransmission();
Wire.beginTransmission(DEVICE);
Wire.requestFrom(DEVICE, num);
int i = 0;
while(Wire.available())
{
   buff[i] = Wire.read();
   i++;
}
Wire.endTransmission();
}

Basically the code was based from various sources from the internet. First, I followed this guide (http://forum.arduino.cc/index.php/topic,8871.0.html) generally on how to build a balancing robot. Then I just copied sensor acquisition codes as I have no time studying it in depth from here https://www.sparkfun.com/tutorials/240 for the accelerometer and here https://gist.github.com/e-Gizmo/7817932 for the gyroscope. These codes were verified first singly before integrating them into one  via complementary filter with this as a guide http://www.pieter-jan.com/node/11. Next is the use of PID control library from here http://playground.arduino.cc/Code/PIDLibrary. Then, everything worked.

The problem is, it only works when the motor is turned off. We get proper results using the serial monitor, displaying the input and output of the PID controller. However, as we turn it on, it works properly when to forward and reverse, but only for a moment and then it freezes. Having the last output shown in the serial monitor, the motor gets stuck on that speed and direction, and we had to reset the code again. First researches on the net indicate that there was a feedback coming from the motor itself which causes the freeze, so what we thought of is using a photocoupler circuit (http://www.ustudy.in/sites/default/files/250px-Optocoupler_Circuit.svg_.png) between the motor driver and the arduino to no avail.

Might the problem have something to do with the code? Or is it in the hardware side?

Sorry for the lengthy post, but thank you for your time reading it and possibly thinking and posting of a solution. :P

jremington

The very large startup current drawn by the motors might be causing the battery voltage to drop so that the 5V power to the Arduino is affected. Try using a separate power source for the Arduino, but connect the grounds of both power sources together.

deathcurl

Hmm. Actually, we are currently using a 12V DC power supply for the motors, then for the arduino, we use the power from a laptop's USB port. You know, the one you use for uploading the code to the arduino. It's for testing purposes because we don't want to recharge the batteries frequently. Is that the same as separating the power supplies?

jremington

Quote
A 5V DC regulator IC for arduino source
If you are using a laptop supply, it was misleading to state this.

deathcurl

Oh I see. I forgot to mention that during testing that  our current setup is a 12V DC power supply and a laptop USB, just for convenience (My bad). Still, this is a separate power source, as what you've said, right? If so, the same problem persists. :( also, we really mean to use that 5V IC regulator after the testing phase (you know, after we finally balanced it, etc.) Thinking it would work the same. Anyway, would that create a problem for our segway? The 5V IC regulator?

EL-Costello

Looks like it's purely electrical problem.
I would suggest to power your board by 12V battery directly via VIN pin with some strong capacitor.

ck444

Hey were you able to detect your problem in project ...?

EscapeVelocityLab

Try optoisolating the motor drivers from the microcontroller.  Galvanic isolation is a must.  Otherwise, inexplicable things happen.  This is my attempt with stepper motors:


https://www.youtube.com/watch?v=Xmt67HTI2Hg&t=5s


Go Up