PID tuning balancing robot

Hi everybody,

I’ve uploaded this balancing robot video to explain my probems with balancing robot. Maybe it’s too aggressive settings of pid variables, but I’m waiting for new large wheels (90x100 are unable to grip the ground well).

/* PID variables */
double Kp = 30;            //                                                    25     
double Ki = 0.001;        // >0.1 makes wheels run forever      0.001     
double Kd = 0;             //                                                     0

Any advices?

def.ino (12.9 KB)

Try setting Ki to 0 and then tune Kp down to see if that reduces the violence of the oscillations.

Thank you john.

I was trying with heavier wheels such as 70x25 scooter wheels.
In the video I am using pololu 90x10 mm wheels.
I’ve chosen heavier ones to tranfer more torque to the ground (it seems not only a grip problem).

Any advices?

I've chosen heavier ones to tranfer more torque to the ground (it seems not only a grip problem).

I haven’t done any PID design since college, so don’t take this as highly authoritative. What I can tell you is that the purpose of the differential (D) term in a PID control law is damping, which you seem to be lacking. Setting KI to zero sounds like a good idea temporarily, in order to tune the KP and KD separately. But I do think you’ll want some KD, and in the end, and you’ll want to put some KI back in. How much KD? I really don’t know. Try trial and error.

try adjusting your proportional only until it can't catch itself no integral yet the increase it till it osculates back and forth... add integral back in until it slows down add derivative in to recover quickly

also note that the motors don't start turning until the PWM (Analog Out) gets to x% so you need to skip that or you waste time getting the motor to reverse directions when needed

Video of my balancing bot :) https://youtu.be/H5M0JTMq0jA

my PID settings:

consKp=10, consKi=40, consKd=.4;

Motor start: int MStartA = 25, MStartB = 25; // Starting of Motor Drive: PWM starts at 25 and varies to 255(always on) at what point does the motor start motion when loaded?

Have you been able to resolve the oscillation with the PID Loop?
What are you using for your gyro?

Did you see the Video?

Arduino Uno Clone balancing Bot

My balancing bot did the same thing. The motors are so powerful that it would start hopping and bouncing all over the place :slight_smile:

Notice the map() function uses the MStart values to reverse the motors fast!!! this was the most important part for me in quickly controlling the balance!
This prevents the PID from having to swing through Zero and back to 25 before the motor would start powering the other direction

// NOTE: Output = feed from PID loop
 
int MStartA = 25, MStartB = 25;   // Starting of Motor Drive: PWM starts at 20 and varies to 255(always on) at what point does the motor start motion when loaded?


void DirectDrive(double Output){
  static double LastOutput; // used to check for motor direction change
  int OutA = map(constrain (abs (Output) ,0,255),0,255,MStartA,255) ;  // Sets the PWM for Motor A
  int OutB = map(constrain (abs (Output) ,0,255),0,255,MStartB,255);  // Sets the PWM for Motor B
  if ((Output == 0) ||((LastOutput > 0) != (Output > 0))){
 // go to Cost mode on the H Bridge 
  digitalWrite(Forward_A, Disable);
  digitalWrite(Reverse_A, Disable); 
  digitalWrite(Forward_B, Disable);
  digitalWrite(Reverse_B, Disable); 
  }
   LastOutput = Output; // Store for Direction Change
  analogWrite(PWM_A, OutA); // Set PWM for Motor A
  analogWrite(PWM_B, OutB); // Set PWM for Motor B
  if(Output < 0){ // Enable Forward direction on H-Bridge
    digitalWrite(Forward_A, Enable);
    digitalWrite(Reverse_A, Disable);
    digitalWrite(Forward_B, Enable);
    digitalWrite(Reverse_B, Disable);

  }
  if(Output > 0){ // Enable Reverse direction on H-Bridge
    digitalWrite(Forward_A, Disable);
    digitalWrite(Reverse_A, Enable);
    digitalWrite(Forward_B, Disable);
    digitalWrite(Reverse_B, Enable);


  }
}

Hi, If you are looking for help, you need to go and re edit your thread title. "look at the video please" is not going to get you many forum members looking at your thread. There are some members out there that have experience with balancing robots.

You can edit the subject because you stated it. Something thing like

"PID tuning balancing robot, please help"

Would make more sense.

Tom.... :)

THANK YOU ALL!

@zhomeslice: I’m not using digitalWrite() function, but direct port access, that is faster, BUT I’VE NOT solved the swing issue you are talking about.

To solve the problem, can I add to my code the coast mode when output is nearby zero, as you did?
In my code, when output is near zero, deadband makes pwm become 30.

void moveMotor(Command motor, Command direction, double speedRaw) { 
// Speed is a value in percentage 0-100%
  if(speedRaw > 100)
    speedRaw = 100;
  int speed = speedRaw*800/100; // Scale from 100 to PWMVALUE       (max PWMVALUE=800)

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

 /* Set PWM Values */
  if (PIDLeft >= 0)
    moveMotor(left, forward, PIDLeft+30);  //to resolve MOTOR DEADBAND
  else
    moveMotor(left, backward, -PIDLeft-30);
  if (PIDRight >= 0)
    moveMotor(right, forward, PIDRight+30);
  else
    moveMotor(right, backward, -PIDRight-30);

ps. I am still using 90x10 pololu (thin) wheels because I want to make it balance with that on. BUT I need more torque, so heavier wheels or structure modification. I choose the second one.
Need I to make it taller? I’m thinking about trying to balance a broomstick on my finger… if long it makes things easier, maybe with lipo battery on the top level.
My robot is 10in tall and 8in wide. Any tips?

I really like how you are controlling the direction of the motors :slight_smile:

I modified your code to help you add the coast mode

Note: I did not test any of the code here to directly work with your code i have tried to adapt some of my code to be compatible. I am using similar code within my balancing bot now. I hope this will help you overcome some of the issues you mentioned and I certainly faced lol.

I am using the FIFO buffer of the mpu6050 or MPU9155 which i have tested. It should work with other MPU6XXX & MPU9xxx. The MPU9xxx has the built in magnetometer.

also I am curious about how you are constraining you PID output value you may be having problems with what is called windup

#include <PID_v1.h>

// PID variables
double Setpoint, Input, Output;

double consKp=10, consKi=40, consKd=.4;

PID myPID(&Input, &Output, &Setpoint,consKp,consKi,consKd, REVERSE);



  myPID.SetOutputLimits(-255.0,255.0); // Limits my PID Output and automatically handles "windup" for you
  myPID.SetSampleTime(10);

also I handle the starting of the balancing with:

// Input is in +- degrees from vertical of the balancing from my MPU6050 FIFO buffer
  I = abs(Input);



  if(I > 45)
  {
    myPID.SetMode(MANUAL); // Stop the PID calculations prevents windup
    //Serial.println("bang");
    Output = 0;
    DirectDrive();
    LastInput = 0;

    return;
  } 
  else if(I < 1)
  {
    myPID.SetMode(AUTOMATIC); // Start the PID Calculations prevents windup
    //Serial.println("BOOYAH");

  }

the coast mode is to make sure that the motors of my H bridges are off before switching to the other direction.
this is only that way for a fraction of a microsecond second. but it was critical if the Hbridge transition to the correct direction before I apply power

The cost mode was necessary because if I didn’t I would command apply power before switching the Hbridge configuration to the direction I want

void moveMotor(Command motor, double speedRaw) {
  static double LastOutputLeft, LastOutputRight; // used to check for motor direction change
  // Speed is a value in percentage 0-100%
  if (speedRaw > 100)  
    speedRaw = 100;
//*** ^^^^^^ This may be causing some windup in your pid loop the pid loop may need to take time to get back below 100 and now its behind in catching the swing to the opposite side Note My PID output was limited to -255 ~ 255 The PID code protected me from windup
// I use constrain because I used addition for causing the bot to turn I added a little extra speed to the left or right motors causing them to kick that sied around and so if I didnt constrain the number it could exceed 255

  int speed = ((abs(speedRaw) + 30) * 800) / 100; // Scale from 100 to PWMVALUE       (max PWMVALUE=800)
  // you might try the the following line after you set your ranges 
  
  // int speed = map(constrain (abs (speedRaw) , 0, 100)    , 0, 255, MStartA, 255) ; 
  
  //Here is what I did with the above line 
  // int x = abs (speedRaw) // I need a positive number only but I still keep the Signed number to set the direction later >0 = forward <0 = backwards
  // int x = constrain (x , 0, 100) // The range must stay within these values even when I add for turning as shown in my video :)
  // int speed = map(x   , 0, 100, 30, 255) ; Shift the range from 0~255 to 30~255 (The range of the analogWrite())
  
  // what we have here is speedRaw us a +- Double that will give us a way to set direction and check for direction change 
  // and speed which is a positive number restricted to be within a specific range
  
  if (motor == left) {
    if ((speed == 0) || ((LastOutputLeft > 0) != (speed > 0))) {
         // Disable your H-Bridges both low and high open (Breaking off) this will allow us to set the speed and not force a reverse affect for a second
      
    }
      
    LastOutputLeft = Output; // Store for Direction Change
    
   // Now we can set the speed
    setPWM(45, speed); // Left motor pwm   
    if (speedRaw > 0) {
      cbi(PORTA, PINA2);
      sbi(PORTA, PINA3);
    }
    if (speedRaw < 0) {
      sbi(PORTA, PINA2);
      cbi(PORTA, PINA3);
    }
  }
  else if (motor == right) {
        if ((speed == 0) || ((LastOutputRight > 0) != (speed > 0))) {
         // Disable your H-Bridges both low and high open (Breaking off) this will allow us to set the speed and not force a reverse affect for a second
      
    }
      
    LastOutputRight = Output; // Store for Direction Change
    
   // Now we can set the speed
    setPWM(44, speed); // Right motor pwm
    if (speedRaw > 0) {
      cbi(PORTA, PINA0);
      sbi(PORTA, PINA1);
    }
     if (speedRaw < 0) {
      sbi(PORTA, PINA0);
      cbi(PORTA, PINA1);
    }
  }
}

/* Set PWM Values */
moveMotor(left, PIDLeft);  //to resolve MOTOR DEADBAND
moveMotor(right, PIDRight);  //to resolve MOTOR DEADBAND

@zhomeslice what you did for me was great! you game me energy! thank you.

I’m asking how do you use encoders.
is PID_v1 library already working for you on updating wheel position?

  /* Calculate pitch */
     pitch = ypr[1] * 180/M_PI; // Calculate the angle using DMP library
     
   /* Drive motors */
     timer = micros();
     PID(targetAngle,targetOffset,turningOffset,(float)(timer - pidTimer) / 1000000.0f);   
     pidTimer = timer;

  /* Update encoders */
  timer = millis();
  if (timer - encoderTimer >= 100) { // Update encoder values every 100ms
    encoderTimer = timer;
    wheelPosition = readLeftEncoder() + readRightEncoder();
    wheelVelocity = wheelPosition - lastWheelPosition;
    lastWheelPosition = wheelPosition;
    //Serial.print(wheelPosition);Serial.print('\t');Serial.print(targetPosition);Serial.print('\t');Serial.println(wheelVelocity);
    if (abs(wheelVelocity) <= 40 && !stopped) { // Set new targetPosition if braking
      targetPosition = wheelPosition;
      stopped = true;
    }}

See the attached code to see if I’ve done some big errors, I’m not so strong coding.

hjhjh.txt (2.6 KB)

I didn't use encoders.

If i were to use encoders I would use them to monitor too much speed in the direction of tilt when requesting location changes. I would use them to return to origin otherwise they are not needed.

A sequence could be change the balance setpoint from 0 to +-X degrees causing the robot to be slightly off balance. the PID loop would try to maintain that off balanced angle. and the balancing bot would accelerate. as the balancing bot got to a rate (using your encoder to get the rate) of you liking then trim the +-X degree setpoint (simple proportional control only) back closer to zero to keep it from running away. Then as the balancing bot got close to the position you desire(using the encoder to monitor distance) have the setpoint return to Zero 0 restoring balance at the desired locaiton.

My balancing bot has no encoders and only uses adjustments in setpoint to get forward motion and because I have no feedback it is difficult to keep from accellerating too fast and having it fall over. In my video I adjusted the setpoint from 0° to +- 5~10° (can't quite remember) using a wireless WII-Chuck thumb stick linked in via I2c. I truned the bot by adding extra power to the right or left motor depending on which way I wanted to turn.

Isn’t it breaking mode? both low and high open. But I need coast mode? Any tips?

void moveMotor(Command motor, double speedRaw) {
  static double LastOutputLeft, LastOutputRight; // used to check for motor direction change
  // Speed is a value in percentage 0-100%
  
  if (speedRaw > 100)  
    speedRaw = 100;
  int speed = ((abs(speedRaw) + 30) * 800) / 100; // Scale from 100 to PWMVALUE
  
  int speed = map(constrain (abs (speedRaw) , 0, 100)    , 0, 255, 30, 255) ; 
  
  if (motor == left) {
    if ((speed == 0) || ((LastOutputLeft > 0) != (speed > 0))) {
        // Disable your H-Bridges both low and high open (Breaking off) this will allow us to set the speed and not force a reverse affect for a second
         stopMotor(right);
         stopMotor(left);
    }
      
    LastOutputLeft = Output; // Store for Direction Change
    
   // Now we can set the speed
    setPWM(45, speed); // Left motor pwm   
    if (speedRaw > 0) {
      cbi(PORTA, PINA2);
      sbi(PORTA, PINA3);
    }
    if (speedRaw < 0) {
      sbi(PORTA, PINA2);
      cbi(PORTA, PINA3);
    }


................. same for right motor

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

Could you post the part you are using for the H bridge I need to understand hour you are enabling the H bridge also a schematic of the h bridge connections could be helpful.

I'm interested in what gyro you are using. I used the mpu6050 and if also had success it is cousin mpu 9155

GY-521 MPU6050

See the files in attachments.

With the H-bridge I’m using port manipulation here
that is making everything difficult to read.

#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))

int speed = speedRaw*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);
    }

Do you suggested me to brake motors(closed circuit) or to coast them(open circuit)?
Do you know how to set coast mode?

Cattura.PNG

vnh3sp30.pdf (572 KB)

Thanks :slight_smile: That is an excellent H-Bridge Chip!
To get the motors to coast by disabling the High and Low side of the H-Bridge set the DIAGX/ENX ( DIAGA/ENA and DIAGB/ENB) to low and the motors should coast. These can be connected together and attached to a single pin of the Arduino to achieve the desired result. this will disable both the high and low sides of the H-bridges allowing your motor to coast between switching directions

MPU6050 is sweet also and is exactly what I use. Did you get the DMP FIFO to work for you. I discovered a problem with the MPU6050 library for the DMP FIFO portion.

FYI the DMP (Digital Motion Processing) automatically calculates Radians for you without Requiring you to use the Arduino to change the raw rates into an angles.

in the MPU6050_6Axis_MotionApps20.h file I had to modify this line to slow down the buffer processing of the data allowing my arduino to keep up. It eliminated the glitchy data that arrived on occasion before i found this i just ignored that data without any ill affects on the balance now with this corrected it can only be better :slight_smile:
MPU6050_6Axis_MotionApps20.h
@Line 261

// lots of number before this :)
//    0x02,   0x16,   0x02,   0x00, 0x01                // D_0_22 inv_set_fifo_rate
// changed it to:
    0x02,   0x16,   0x02,   0x00, 0x09                // D_0_22 inv_set_fifo_rate

    // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
    // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data.
    // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value))

    // It is important to make sure the host processor can keep up with reading and processing
    // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea.

My apologies for sloppy coding I didn’t have time to edit or clean it up…
The following code is just a portion of the code i’m using in my balancing robot specifically for collecting data from the MPU 6050 using MPU6050_6Axis_MotionApps20.h as the library.

I use the following to get the DMP Data directly from the MPU6050 (In my case MPU9155 which has the MPU6050 and a compass chip attached to it internally. The MPU6050_6Axis_MotionApps20.h code works with both chips!)

#include "MPU6050_6Axis_MotionApps20.h"


MPU6050 mpu;


// MPU control/status vars
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer



void dmpDataReady() {
    mpuInterrupt = true;
}

void setup() {
  MPU6050Connect();

}


void loop() {
  if (mpuInterrupt ) { // the MPU int pin triggers high when DMP data is available in the FIFO buffer
    GetDMP();// Get the data and process it
    myPID.Compute();
    DirectDrive();  // Controls the motors after calculation of PID
  }
}

void MPUMath(){ // this included gravity in the angle equation. My MPU6050 is not at the bottom it is near the top of my robot and slightly off center! and it works great because of the added calculation using the accelerometer 
  mpu.dmpGetQuaternion(&q, fifoBuffer);
  mpu.dmpGetGravity(&gravity, &q);
  mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  Input = ypr[1] * 180/M_PI;
}


void MPU6050Connect(){

  // initialize device
  mpu.initialize();

  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));


  // load and configure the DMP
  Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();

  // supply your own gyro offsets here, scaled for min sensitivity
byte  gyroBias[6] = {0,1,255,242,0,8};
byte  accelBias[6] = {232,5,25,136,43,201};
  mpu.LoadGyroBiases(gyroBias);
  mpu.LoadAccelerometerBiases(accelBias);

  // make sure it worked (returns 0 if so)

  // turn on the DMP, now that it's ready
  //  Serial.println(F("Enabling DMP..."));
  mpu.setDMPEnabled(true);

  // enable Arduino interrupt detection
  //  Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        attachInterrupt(0, dmpDataReady, RISING);
  mpuIntStatus = mpu.getIntStatus();

  // get expected DMP packet size for later comparison
  packetSize = mpu.dmpGetFIFOPacketSize();
  // lcd.at(0,3,"Complete");

  delay(1000);
  //  lcd.empty();  
  mpu.resetFIFO();
  mpuInterrupt = false;
}



void GetDMP(){
  // reset interrupt flag and get INT_STATUS byte
  mpuInterrupt = false;
  // get current FIFO count

  fifoCount = mpu.getFIFOCount();

  //  Serial.print("\t");
  //  Serial.print(OCR1A);
  if(fifoCount < packetSize){
    mpu.resetFIFO();
    OCR1A = min(OCR1A + 5,180);
    return;  
  } 
  else if (fifoCount > (packetSize * 2) -1){
    OCR1A = max(OCR1A - 1,140);
  }

  if (fifoCount == 1024) {
    // reset so we can continue cleanly
    mpu.resetFIFO();
    //Serial.println(F("FIFO overflow!"));
    // lcd.at( 0, 1, "FIFO overflow");

    return;
    // otherwise, check for DMP data ready interrupt (this should happen frequently)
  } 

  while (fifoCount  >= packetSize) { // read all packet from FIFO looking for the latest

    if(fifoCount < packetSize)return;
    mpu.getFIFOBytes(fifoBuffer, packetSize);//get the packet
    fifoCount -= packetSize;
    if(fifoCount < packetSize) break;
  }


  //stevetsorensen@gmail.com  Serial.print(fifoCount);
  // display Euler angles in degrees

  // blink LED to indicate activity
  blinkState = !blinkState;
  digitalWrite(LED_PIN, blinkState);
  SampleCnt++;
  HBridgeTimer = Timer + 30;
  MPUMath();
  if (fifoCount > 0){// if there is anything left buffer is corrupt
    mpu.resetFIFO(); 
    //    Serial.println(F("FIFO Corruption!"));
    //    OCR1A = min(OCR1A + 1,170);
    return;
  }
}

awesome!!

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

if ((speed == 0) || ((LastOutputRight > 0) != (speed > 0))) { // Disable your H-Bridges both low and high open (Breaking off) this will allow us to set the speed and not force a reverse affect for a second stopMotor(right); ** stopMotor(left);**

Is that both low and high open you were taking about?

Yes set both low and high openon your h bridges is how I coat the motors. Setting the h bridge to low or high for both sides will cause a breaking action based on your pwm setting. When breaking I prefer the low side because of the lower resistance of the MOSFET transistors. Setting both to low and turning the PWM output to full 100% will make the motors difficult to turn.

I do this before adjusting speed when switching directions. Costing the motors seems a better solution than turning breaking on. Now everything happens so fast in the end it may not even matter. In my case I wanted a clean transition from forward to reverse. I chose to coast the motors then adjust the pwm to the new vale then engage the motors in the new direction.

motors are not moving… : (

void moveMotor(Command motor, double speedRaw, double x) {
  static double LastOutputLeft, LastOutputRight; // used to check for motor direction change
  
   int speed = map(constrain (abs (speedRaw) , 0, 100)    , 0, 255, 30, 255) ; 
  
  if (motor == left) {
    if ((speed == 0) || ((LastOutputLeft > 0) != (speed > 0))) {
         // Disable your H-Bridges both low and high open (Breaking off) this will allow us to set the speed and not force a reverse affect for a second
         stopMotor(right);
         stopMotor(left);
    }
      
    LastOutputLeft = x; // Store for Direction Change
    
   // Now we can set the speed
    setPWM(45, speed); // Left motor pwm   
    if (speedRaw > 0) {
      cbi(PORTA, PINA2);
      sbi(PORTA, PINA3);
    }
    if (speedRaw < 0) {
      sbi(PORTA, PINA2);
      cbi(PORTA, PINA3);
    }
  }
  else if (motor == right) {
        if ((speed == 0) || ((LastOutputRight > 0) != (speed > 0))) {
         // Disable your H-Bridges both low and high open (Breaking off) this will allow us to set the speed and not force a reverse affect for a second
     stopMotor(left);
     stopMotor(right);
      
    }
      
    LastOutputRight = x; // Store for Direction Change
    
   // Now we can set the speed
    setPWM(44, speed); // Right motor pwm
    if (speedRaw > 0) {
      cbi(PORTA, PINA0);
      sbi(PORTA, PINA1);
    }
     if (speedRaw < 0) {
      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, double dt) {

  /* Update PID values */
  double error = restAngle - pitch;
  double pTerm = Kp * error;
  iTerm += Ki * error * dt;
  iTerm = constrain(iTerm, -100, 100);    // Limit the integrated error - prevents windup
  double dTerm = Kd * (error - lastError)/dt;    
  lastError = error;
  double PIDValue = pTerm + iTerm - dTerm;

  /* Steer robot sideways */
 double PIDLeft;
 double PIDRight;
 double PIDL;
 double PIDR;
 
    PIDLeft = PIDValue;
    PIDRight = PIDValue;

    PIDLeft = PIDL;
    PIDRight = PIDR;
 // PIDRight *= 0.95; // compensate for difference in the motors

  /* Set PWM Values */
  
    moveMotor(left, PIDLeft, PIDL);  

   
    moveMotor(right, PIDRight, PIDR);
 
}

motors are not moving… : (

What does the “stopMotor();” function do electronically?

The sequence needs to be like:

When speed transitions to opposite direction:

  1. before adjusting PWM setting ( which is used for both directions)
  2. Set H-bridge to cost in your case disable the H-Bridge set #diag/en to low
  3. adjust PWM to the new value for opposite direction
  4. Set H-Bridge to reverse direction
  5. and with your H-Bridge Enable H-Bridge operation set #diag/en to high

I am assuming you using a breakout board for the H-bridge something like: