Folks, hoping for some tips. I've built a balancing robot using an IMU 5DOF, the MD25 motor controller and EMG30s but the motors simply don't seem to respond quickly enough.
I've pretty much used the code being discussed on this thread with a few exceptions -
I'm using an arduino mega so have used the internal ref voltage of 2.56V and the amplified gyro outputs on the IDG500 so my gyro conversion looks like this:
int getGyroRate()
{
return int(sensorValue[GYR_Y] * 0.781440781); // in quid/sec:(1024/360)/1024 * 2.56/0.0091)
}
I'm using I2C for the motor controller which needs a 0 to 255 where 128=stopped so to get my drive value I do:
int updatePid(int targetPosition, int currentPosition)
{
int error = targetPosition - currentPosition;
pTerm = Kp * error;
integrated_error += error;
iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
dTerm = Kd * (error - last_error);
last_error = error;
int x = -constrain(K*(pTerm + iTerm + dTerm), -255, 255);
x /= 2;
x += 128;
return x;
}
When calibrating my sensors and getting readings I disregarded any outliers:
int compare (const void * a, const void * b)
{
return ( *(int*)a - *(int*)b );
}
void calibrateSensors()
{
int allVals[50];
long v;
for(int n=0; n<3; ++n)
{
v = 0;
for(int i=0; i<50; ++i)
{
allVals[i] = readSensor(n);
}
qsort(allVals, 50, sizeof(int), compare);
for(int i=5; i<45; ++i)
{
v+= allVals[i];
}
sensorZero[n] = v/40;
}
sensorZero[ACC_Z] -= 137;
}
void updateSensors()
{
int allVals[5];
long v;
for(int n=0; n<3; ++n)
{
v = 0;
for(int i=0; i<5; ++i)
{
allVals[i] = readSensor(n);
}
qsort(allVals, 5, sizeof(int), compare);
for(int i=1; i<4; ++i)
{
v+= allVals[i];
}
sensorValue[n] = v/3 - sensorZero[n];
}
}
I also timed my loop over 1000 iterations and found it took about 3.5ms per iteration so I've tried setting the STD_LOOP_TIME to 5 but it didn't help.
My angles look pretty good and if I print out the time, angle and drive as they change they look ok though the time is slowed down by the printing.
However, if I let the robot go from vertical I can see it's almost beyond the point of no return before the motors kick into life.
If I stop it falling too far on each side by hand I can see that the motors are doing the right thing, just not quick enough.
I've mainly just tried increasing the P val for pid so far, I and D are 0, though I've tinkered there too with no joy.
I don't know much about I2C, could it be just to slow for this task? Surely the EMG30s are up to it?
Appreciate any advice!