Problem with mobile robot

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!

control_dos_ruedas.ino (7.05 KB)

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

Example:

// 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;
}