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:
- 3-Axis Gyroscope Module (ITG3205 breakout board specifically this: http://www.e-gizmo.com/KIT/digital%20gyroscope.html)
- 3-Axis Digital Accelerometer (ADXL345 breakout board: http://www.e-gizmo.com/KIT/ADXL345.html)
- Arduino (http://www.e-gizmo.com/KIT/Gizduino.html)
- This motor driver (3-36v Dual 15a H-bridge DC Motor Driver 30a for Robot & Car Arduino Compatible for sale online | eBay)
- Two 24V DC motor at 120W and these motors were chopped off from two electric scooters
- Two 12V lead acid batteries taken from the salvaged electric scooters
- A 5V DC regulator IC for arduino source
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:
#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 ADXL345 Hookup Guide - SparkFun Learn for the accelerometer and here 3-Axis GYRO.ino · GitHub for the gyroscope. These codes were verified first singly before integrating them into one via complementary filter with this as a guide Loading.... Next is the use of PID control library from here Arduino Playground - 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.