Problem with mobile robot

[rubensabuu]

Hello guys, im working with a robot and i need to apply a simply control with Arduino for follow a linear trajectory, and it doesnt seems so difficult. My robot is composed by 2 wheel working with PWM (sorry for my english)

void programaControlAngulo() {
  e0 = ref - orientation;
  inA = KpA * e0;
  inB = KpB * e0;
}

This is my basic control, where ref is the reference i want to reach and orientation is the real orientation at the moment.

And now, we apply the control in loop:

  programaControlAngulo();
  pwm_valueA = pwm_valueA + inA;
  pwm_valueB = pwm_valueB - inB;
  if (pwm_valueB > 255) {
    pwm_valueB = 120;
  }
  if (pwm_valueB < 0) {
    pwm_valueB = 0;
  }
    if (pwm_valueA > 255) {
    pwm_valueA = 120;
  }
  if (pwm_valueA < 0) {
    pwm_valueA = 0;
  }

BNO-055 displays orientation 0 to 360 degrees. My problem is that: when i have 0 degrees on reference, my robot varies the orientation from 355 to 5 degrees to finally reach 0 degrees. The problem is when orientation is 355 degrees, my robot turn a full round to the contrary side (355 -> 300 -> 250... ->5). When the robot reachs again 5 degrees it "fails" and he gets again 355 degrees so turn again a full round.

If you dont understand so much my topic, ask me questions! Thank you vey much!

[jremington]

To avoid that problem, you can map the heading error to the range -180 to 179, with 0 being "on course".

Example:

Code: [Select]

// routine to calculate heading error in degrees, taking into account compass wrap

int heading_error(int bearing, int current_heading)
{
 int error = current_heading - bearing;
 if (error >  180) error -= 360;
 if (error < -180) error += 360;
 return error;
}