Help with self balancing robot

Projects General Guidance
1

Can somebody give me a hand and see what I can do to make this work? Readings from MPU6050 are not correct regardless of what I try.

#include "Wire.h"
#include "I2Cdev.h"
#include "math.h"
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

const int IN1 = 9;
const int IN2 = 8;
const int IN3 = 10;
const int IN4 = 11;
const int ENA = 5;
const int ENB = 6;

#define Kp  100
#define Kd  0.05
#define Ki  40
#define sampleTime  0.005
#define targetAngle -2.5

Adafruit_MPU6050 mpu;

int x, count, time;
float motorPower, currentAngle, accAngle, gyroRate, gyroAngle, prevAngle, error, errorSum, A, B, C;
sensors_event_t a, g, temp;


void setup() {

// MOTOR SETUP
  Serial.begin(9600);

  while (!Serial)
    delay(10); // will pause Zero, Leonardo, etc until serial console opens

  pinMode (IN1, OUTPUT);
  pinMode (IN2, OUTPUT);
  pinMode (IN3, OUTPUT);
  pinMode (IN4, OUTPUT);
  pinMode (ENA, OUTPUT);
  pinMode (ENB, OUTPUT);

  analogWrite(ENA, 0);
  analogWrite(ENB, 0); 

  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
//END MOTOR SETUP

//CONFIGURA O PINO DO LED
  pinMode(13, OUTPUT);

  // Try to initialize!
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  Serial.print("Accelerometer range set to: ");
  switch (mpu.getAccelerometerRange()) {
  case MPU6050_RANGE_2_G:
    Serial.println("+-2G");
    break;
  case MPU6050_RANGE_4_G:
    Serial.println("+-4G");
    break;
  case MPU6050_RANGE_8_G:
    Serial.println("+-8G");
    break;
  case MPU6050_RANGE_16_G:
    Serial.println("+-16G");
    break;
  }
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  Serial.print("Gyro range set to: ");
  switch (mpu.getGyroRange()) {
  case MPU6050_RANGE_250_DEG:
    Serial.println("+- 250 deg/s");
    break;
  case MPU6050_RANGE_500_DEG:
    Serial.println("+- 500 deg/s");
    break;
  case MPU6050_RANGE_1000_DEG:
    Serial.println("+- 1000 deg/s");
    break;
  case MPU6050_RANGE_2000_DEG:
    Serial.println("+- 2000 deg/s");
    break;
  }

  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  Serial.print("Filter bandwidth set to: ");
  switch (mpu.getFilterBandwidth()) {
  case MPU6050_BAND_260_HZ:
    Serial.println("260 Hz");
    break;
  case MPU6050_BAND_184_HZ:
    Serial.println("184 Hz");
    break;
  case MPU6050_BAND_94_HZ:
    Serial.println("94 Hz");
    break;
  case MPU6050_BAND_44_HZ:
    Serial.println("44 Hz");
    break;
  case MPU6050_BAND_21_HZ:
    Serial.println("21 Hz");
    break;
  case MPU6050_BAND_10_HZ:
    Serial.println("10 Hz");
    break;
  case MPU6050_BAND_5_HZ:
    Serial.println("5 Hz");
    break;
  }

  delay(200);

  // initialize PID sampling loop
  // init_PID();
  

}

void motor_forward(){
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}

void motor_backward(){
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}

void alter_direction(){
  analogWrite(ENA, 0);
  analogWrite(ENB, 0); 
  if (digitalRead( IN1 ) == HIGH){
    Serial.println("IF");
    motor_forward();
  }else{
    Serial.println("ELSE");
    motor_backward();
  }

  time = 1;
  delay(100);
}

// The ISR will be called every 5 milliseconds
void calculate()
{

  // calculate the angle of inclination
  accAngle = atan2(A, B)*RAD_TO_DEG;
  gyroRate = map(C, -32768, 32767, -250, 250);
  gyroAngle = (float)gyroRate*sampleTime;  
  currentAngle = 0.9934*(prevAngle + gyroAngle) + 0.0066*(accAngle);
  Serial.println("MEASUREMENT: ");             
  Serial.print(currentAngle);
  
  error = currentAngle - targetAngle;
  errorSum = errorSum + error;  
  errorSum = constrain(errorSum, -300, 300);
  //calculate output from P, I and D values
  motorPower = Kp*(error) + Ki*(errorSum)*sampleTime - Kd*(currentAngle-prevAngle)/sampleTime;
  prevAngle = currentAngle;
  // toggle the led on pin13 every second
  count++;
  if(count == 5)  {
    count = 0;
    digitalWrite(13, !digitalRead(13));
  }
}

void loop() {

  if(millis() - time >= 5){
    calculate();
    time = millis();
  }
    /* Get new sensor events with the readings */
  
  mpu.getEvent(&a, &g, &temp);

  A = a.acceleration.x;
  B = a.acceleration.z;
  C = g.gyro.y;
  

  // set motor power after constraining it
  motorPower = constrain(motorPower, 150, 255);

  if(currentAngle>0){
    motor_forward();
  }else{
    motor_backward();
  }

  analogWrite(ENA, motorPower);
  analogWrite(ENB, motorPower);

}
2

Is the 6950 connected to the controller board? If it is connected please post schematics showing how the setup is done.

3

I believe you must apply kalman filtering. The following links may help you.

4

This won't fix your mpu issues, but will bite you if you let it run very long... millis() returns an unsigned long value, not an int which will overflow very quickly.

5

What readings are you getting and what are the 'correct' values you are expecting?

6

I'm trying to get accurate(without drifting) y measurements to balance the robot.
I'm about to post the schematics

7

I have the MPU connected to the arduino nano like this:
SCL -> A5
SDA -> A4
INT -> D2

I want to get accurate Y measurements in order to use as input to a PID function to generate a number for the motor PWM.

The PWM pins for the motors are D5 and D6.

This is the second time I'm trying to build a self-balancing robot and struggle like this. For the first one, I was using a seeduino and stepper motors, this time I'm trying a Nano and dc motors to see if it's easier.

The plan is to build a large self-balancing robot in the future. This is my learning projects.

Thanks for the help.

8

The request was for schematics showing the lot, power supply, drivers, motors.. If there are mistakes in the hardware it's a waste time looking at any coding.
Did You read and consider reply #4 and #5?

9

Hi, I responded #5 and liked #4.

diagram
diagram793×434 55.3 KB

10

What kind of 9 volt battery is it? A PP3 fire alarm battery?

11

No, 6 AA batteries.

12

You have to use Kalman Filter. Without that, these gyros are very likely to output unrealistic data.

13

Hi, did you manage to complete your project?
I have a general question to all, is it possible to keep a robot balanced from a stable set point by using only one axis of the accelerometer? I am trying to keep my self balancing robot as simple as possible but I don't mind implementing a PID loop or apply some filtering if required. I just want the easiest solution to keep my robot still in place without tilting too much forward or backwards.

14

Hi, I haven't finished that... Apparently, it's tough to figure out this stuff and get good answers.
I also like the easiest ways to do things but it's not always that simple. But I think you would need at least, to measure the angle and the acceleration of the said axis. That way you can react to whatever motion your robot is going through. It's important to notice that those units drift a bit if you don't set it up properly and you will end up losing the balance regardless. Also, I had about 3 modules that didn't work for me, and one Arduino Nano (knock-off brand) that was messed up and just wouldn't work. I would invest in good-quality hardware for this type of project.

I've been working on other projects lately and might get back to this soon. I'll update this topic whenever I do so.

15

The simple 1-D complementary filter used in your code is essentially equivalent to a 1-D Kalman filter, and will work fine if correctly implemented.

However there are several major errors in the code, and it can't possibly work until all of these are fixed.

  1. Gyro range is set to 500 DPS, but the scaling assumes 250 DPS.
mpu.setGyroRange(MPU6050_RANGE_500_DEG);
...
  gyroRate = map(C, -32768, 32767, -250, 250);
  1. The angular integration is not correct. This line assumes that the tilt angle was zero at the start of the interval, and calculates how much it changed in 5 milliseconds, which won't be much.

gyroAngle = (float)gyroRate*sampleTime;

The fix is to integrate the angular rate, starting from some reasonable value for gyroAngle (like zero, when the robot is held vertical). Note the "+":

gyroAngle += (float)gyroRate*sampleTime;

  1. The gyro rate is almost certainly wrong, because the gyro is not calibrated. You need to calibrate the gyro and subtract the fixed rate offset in y.

  2. As already mentioned "time" is int, which overflows after about 32 seconds, giving negative values, which produces nonsense:
    time = millis();

  3. Make sure that the static tilt angle accAngle, calculated from the accelerometer, has the right sign and magnitude by printing out some values, while you tilt the robot by hand.

accAngle = atan2(A, B)*RAD_TO_DEG;

  1. Make sure that the integrated angle agrees in sign and value to the accelerometer tilt angle, when you tilt the robot by hand.

  2. Once you fix all of these things, Kp, Ki and Kd will need to be (re)determined. Look up "PID tuning" for tutorials.

16

Hi all, wanted to post some progress I made with my own project. I am using an Arduino Uno with the MPU6050 and the L298N as motor driver.

My idea is to get the reading on only one axis of the accelerometer and use the angle to generated a PWM with a PD loop.
So far the angle read for the chosen axis is correct, I get 0 deg when the robot is balanced, and the angle keeps increasing when tilted. The direction it gets tilted it gives me either a positive or negative value. I am using the adafruit library to process data to keep my work simple. (NOTE: Don't use the atan to calculate the angle or you will get incorrect reading at 90 deg. )

Then I have implemented a sort of logic to determine the proportional and derivative aspect of the error in two separate variables, then combine them and feed them to the PWM value.

Finally I have tested the robot. It moves to the correct direction when tilted, and the power increases as the angle goes up. Obviously, it doesn't balance. I haven't tuned the parameters and I realised that the motors I have are not exactly the same. One of them starts moving before the other and it is generally a bit faster. I am going to order 2 new motors and refine the code meanwhile.

IN CASE this approach won't work (very likely :P) I will have to use the gyro and integrate the two readings as mentioned by other users as well.

code used:

#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>


Adafruit_MPU6050 mpu;

//Bunch of variables
int pwm;
float err, Kp = 89, Kd = 0; //this sets the sensitivity of the motors in regards to tilt. Tune it with trial and error
bool fwd = false;  //decide to drive motor fwd or bkwd
float errd;  //variable to store derivative of error
float fltr; //use this to smooth out rough data
double Ox=0;  //store previous reading for next cycle

//pins for the motor driver

#define M1_d 2
#define M1_dr 3
#define M1_speed 9
#define M2_d 4
#define M2_dr 5
#define M2_speed 10

 
void setup(){
  Serial.begin(115200);

	// Try to initialize!
	if (!mpu.begin()) {
		Serial.println("Failed to find MPU6050 chip");
		while (1) {
		  delay(10);
		}
	}

	// set accelerometer range to +-8G
	mpu.setAccelerometerRange(MPU6050_RANGE_8_G);

	// set gyro range to +- 500 deg/s
	mpu.setGyroRange(MPU6050_RANGE_500_DEG);

	// set filter bandwidth to 21 Hz
	mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

	delay(100);


//set motor driver pin as output
pinMode(M1_d , OUTPUT);
pinMode(M1_dr , OUTPUT);
pinMode(M1_speed , OUTPUT);
pinMode(M2_d , OUTPUT);
pinMode(M2_dr , OUTPUT);
pinMode(M2_speed , OUTPUT);

// make surre the motors are OFF

digitalWrite(M1_d , LOW);
digitalWrite(M1_dr , LOW);
digitalWrite(M1_speed , LOW);
digitalWrite(M2_d , LOW);
digitalWrite(M2_dr , LOW);
digitalWrite(M2_speed , LOW);

delay(2000);
}
void loop(){

	/* Get new sensor events with the readings */
	sensors_event_t a, g, temp;
	mpu.getEvent(&a, &g, &temp);

  //a.acceleration.x  = (1-filtr) * a.acceleration.x + filtr * Ox;  //filterring

  errd = a.acceleration.x - Ox;   //difference from previous reading 
  Ox = a.acceleration.x ; //get ready for next cycle

//Serial.println(a.acceleration.x);  //debug line

//Calculate how many degrees are we far from the centre
err = float(a.acceleration.x); 

if (err > 0){   //if tilted fw/bkw
  //set direction
  fwd = true;
  //bkwd = false;
}
else{
  //set direction
  fwd = false;
  //bkwd = true;
}
err = abs(err); //dont want to feed a negative pwm value
//Serial.println(err);  //debug line

//set motor speed
pwm = err * Kp + errd * Kd;  //value with PD loop
pwm = constrain(pwm, 0 , 255);  //make sure value fits input parameter

analogWrite(M1_speed , pwm);  //set motor speed 
analogWrite(M2_speed , pwm);  //set motor speed 

if (fwd){   //if tilted fw/bkw
  //set direction
  digitalWrite(M1_d , HIGH);
  digitalWrite(M1_dr , LOW);

  digitalWrite(M2_d , HIGH);
  digitalWrite(M2_dr , LOW);
}
else{
  //set direction
  digitalWrite(M1_d , LOW);
  digitalWrite(M1_dr , HIGH);

  digitalWrite(M2_d , LOW);
  digitalWrite(M2_dr , HIGH);
}
//Serial.println(pwm); //debug line

//delay(10);
}
17

UPDATE!!!!!!!!
I am not giving up...yet. I have realised that just the accelerometer is too sensitive to external forces, so the readings are not reliable while the robot is moving. Therefore, I found out how to use primarily the Gyro and complement it with the Accelerometer.
To adjust the values of the PD loop and to calibrate the offset from the desired set point, I decided to put a potentiometer for each value.
There are extra function here and there for quality of life.
My problem right now is that I cannot find the right values to tune the robot.

  • If the Kp is too low, the robot rolls on a side, if it is too high instead, it oscillates aggressively, I feel like a found that the middle point is a Kp of 16.5
  • Then I tried to update the Kd, regardless of the value I put, the robot still doesn't balance, it is either too weak or starts oscillating like crazy.
  • Setting the set point offset is very easy because I am able to make the robot balance for like 10/20 seconds when powered off, then I repeat the same process powering the robot and tuning the pot until the LED turns ON ( it is programmes to do so only at 0 degree with tolerance of 0.3 degrees). Eventually I will hard code also this value like the Gyro offsets.

Here is the code used, let me know if there is something wrong.

//Include I2C library and declare variables
#include <Wire.h>
#include <MPU6050.h>

long loopTimer, loopTimer2;
int temperature;
double accelPitch;
double accelRoll;
long acc_x, acc_y, acc_z;
double accel_x, accel_y, accel_z;
double gyroRoll, gyroPitch, gyroYaw;
int gyro_x, gyro_y, gyro_z;
//long gyro_x_cal, gyro_y_cal, gyro_z_cal, acc_x_cal, acc_y_cal, acc_z_cal;  //dont need this after hard coding the offset
long gyro_x_cal = 181, gyro_y_cal = -655, gyro_z_cal = -39 , acc_x_cal, acc_y_cal, acc_z_cal;
double rotation_x, rotation_y, rotation_z;
double freq, dt;
double tau = 0.98;
double roll = 0;
double pitch = 0;

// 250 deg/s --> 131.0, 500 deg/s --> 65.5, 1000 deg/s --> 32.8, 2000 deg/s --> 16.4
long scaleFactorGyro = 65.5;

// 2g --> 16384 , 4g --> 8192 , 8g --> 4096, 16g --> 2048
long scaleFactorAccel = 8192;


//pins for the motor driver

#define M1_d 2
#define M1_dr 3
#define M1_speed 9
#define M2_d 4
#define M2_dr 5
#define M2_speed 10


//Bunch of variables
int pwm;
float err, Kp = 14.0, Kd = 0; //this sets the sensitivity of the motors in regards to tilt. Tune it with trial and error
bool fwd = false;  //decide to drive motor fwd or bkwd
float errd;  //variable to store derivative of error
float fltr; //use this to smooth out rough data
double Ox=0;  //store previous reading for next cycle
float set_point_offset;  //allowing set point calibration

#define P_pot A0           //pot to set the Kp value 0 to 30
#define D_pot A1           //pot to set the Kd value 0 to 20
#define set_point_offset_pot A2   // use this pot to change the centre set point

#define BALANCE_LED 12    //LED that lights up when robot is balanced

void setup() {
  // Start
  Wire.begin();
  Serial.begin(115200);

  // Setup the registers of the MPU-6050 and start up
  setup_mpu_6050_registers();

  //calibrate_sensor();   //dont need this anymore

  // Display headers
  /*Serial.print("\nNote 1: Yaw is not filtered and will drift!\n");
  Serial.print("\nNote 2: Make sure sampling frequency is ~250 Hz\n");
  Serial.print("Sampling Frequency (Hz)\t\t");
  Serial.print("Roll (deg)\t\t");
  Serial.print("Pitch (deg)\t\t");
  Serial.print("Yaw (deg)\t\t\n");*/

  //set motor driver pin as output
  pinMode(M1_d , OUTPUT);
  pinMode(M1_dr , OUTPUT);
  pinMode(M1_speed , OUTPUT);
  pinMode(M2_d , OUTPUT);
  pinMode(M2_dr , OUTPUT);
  pinMode(M2_speed , OUTPUT);
  pinMode(P_pot , INPUT);
  pinMode(D_pot , INPUT);
  pinMode(set_point_offset_pot , INPUT);

  // make surre the motors are OFF when powered up
  digitalWrite(M1_d , LOW);
  digitalWrite(M1_dr , LOW);
  digitalWrite(M1_speed , LOW);
  digitalWrite(M2_d , LOW);
  digitalWrite(M2_dr , LOW);
  digitalWrite(M2_speed , LOW);

  //set teh LED as output
  pinMode(BALANCE_LED, OUTPUT); 

  // Reset the loop timer
  loopTimer = micros();
  loopTimer2 = micros();
}


void loop() {
  freq = 1/((micros() - loopTimer2) * 1e-6);
  loopTimer2 = micros();
  dt = 1/freq;

  // Read the raw acc data from MPU-6050
  read_mpu_6050_data();

  // Subtract the offset calibration value
  gyro_x -= gyro_x_cal;
  gyro_y -= gyro_y_cal;
  gyro_z -= gyro_z_cal;

  // Convert to instantaneous degrees per second
  rotation_x = (double)gyro_x / (double)scaleFactorGyro;
  rotation_y = (double)gyro_y / (double)scaleFactorGyro;
  rotation_z = (double)gyro_z / (double)scaleFactorGyro;

  // Convert to g force
  accel_x = (double)acc_x / (double)scaleFactorAccel;
  accel_y = (double)acc_y / (double)scaleFactorAccel;
  accel_z = (double)acc_z / (double)scaleFactorAccel;

  // Complementary filter
  accelPitch = atan2(accel_y, accel_z) * RAD_TO_DEG;
  accelRoll = atan2(accel_x, accel_z) * RAD_TO_DEG;

  pitch = (tau)*(pitch + rotation_x*dt) + (1-tau)*(accelPitch);
  roll = (tau)*(roll - rotation_y*dt) + (1-tau)*(accelRoll);

  gyroPitch += rotation_x*dt;
  gyroRoll -= rotation_y*dt;
  gyroYaw += rotation_z*dt;

  // Visualize just the roll
  // Serial.print(roll); Serial.print(",");
  // Serial.print(gyroRoll); Serial.print(",");
  // Serial.println(accelRoll);

  // Visualize just the pitch
  // Serial.print(pitch); Serial.print(",");
  // Serial.print(gyroPitch); Serial.print(",");
  // Serial.println(accelPitch);

  // Data out serial monitor
   Serial.print(freq,0);   Serial.print(",");
   Serial.print(roll - set_point_offset,1);   Serial.print(", \t");
   Serial.print(" Kp = ");    Serial.print(Kp);
   Serial.print(" Kd = ");    Serial.println(Kd);     

  balance_robot();      //update the pwma and direction
  set_PD_constants();   //reading pots to calibrate PD values
  calibrate_set_point_offset(); //read pot to adjust centre point
  balance_LED();         //light up an LED if robot is balanced

  // Wait until the loopTimer reaches 4000us (250Hz) before next loop
  while (micros() - loopTimer <= 4000);
  loopTimer = micros();

}

void calibrate_set_point_offset(){
  set_point_offset = analogRead(set_point_offset_pot);
  set_point_offset = (float(set_point_offset / 1023) * 10) - 5 ; //the value goes from +- 5 degrees

}

void set_PD_constants(){
  Kp = analogRead(P_pot); //read Kp pot
  Kd = analogRead(D_pot); //read Kd pot
  Kp = (Kp / 1023) * 30;  //map the values
  Kd = (Kd / 1023) * 20;
}

void balance_LED(){
  if ((roll - set_point_offset) > -0.3 && (roll - set_point_offset) < 0.3){ //if almost balanced with 0.3 degrees of tollerance
    digitalWrite(BALANCE_LED ,1);
  }
  else{
    digitalWrite(BALANCE_LED ,0);
  }
}

void read_mpu_6050_data() {
  // Subroutine for reading the raw data
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  Wire.endTransmission();
  Wire.requestFrom(0x68, 14);

  // Read data --> Temperature falls between acc and gyro registers
  acc_x = Wire.read() << 8 | Wire.read();
  acc_y = Wire.read() << 8 | Wire.read();
  acc_z = Wire.read() << 8 | Wire.read();
  temperature = Wire.read() <<8 | Wire.read();
  gyro_x = Wire.read()<<8 | Wire.read();
  gyro_y = Wire.read()<<8 | Wire.read();
  gyro_z = Wire.read()<<8 | Wire.read();
}

void setup_mpu_6050_registers() {
  //Activate the MPU-6050
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();
  // Configure the accelerometer
  // Wire.write(0x__);
  // Wire.write; 2g --> 0x00, 4g --> 0x08, 8g --> 0x10, 16g --> 0x18
  Wire.beginTransmission(0x68);
  Wire.write(0x1C);
  Wire.write(0x08);
  Wire.endTransmission();
  // Configure the gyro
  // Wire.write(0x__);
  // 250 deg/s --> 0x00, 500 deg/s --> 0x08, 1000 deg/s --> 0x10, 2000 deg/s --> 0x18
  Wire.beginTransmission(0x68);
  Wire.write(0x1B);
  Wire.write(0x08);
  Wire.endTransmission();
}

void calibrate_sensor(){
  int readings_num = 1000;  //1000 samples
  // Calibration
  Serial.println("Calibrating sensor, place on level surface and do not move.");

  // Take  readings for each coordinate and then find average offset
  for (int cal_int = 0; cal_int < readings_num; cal_int ++){
    if(cal_int % 200 == 0)Serial.print(".");  //print . every 200 cycles
    read_mpu_6050_data();
    gyro_x_cal += gyro_x;
    gyro_y_cal += gyro_y;
    gyro_z_cal += gyro_z;

    acc_x_cal += acc_x;
    acc_y_cal += acc_y;
    acc_z_cal += acc_z;
    delay(3);
  }
  // Average the values
  gyro_x_cal /= readings_num;
  gyro_y_cal /= readings_num;
  gyro_z_cal /= readings_num;

  acc_x_cal /= readings_num;
  acc_y_cal /= readings_num;
  acc_z_cal /= readings_num;  
}


void balance_robot(){
  err = float(roll) - set_point_offset;   //error from setpoint
  errd = err - Ox;                        //error for Kd
  Ox = err;                               //preparing for next cycle 

  if (err > 0){   //if tilted fw/bkw
    fwd = true;
  }
  else{
    fwd = false;
  }
  err = abs(err); //dont want to feed a negative pwm value
  //Serial.println(err);  //debug line
  //Serial.println(set_point_offset);  //debug line
  //set motor speed
  pwm = err * Kp + errd * Kd;  //value with PD loop
  pwm = constrain(pwm, 0 , 255);  //make sure value fits input parameter

  analogWrite(M1_speed , pwm);  //set motor speed 
  analogWrite(M2_speed , pwm);  //set motor speed 

  if (fwd){   //if tilted fw/bkw
    digitalWrite(M1_d , HIGH);
    digitalWrite(M1_dr , LOW);
    digitalWrite(M2_d , HIGH);
    digitalWrite(M2_dr , LOW);
  }
  else{
    digitalWrite(M1_d , LOW);
    digitalWrite(M1_dr , HIGH);
    digitalWrite(M2_d , LOW);
    digitalWrite(M2_dr , HIGH);
  }
}