DC motor control with PID

Robot body is nearly finished, waiting for the 12V battery to be placed on the top

Angle measurement is performed with a 5 DOF IMU (2 axis Gyroscope + 3 axis accelerometer)

The sensors board is located between the wheels, near the rotation axis

Actually motor encoding is not required for balancing.
This feature can be added later on to prevent robot from drifting forward or backward

Here is the pseudo code for the balancing process:

loop (fixed time loop 10ms = 100Hz)

  • sensors aquisition, smoothing and zeroing
  • Acc angle (degrees) and Gyro rate (degrees per second) calculation and scaling
  • Acc and Gyro data fusing though Kalman algorithm
  • PWM calculation using a PID control algorithm
  • PWM feeding to DC motors
    endLoop

Before tackling next step, make sure that previous data is OK
we all know: Garbage in ... :wink:

The actual loop will look like that:

void loop() {
// ********************* Sensor aquisition & filtering *******************
  updateSensors();
  ACC_angle = angleDeg();                                                // in degrees
  GYRO_rate = rateDegPerSec();                                           // in degrees/seconds
  actAngle = kalmanCalculate(ACC_angle, GYRO_rate, lastLoopTime);        // calculate Absolute Angle

// *********************** Angle Control and motor drive *****************
  drive = updatePid(setPoint, actAngle);                                // PID algorithm

  if(actAngle>(setPoint-20) && actAngle<(setPoint+20))    Drive_Motor(drive); 
  else   Drive_Motor(0);                                                // stop motors if situation is hopeless
  
// ********************* Debug info **********************************  
//  SerialOut_timing();
//  SerialOut_raw();
//  SerialOut_sensor();
    SerialOut_labView();
  
// *********************** loop timing control **************************
  lastLoopUsefulTime = millis()-loopStartTime;
  if(lastLoopUsefulTime<STD_LOOP_TIME)         delay(STD_LOOP_TIME-lastLoopUsefulTime);
  lastLoopTime = millis() - loopStartTime;
  loopStartTime = millis();
}

Further readings:

wheel encoder: http://www.mindspring.com/~tom2000/Delphi/Codewheel.html
Kalman filter: Kalman filter - Wikipedia
PID conrol: PID controller - Wikipedia
those documents are "hard theory" and do not need to be fully understood for implementing the project