You might want to set full correction (full power) if you are already at 4-degrees. What is really important is the Derivative factor, or, how fast is it falling when it passes 4-degrees?

Judging by my eyes, at about 4 degree, the falling is pretty slow.

Do you have full forward and reverse PWM control of your motors?

Yes, at least I think so, I am driving the motors with 8 bits PWM through two H-bridges.

PS:
I think I need bigger wheels, as I plot the PWM and angle at the same graph, at about 5-6 degrees, my PWM is already at 100%.

 

Or should add the I_TERM?

 

Just to be clear, the math used to convert the sensor input to a PWM signal allows -100% duty cycle (full reverse) to 0 to +100% duty cycle (full forward), correct?

 

Just to be clear, the math used to convert the sensor input to a PWM signal allows -100% duty cycle (full reverse) to 0 to +100% duty cycle (full forward), correct?

Yes, it can do -100% (full reverse) and 100% (full forward)

PS:

According to my motors datasheet, the motor can do 100 rpm under load @ 12V, my wheels are 35mm diameter

 

What is your current code to calculate the PWM for the next 0.01 seconds based on sensor input?

Also, which accelerometer are you using?

Do you have an oscilloscope to check that the Pwm is correcting from -100 to +100 duty cycle?

 

I assume the motors are gear motors, not direct drive. True?

 

here is my codes:

    // read raw accel/gyro measurements from device
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
	
        //within 30 degree, accel ~= radius
	accl_angle = (float)(ax - ACCE_OFFSET) / ACCE_SENSITIVITY  * 180 / PI;
	gyro_angle = (float)(gz - GYRO_OFFSET) / GYRO_SENSITIVITY;

	coefficient = 0.98;

        //combine accel and gyro data with complementary filter
        //time constant t=0.49
	filtered_angle = coefficient * (filtered_angle + gyro_angle * 0.01) - (1 - coefficient) * accl_angle;

        //if angle > 30 degrees , stop the motors
	if ((filtered_angle > ANGLE_MAX) || (filtered_angle < - ANGLE_MAX))
	{
		setMotorLeft(0);
		setMotorRight(0);
	}
	else
	{
		feedback = updatePID(filtered_angle,gyro_angle, PID_OFFSET_ANGLE);

                //me trying to push the robot back by over correcting, 
                //if the angle is greater than 3 degree
		if ((filtered_angle - PID_OFFSET_ANGLE) > 3.0 ) feedback = feedback * 1.5;
		if ((filtered_angle - PID_OFFSET_ANGLE) < -3.0 ) feedback = feedback * 1.5;

                //PWM_MAX_VALUE = 255, PWM_MIIN_VALUE = -255
		if (feedback > PWM_MAX_VALUE) feedback = PWM_MAX_VALUE;
		if (feedback < PWM_MIN_VALUE) feedback = PWM_MIN_VALUE;

		setMotorLeft((int)feedback);
		setMotorRight((int)feedback);

                //print datas to StampPlot
		Serial.print(feedback);
		Serial.print(",");
		Serial.println(filtered_angle);
	}

and my PID codes, angle is the fusion angle, gyro_angle is the gyro angle:

float updatePID(float angle, float gyro_angle, float setPoint)
{
	float pTerm;
	float error;
	float dTerm;

	error = angle - setPoint;

	pTerm = P_GAIN * error;
	dTerm = D_GAIN * gyro_angle;


	return pTerm + dTerm;
}

I am using MPU6050, it comes with accelerator and gyro in one package.

And yes, the motors are geared motors like these., but datasheet of my motors says 100rpm under load @ 12V

 

I will check the PWM of my motors and report back with a scope.

edited:

The signal to from the Arduino to the H-bridge is 0-100% PWM and negative 100% PWM to 0, nice square wave. But the PWM signal at the motors are sort of PWM wave (not perfect PWM wave), but I can tell it has 0 - 100% and negative 100% to 0 signal.

Do you want to see a video my robot on balancing and falling over? Will it help?

 

Ooops, I think I might find the probem.

The motors I ordered are 100rpm after gearbox reduction, but the ones I got are 32rpm, that's about 0.1m/s at the wheel, that might be a bit too slow...

 

Ooops, I think I might find the probem.

The motors I ordered are 100rpm after gearbox reduction, but the ones I got are 32rpm, that's about 0.1m/s at the wheel, that might be a bit too slow...

It shouldn't be, providing that the load applied doesn't exceed it's ability to correct. Think of it this way, you are simply maintaining a vertical position over a fixed point, even if it needs assistance to initially get there. If you push on it, you should feel resistance approaching the torque of the drive,until you exceed and push it over, or, you could employ that torque to travel. You could also consider that if you held your bot vertical, then let it fall in an arc about it' pivot (axle), you would recognize that it accelerates at some rate. The angle at which it's speed exceeds your drives capabilities, is it' maximum angle of recovery, less some fudge for dynamic response. I would absolutely employ your I component, but again it comes down to dynamic response. Step your drive train and tune the loop for a classic response.

 

Step your drive train and tune the loop for a classic response.

This is my first ever PID(actually on PD) control, I don't understand what is a classic response.

Could you point me a link so I can have a read?

 

Can you post the reading of your sensors for:

0 degrees (vertical)
5 degrees
10 degrees
15 degrees

For both the inclinatiion of your robot?

This will hep in giving you a better solution, since I think PID is not the best solution for this Kind of problem.

Cheers

Alberto

 

Can you post the reading of your sensors for:

0 degrees (vertical)
5 degrees
10 degrees
15 degrees

For both the inclinatiion of your robot?

This will hep in giving you a better solution, since I think PID is not the best solution for this Kind of problem.

Cheers

Alberto

Sorry for the late reply, because I need to prepare for my final exams, the attached file are all the datas.

I do not have the suitable measurement equipment to measure the angle, I just use a protractor I got.

here are the codes that I combine the sensor datas:

	accl_angle = (float)(ax - ACCE_OFFSET) / ACCE_SENSITIVITY  * 180 / PI;
	gyro_angle = (float)(gz - GYRO_OFFSET) / GYRO_SENSITIVITY;
        //ax is reading from accelerometer, x axle
        //gz is reading from gyro, z axle

	//coefficient = TIME_CONSTANT / (TIME_CONSTANT + float(dt_old - millis()));
	coefficient = 0.98;

	filtered_angle = coefficient * (filtered_angle + gyro_angle * 0.01) - (1 - coefficient) * accl_angle;
	//filtered_angle = filtered_angle + OFFSET_ANGLE;