trying to balance it - debugging help

HI and GREAT THANKS for your help :wink: I can’t wait for pid tuning but I still got some coding problems that make me unable to arrive there.

I have problems with my balancing robot based on mega2560 with 2pololu motors 350 rpm pololu and motor carrier vnh3sp30 and mpu 6050 GY-521. A lipo battery 3000mah 3Cells 11.1v and Mega 2560 is powered through USB.

I did not post code about mpu6050 because of space, but I‚Äôve attached OUTPUT_example.txt showing OUTPUT_READABLE_YAWPITCHROLL so pitch in degrees. I use DMP sort of ‚Äėblack box‚Äô sensor fusing instead of Kalman filtering itself. IMU is ok, so there‚Äôs no IMU code, just output.

I’m using port access and Timer5() so mega pwm pins 44 and 54. It works. BECAUSE OF YOU.

I did not post variables declaration‚Äď>variab.txt. Note that:

/* PID variables */
double Kp = 1;
double Ki = 0;
double Kd = 0;
double targetAngle = 0;

This is setup() without IMU code:

void setup() {
   
  /* Setup motor pins to output */
  sbi(DDRL, PINL4);  //left motor
  sbi(DDRA, PINA2);
  sbi(DDRA, PINA3);

  sbi(DDRL, PINL5);  //right motor
  sbi(DDRA, PINA0);
  sbi(DDRA, PINA1);  

 TCCR5A = B00101001;  // Phase and frequency correct PWM change at OCR5A
 TCCR5B = B10001;      // System clock
 OCR5A = 800;            // 16MHz/10kHz/2=800  prescaler a 1   (0-799)
 OCR5B = 0;               // Motors stop  D45
 OCR5C = 0;               // D44

  /* Setup encoders */
  pinMode(2,INPUT);
  pinMode(4,INPUT);
  pinMode(3,INPUT);
  pinMode(5,INPUT); 
  attachInterrupt(0,leftEncoder,RISING); // pin 2
  attachInterrupt(1,rightEncoder,RISING); // pin 3

loop() :

   /* Calculate pitch */
  //  accYangle = getAccY();
  //  gyroYrate = getGyroYrate();
  //  gyroAngle += gyroYrate*((double)(micros()-timer)/1000000);
  //  pitch = kalman.getAngle(accYangle, gyroYrate, (double)(micros() - timer)/1000000); 
  // Calculate the angle using a Kalman filter
  
   pitch = ypr[1] * 180/M_PI; // Calculate the angle using DMP library

   /* Drive motors */
   PID(targetAngle,targetOffset,turningOffset);        

  /* Update wheel velocity every 100ms */
  loopCounter++;
  if (loopCounter == 10) {
    loopCounter = 0; // Reset loop counter
    wheelPosition = readLeftEncoder() + readRightEncoder();
    wheelVelocity = wheelPosition - lastWheelPosition;
    lastWheelPosition = wheelPosition;
    if (abs(wheelVelocity) <= 20 && !stopped) { // Set new targetPosition if braking
      targetPosition = wheelPosition;
      stopped = true;
    }
  }

and other important functions:

void setPWM(uint8_t pin, int dutyCycle) { // dutyCycle is a value between 0-ICR
 if(pin == 45) {
  //OCR5B = dutyCycle;
 OCR5BH = (dutyCycle >> 8); 
 OCR5BL = (dutyCycle & 0xFF);
  } else if (pin == 44) {
  //OCR5C = dutyCycle;
  OCR5CH = (dutyCycle >> 8);
  OCR5CL = (dutyCycle & 0xFF);  
 }
}

void moveMotor(Command motor, Command direction, double speedRaw) { // Speed is a value in percentage 0-100%
  if(speedRaw > 100)
    speedRaw = 100;
  int speed = speedRaw*((double)800)/100; // Scale from 100 to PWMVALUE
  if (motor == left) {
    setPWM(45,speed); // Left motor pwm
    if (direction == forward) {
      cbi(PORTA, PINA2);
      sbi(PORTA, PINA3);
    } 
    else if (direction == backward) {
      sbi(PORTA, PINA2);
      cbi(PORTA, PINA3);
    }
  } 
  else if (motor == right) {
    setPWM(44,speed); // Right motor pwm
    if (direction == forward) {
      cbi(PORTA, PINA0);
      sbi(PORTA, PINA1);
    } 
    else if (direction == backward) {
      sbi(PORTA, PINA0);
      cbi(PORTA, PINA1);
    }
  }
}

/* Interrupt routine and encoder read functions - I read using the port registers for faster processing */
void leftEncoder() { 
  if(PIND & _BV(PIND4)) // read pin 4
    leftCounter--;
  else
    leftCounter++;    
}
void rightEncoder() {
  if(PIND & _BV(PIND5)) // read pin 5
    rightCounter--;
  else
    rightCounter++;  
}
long readLeftEncoder() { // The encoders decrease when motors are traveling forward and increase when traveling backward
  return leftCounter;
}
long readRightEncoder() {
  return rightCounter;
}

void stopAndReset() {
  stopMotor(left);
  stopMotor(right);  
  lastError = 0;
  iTerm = 0;
  targetPosition = wheelPosition;
}

void stopMotor(Command motor) {  
  if (motor == left) {
    setPWM(45, 800); // Set high
    sbi(PORTA, PINA2);
    sbi(PORTA, PINA3);
  } 
  else if (motor == right) {
    setPWM(44, 800); // Set high
    sbi(PORTA, PINA0);
    sbi(PORTA, PINA1);
  }
}

void PID(double restAngle, double offset, double turning) {
  /* Steer robot */
  if (steerForward) {
    offset += (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing
    restAngle -= offset;
  } 
  else if (steerBackward) {
    offset -= (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing
    restAngle += offset;
  }
  /* Brake */
  else if (steerStop) {
    long positionError = wheelPosition - targetPosition;
    if (abs(positionError) > zoneA) // Inside zone A
      restAngle -= (double)positionError/positionScaleA;
    else if (abs(positionError) > zoneB) // Inside zone B
      restAngle -= (double)positionError/positionScaleB;
    else // Inside zone C
      restAngle -= (double)positionError/positionScaleC;   
    restAngle -= (double)wheelVelocity/velocityScaleStop;
    if (restAngle < 160) // Limit rest Angle
      restAngle = 160;
    else if (restAngle > 200)
      restAngle = 200;
  }
  /* Update PID values */
  double error = (restAngle - pitch);
  double pTerm = Kp * error;
  iTerm += Ki * error;
  double dTerm = Kd * (error - lastError);
  lastError = error;
  double PIDValue = pTerm + iTerm + dTerm;

  /* Steer robot sideways */
  double PIDLeft;
  double PIDRight;
  if (steerLeft) {
    turning -= abs((double)wheelVelocity/velocityScaleTurning); // Scale down at high speed
    if(turning < 0)
      turning = 0;
    PIDLeft = PIDValue-turning;
    PIDRight = PIDValue+turning;
  }
  else if (steerRight) {
    turning -= abs((double)wheelVelocity/velocityScaleTurning); // Scale down at high speed
    if(turning < 0)
      turning = 0;
    PIDLeft = PIDValue+turning;
    PIDRight = PIDValue-turning;
  }
  else {
    PIDLeft = PIDValue;
    PIDRight = PIDValue;
  }

  /* Set PWM Values */
  if (PIDLeft >= 0)
    moveMotor(left, forward, PIDLeft);
  else
    moveMotor(left, backward, PIDLeft * -1);
  if (PIDRight >= 0)
    moveMotor(right, forward, PIDRight);
  else
    moveMotor(right, backward, PIDRight * -1);
}

My robot is laying down on the table. If I try to raise it up in an upright position motors immediately go at full power, same direction as you can see in PWMValue.txt. The pwm value is always the same. Kp is =1, so where does PWMValue become so BIG?

Of course there are software modifications to do and I‚Äôm here to understand where and why: IMU angle values go from -90¬įto0¬į and from 0¬įto90¬į, so I‚Äôm asking if double error= restangle-pitch; doesn‚Äôt need an abs() for pitch.

I’ve set double targetAngle = 0; in PID variables declaration. Is it the first used restAngle?

OUTPUT_example.txt (4.89 KB)

PWMValue.txt (260 Bytes)

variab.txt (3.51 KB)