I need a help for Bluetooth control (HC05) using LmotorController library

Projects Programming
1

I almost finished the controller for my self balancing robot. The problem is, when I connect to my bot with my controller app, then press turn right/left, the bot interrrupt PID process and keep doing the command from button.It seems not break the loop until I press it one more time, the PID process works. Can someone indicate the problem with my RC codes? My aim is to interrupt the PID process for 100-150ms only. I tried millis() function and loop for but kinda not effectively.

#include "PID_v1.h"
#include "LMotorController.h"
#include "I2Cdev.h"
#include <SimpleKalmanFilter.h>
#include "MPU6050_6Axis_MotionApps20.h"
#include<SoftwareSerial.h>

//RC
SoftwareSerial mySerial(10,11); // RX, TX

String data;
int btVal;

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif
#define LOG_INPUT 0//Hiển thị trên serial plotter
#define MANUAL_TUNING 0//Hiệu chỉnh bằng tay
#define LOG_PID_CONSTANTS 0 //Ghi lại các giá trị hiệu chỉnh
#define MOVE_BACK_FORTH 0//Điều chỉnh setpoint để kéo robot theo hướng ngược lại. Chỉ số movingAngleOffset nên để nhỏ cỡ 0.1 - 0.2. Để lớn quá thì robot dễ mất ổn định
#define MIN_ABS_SPEED 5//Mức tốc độ tối thiểu

//MPU
MPU6050 mpu;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
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

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

//PID
#define original_setpoint_offsets -1.7//Mặc định với cấu hình hiện tại (-2.3 -2.2)
#if MANUAL_TUNING
  double kp , ki, kd;
  double prevKp, prevKi, prevKd;
#endif
double originalSetpoint = 0 + original_setpoint_offsets;
double setpoint = originalSetpoint;
double movingAngleOffset = 0.15;//bù vừa phải
double input, output;
int moveState=0; 

//Tham số PID: 19 160 1.7
#define Kp 20
#define Ki 190
#define Kd 1.7
#if MANUAL_TUNING
  PID pid(&input, &output, &setpoint, 0, 0, 0, DIRECT);
#else
  PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);
#endif

//MOTOR CONTROLLER

int ENA = 3;
int IN1 = 4;
int IN2 = 5;
int IN3 = 6;
int IN4 = 7;
int ENB = 9;

LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, 1, 1);

//timers

long time1Hz = 0;
long time5Hz = 0;
long timePID = 0;
long timeRC = 0;
volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady()
{
    mpuInterrupt = true;
}

void setup()
{ //RC
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  //pinMode(EN1, OUTPUT);
  //pinMode(EN2, OUTPUT);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
  //analogWrite(EN1,63);
  //analogWrite(EN2,63);
  mySerial.begin(9600);

  Serial.println(ypr[1] * 180/M_PI);
    // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    // initialize serial communication
    // (115200 chosen because it is required for Teapot Demo output, but it's
    // really up to you depending on your project)
    Serial.begin(115200);
    while (!Serial); // wait for Leonardo enumeration, others continue immediately

    // initialize device
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();

    // verify connection
    Serial.println(F("Testing device connections..."));
    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
    mpu.setXGyroOffset(39);
    mpu.setYGyroOffset(14);
    mpu.setZGyroOffset(6);
    mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

    // make sure it worked (returns 0 if so)
    if (devStatus == 0)
    {
        // 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();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
        
        //setup PID
        
        pid.SetMode(AUTOMATIC);
        pid.SetSampleTime(5);
        pid.SetOutputLimits(-255, 255);
    }
    else
    {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }
}

void loop()
{   //loop_RC();
    // if programming failed, don't try to do anything
    if (!dmpReady) return;
    unsigned long currentMillis = millis();
    // wait for MPU interrupt or extra packet(s) available
    
    while (!mpuInterrupt && fifoCount < packetSize)
    {   
        pid.Compute();
        motorController.move(output, MIN_ABS_SPEED);
        //no mpu data - performing PID calculations and output to motors
        
        if (currentMillis - time1Hz >= 1000)
        {
            loopAt1Hz();
            time1Hz = currentMillis;
        }
        
        if (currentMillis - time5Hz >= 800)
        {
            loopAt5Hz();
            time5Hz = currentMillis;
        }
        
    }
    loop_RC();
      
    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();

   
    if ((mpuIntStatus & 0x10) || fifoCount == 1024)
    {
        // reset so we can continue cleanly
        mpu.resetFIFO();
        Serial.println(F("FIFO overflow!"));

    
    }
    else if (mpuIntStatus & 0x02)
    {
        // wait for correct available data length, should be a VERY short wait
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        
      
        fifoCount -= packetSize;

        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
        #if LOG_INPUT
            Serial.print("Yaw:");
            Serial.println(ypr[0] * 180/M_PI);
            Serial.print("Pitch:");
            Serial.println(ypr[1] * 180/M_PI);
            Serial.print("Roll:");
            Serial.println(ypr[2] * 180/M_PI);
            Serial.print("setpoint:");
            Serial.println(setpoint);
            Serial.println();
        #endif
        input = ypr[1] * 180/M_PI;
   }
     
}
void loop_RC()
{
    if (mySerial.available() > 0)                              
    {
     //Serial.println("No Bluetooth Data ");    
     data = mySerial.read();      
    }
    btVal = (data.toInt());
    //Serial.print("BlueTooth Value ");
    //Serial.println(btVal);  
    RC();
}
void RC()
{ 
  unsigned long startTime = 0;
  unsigned long currentTime = millis();
  switch (btVal) 
   {
      //Leftside controllers
      case 'F':                                
        Serial.println("Forward");
        setpoint = originalSetpoint + 2;
        motorController.move(150, MIN_ABS_SPEED);
        break;
      case 'B':                 
       Serial.println("Reverse");
        setpoint = originalSetpoint - 2;
        motorController.move(-150, -1*MIN_ABS_SPEED);
        break;

      case 'L': // turn left
        startTime = currentTime;
        for (int i = 0; i < 150; i += 10) { 
        if (currentTime - startTime < 150) { // time check
          motorController.turnLeft(100, true); // control motor
        } else 
        {
          break; // get rid of the loop
        }
        delay(10); // delay for 10ms
        currentTime = millis(); // update to realtime
        }
        break;

      case 'R':                     
        startTime = currentTime;
        for (int i = 0; i < 150; i += 10) {
        if (currentTime - startTime < 150) { // time check. When time is plus 150ms, end the loop
          motorController.turnRight(100, true); // motor control
        } else 
        {
          break; // get rid of the loop
        }
        delay(10); // delay for 10ms
        currentTime = millis(); // update real time
        }
        break;

        // controllers
      case 'T':                     
        Serial.println("boost");
        setpoint = originalSetpoint + 3;
        motorController.move(150, MIN_ABS_SPEED);
        break;  
      /*case 'C':                     
        Serial.println("spinright");
        break;  
      case 'S':                     
        Serial.println("spinleft");
        break;  */
      case 'X':                     
        Serial.println("brake");//brake
        setpoint = originalSetpoint;
        motorController.stopMoving();
        break;
  }

                                               
   if (mySerial.available() < 0)                              
    {
     //Serial.println("No Bluetooth Data ");          
    }
}

void loopAt1Hz()
{
#if MANUAL_TUNING
    setPIDTuningValues();
#endif
}

void loopAt5Hz()
{
    #if MOVE_BACK_FORTH
        moveBackForth();
    #endif
}

//move back and forth


void moveBackForth()
{
    moveState++;
    if (moveState > 2) moveState = 0;
    
    if (moveState == 0)
      setpoint = originalSetpoint;
    else if (moveState == 1)
      setpoint = originalSetpoint - movingAngleOffset;
    else
      setpoint = originalSetpoint + movingAngleOffset;
}

//PID Tuning (3 potentiometers)

#if MANUAL_TUNING
void setPIDTuningValues()
{
    readPIDTuningValues();
    
    if (kp != prevKp || ki != prevKi || kd != prevKd)
    {
#if LOG_PID_CONSTANTS
        Serial.print(kp);Serial.print(", ");Serial.print(ki);Serial.print(", ");Serial.println(kd);
#endif

        pid.SetTunings(kp, ki, kd);
        prevKp = kp; prevKi = ki; prevKd = kd;
    }
}

void readPIDTuningValues()
{
    int potKp = analogRead(A0);
    int potKi = analogRead(A1);
    int potKd = analogRead(A2);
        
    kp = map(potKp, 0, 1023, 0, 25000) / 100.0; //0 - 250
    ki = map(potKi, 0, 1023, 0, 100000) / 100.0; //0 - 1000
    kd = map(potKd, 0, 1023, 0, 500) / 100.0; //0 - 5
}
#endif

Here's the Lmotorcontroller library:

#include "LMotorController.h"
#include "Arduino.h"


LMotorController::LMotorController(int ena, int in1, int in2, int enb, int in3, int in4, double motorAConst, double motorBConst)
{
    _motorAConst = motorAConst;
    _motorBConst = motorBConst;
    
	_ena = ena;
	_in1 = in1;
	_in2 = in2;
	_enb = enb;
	_in3 = in3;
	_in4 = in4;
	
	pinMode(_ena, OUTPUT);
	pinMode(_in1, OUTPUT);
	pinMode(_in2, OUTPUT);
    
	pinMode(_enb, OUTPUT);
	pinMode(_in3, OUTPUT);
	pinMode(_in4, OUTPUT);
}


void LMotorController::move(int leftSpeed, int rightSpeed, int minAbsSpeed)
{
    if (rightSpeed < 0)
    {
        rightSpeed = min(rightSpeed, -1*minAbsSpeed);
        rightSpeed = max(rightSpeed, -200);
    }
    else if (rightSpeed > 0)
    {
        rightSpeed = max(rightSpeed, minAbsSpeed);
        rightSpeed = min(rightSpeed, 200);
    }
    
    int realRightSpeed = map(abs(rightSpeed), 0, 255, minAbsSpeed, 255);

    if (leftSpeed < 0)
    {
        leftSpeed = min(leftSpeed, -1*minAbsSpeed);
        leftSpeed = max(leftSpeed, -200);
    }
    else if (leftSpeed > 0)
    {
        leftSpeed = max(leftSpeed, minAbsSpeed);
        leftSpeed = min(leftSpeed, 200);
    }
    
    int realLeftSpeed = map(abs(leftSpeed), 0, 255, minAbsSpeed, 255);
    
    digitalWrite(_in3, rightSpeed > 0 ? HIGH : LOW);
    digitalWrite(_in4, rightSpeed > 0 ? LOW : HIGH);
    digitalWrite(_in1, leftSpeed > 0 ? HIGH : LOW);
    digitalWrite(_in2, leftSpeed > 0 ? LOW : HIGH);
    analogWrite(_ena, realRightSpeed * _motorAConst);
    analogWrite(_enb, realLeftSpeed * _motorBConst);
}


void LMotorController::move(int speed, int minAbsSpeed)
{
    int direction = 1;
    
    if (speed < 0)
    {
        direction = -1;
        
        speed = min(speed, -1*minAbsSpeed);
        speed = max(speed, -200);
    }
    else
    {
        speed = max(speed, minAbsSpeed);
        speed = min(speed, 200);
    }
    
    if (speed == _currentSpeed) return;
    
    int realSpeed = max(minAbsSpeed, abs(speed));
    
    digitalWrite(_in1, speed > 0 ? HIGH : LOW);
    digitalWrite(_in2, speed > 0 ? LOW : HIGH);
    digitalWrite(_in3, speed > 0 ? HIGH : LOW);
    digitalWrite(_in4, speed > 0 ? LOW : HIGH);
    analogWrite(_ena, realSpeed * _motorAConst);
    analogWrite(_enb, realSpeed * _motorBConst);
    
    _currentSpeed = direction * realSpeed;
}


void LMotorController::move(int speed)
{
    if (speed == _currentSpeed) return;
    
    if (speed > 200) speed = 200;
    else if (speed < -200) speed = -200;
    
    digitalWrite(_in1, speed > 0 ? HIGH : LOW);
    digitalWrite(_in2, speed > 0 ? LOW : HIGH);
    digitalWrite(_in3, speed > 0 ? HIGH : LOW);
    digitalWrite(_in4, speed > 0 ? LOW : HIGH);
    analogWrite(_ena, abs(speed) * _motorAConst);
    analogWrite(_enb, abs(speed) * _motorBConst);
    
    _currentSpeed = speed;
}


void LMotorController::turnLeft(int speed, bool kick)
{
    digitalWrite(_in1, HIGH);
    digitalWrite(_in2, LOW);
    digitalWrite(_in3, LOW);
    digitalWrite(_in4, HIGH);
    
    if (kick)
    {
        analogWrite(_ena, 100);
        analogWrite(_enb, 100);
    
        delay(50);
    }
    
    analogWrite(_ena, speed * _motorAConst);
    analogWrite(_enb, speed * _motorBConst);
}


void LMotorController::turnRight(int speed, bool kick)
{
    digitalWrite(_in1, LOW);
    digitalWrite(_in2, HIGH);
    digitalWrite(_in3, HIGH);
    digitalWrite(_in4, LOW);
 
    if (kick)
    {
        analogWrite(_ena, 100);
        analogWrite(_enb, 100);
    
        delay(50);
    }
    
    analogWrite(_ena, speed * _motorAConst);
    analogWrite(_enb, speed * _motorBConst);
}


void LMotorController::stopMoving()
{
    digitalWrite(_in1, LOW);
    digitalWrite(_in2, LOW);
    digitalWrite(_in3, LOW);
    digitalWrite(_in4, LOW);
    digitalWrite(_ena, HIGH);
    digitalWrite(_enb, HIGH);
    
    _currentSpeed = 0;
}
2

Add serial printing to your code to make visible which lines of code are executed and which not.

Here is your code with additional debug-macros

// MACRO-START * MACRO-START * MACRO-START * MACRO-START * MACRO-START * MACRO-START *
// a detailed explanation how these macros work is given in this tutorial
// https://forum.arduino.cc/t/comfortable-serial-debug-output-short-to-write-fixed-text-name-and-content-of-any-variable-code-example/888298

#define dbg(myFixedText, variableName) \
  Serial.print( F(#myFixedText " "  #variableName"=") ); \
  Serial.println(variableName);

#define dbgi(myFixedText, variableName,timeInterval) \
  { \
    static unsigned long intervalStartTime; \
    if ( millis() - intervalStartTime >= timeInterval ){ \
      intervalStartTime = millis(); \
      Serial.print( F(#myFixedText " "  #variableName"=") ); \
      Serial.println(variableName); \
    } \
  }

#define dbgc(myFixedText, variableName) \
  { \
    static long lastState; \
    if ( lastState != variableName ){ \
      Serial.print( F(#myFixedText " "  #variableName" changed from ") ); \
      Serial.print(lastState); \
      Serial.print( F(" to ") ); \
      Serial.println(variableName); \
      lastState = variableName; \
    } \
  }

#define dbgcf(myFixedText, variableName) \
  { \
    static float lastState; \
    if ( lastState != variableName ){ \
      Serial.print( F(#myFixedText " "  #variableName" changed from ") ); \
      Serial.print(lastState); \
      Serial.print( F(" to ") ); \
      Serial.println(variableName); \
      lastState = variableName; \
    } \
  }
// MACRO-END * MACRO-END * MACRO-END * MACRO-END * MACRO-END * MACRO-END * MACRO-END *

unsigned long MyTestTimer = 0;                   // Timer-variables MUST be of type unsigned long
const byte    OnBoard_LED = 2;     // onboard-LEDESP32 / ESP8266
//const byte    OnBoard_LED = 25;  // onboard-LED Raspberry Pi pico
//const byte    OnBoard_LED = 13;   // onboard-LED uno, mega

/*
  BlinkHeartBeatLED(OnBoard_LED,250);

  if ( TimePeriodIsOver(MyTestTimer,1000) ) {

  }

*/

#include "PID_v1.h"
#include "LMotorController.h"
#include "I2Cdev.h"
#include <SimpleKalmanFilter.h>
#include "MPU6050_6Axis_MotionApps20.h"
#include<SoftwareSerial.h>

//RC
SoftwareSerial mySerial(10, 11); // RX, TX

String data;
int btVal;

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#define LOG_INPUT 0//Hiển thị trên serial plotter
#define MANUAL_TUNING 0//Hiệu chỉnh bằng tay
#define LOG_PID_CONSTANTS 0 //Ghi lại các giá trị hiệu chỉnh
#define MOVE_BACK_FORTH 0//Điều chỉnh setpoint để kéo robot theo hướng ngược lại. Chỉ số movingAngleOffset nên để nhỏ cỡ 0.1 - 0.2. Để lớn quá thì robot dễ mất ổn định
#define MIN_ABS_SPEED 5//Mức tốc độ tối thiểu

//MPU
MPU6050 mpu;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
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

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

//PID
#define original_setpoint_offsets -1.7//Mặc định với cấu hình hiện tại (-2.3 -2.2)
#if MANUAL_TUNING
double kp , ki, kd;
double prevKp, prevKi, prevKd;
#endif
double originalSetpoint = 0 + original_setpoint_offsets;
double setpoint = originalSetpoint;
double movingAngleOffset = 0.15;//bù vừa phải
double input, output;
int moveState = 0;

//Tham số PID: 19 160 1.7
#define Kp 20
#define Ki 190
#define Kd 1.7
#if MANUAL_TUNING
PID pid(&input, &output, &setpoint, 0, 0, 0, DIRECT);
#else
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);
#endif

//MOTOR CONTROLLER

int ENA = 3;
int IN1 = 4;
int IN2 = 5;
int IN3 = 6;
int IN4 = 7;
int ENB = 9;

LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, 1, 1);

//timers

long time1Hz = 0;
long time5Hz = 0;
long timePID = 0;
long timeRC = 0;
volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady()
{
  mpuInterrupt = true;
}

void setup()
{ //RC
  // initialize serial communication
  // (115200 chosen because it is required for Teapot Demo output, but it's
  // really up to you depending on your project)
  Serial.begin(115200);
  while (!Serial); // wait for Leonardo enumeration, others continue immediately
  Serial.println("Setup-Start");
  PrintFileNameDateTime();

  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  //pinMode(EN1, OUTPUT);
  //pinMode(EN2, OUTPUT);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
  //analogWrite(EN1,63);
  //analogWrite(EN2,63);
  mySerial.begin(9600);

  Serial.println(ypr[1] * 180 / M_PI);
  // join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif


  // initialize device
  Serial.println(F("Initializing I2C devices..."));
  mpu.initialize();

  // verify connection
  Serial.println(F("Testing device connections..."));
  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
  mpu.setXGyroOffset(39);
  mpu.setYGyroOffset(14);
  mpu.setZGyroOffset(6);
  mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

  // make sure it worked (returns 0 if so)
  if (devStatus == 0)
  {
    // 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();

    // set our DMP Ready flag so the main loop() function knows it's okay to use it
    Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;

    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();

    //setup PID

    pid.SetMode(AUTOMATIC);
    pid.SetSampleTime(5);
    pid.SetOutputLimits(-255, 255);
  }
  else
  {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  }
}

void loop()
{ 
  // only in case variable btVal has CHANGED its value print a SINGLE time
  dbgc("ToL",btVal);
  //loop_RC();
  // if programming failed, don't try to do anything
  if (!dmpReady) return;
  unsigned long currentMillis = millis();
  // wait for MPU interrupt or extra packet(s) available

  while (!mpuInterrupt && fifoCount < packetSize)
  {
    pid.Compute();
    motorController.move(output, MIN_ABS_SPEED);
    //no mpu data - performing PID calculations and output to motors

    if (currentMillis - time1Hz >= 1000)
    {
      loopAt1Hz();
      time1Hz = currentMillis;
    }

    if (currentMillis - time5Hz >= 800)
    {
      loopAt5Hz();
      time5Hz = currentMillis;
    }

  }
  loop_RC();

  // reset interrupt flag and get INT_STATUS byte
  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();

  // get current FIFO count
  fifoCount = mpu.getFIFOCount();


  if ((mpuIntStatus & 0x10) || fifoCount == 1024)
  {
    // reset so we can continue cleanly
    mpu.resetFIFO();
    Serial.println(F("FIFO overflow!"));


  }
  else if (mpuIntStatus & 0x02)
  {
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

    // read a packet from FIFO
    mpu.getFIFOBytes(fifoBuffer, packetSize);


    fifoCount -= packetSize;

    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
#if LOG_INPUT
    Serial.print("Yaw:");
    Serial.println(ypr[0] * 180 / M_PI);
    Serial.print("Pitch:");
    Serial.println(ypr[1] * 180 / M_PI);
    Serial.print("Roll:");
    Serial.println(ypr[2] * 180 / M_PI);
    Serial.print("setpoint:");
    Serial.println(setpoint);
    Serial.println();
#endif
    input = ypr[1] * 180 / M_PI;
  }

}
void loop_RC()
{
  if (mySerial.available() > 0)
  {
    //Serial.println("No Bluetooth Data ");
    data = mySerial.read();
  }
  btVal = (data.toInt());
  //Serial.print("BlueTooth Value ");
  //Serial.println(btVal);
  RC();
}
void RC()
{
  unsigned long startTime = 0;
  unsigned long currentTime = millis();
  switch (btVal)
  {
    //Leftside controllers
    case 'F':
      Serial.println("Forward");
      setpoint = originalSetpoint + 2;
      motorController.move(150, MIN_ABS_SPEED);
      break;
    case 'B':
      Serial.println("Reverse");
      setpoint = originalSetpoint - 2;
      motorController.move(-150, -1 * MIN_ABS_SPEED);
      break;

    case 'L': // turn left
      startTime = currentTime;
      for (int i = 0; i < 150; i += 10) {
        if (currentTime - startTime < 150) { // time check
          motorController.turnLeft(100, true); // control motor
        } else
        {
          break; // get rid of the loop
        }
        delay(10); // delay for 10ms
        currentTime = millis(); // update to realtime
      }
      break;

    case 'R':
      startTime = currentTime;
      for (int i = 0; i < 150; i += 10) {
        if (currentTime - startTime < 150) { // time check. When time is plus 150ms, end the loop
          motorController.turnRight(100, true); // motor control
        } else
        {
          break; // get rid of the loop
        }
        delay(10); // delay for 10ms
        currentTime = millis(); // update real time
      }
      break;

    // controllers
    case 'T':
      Serial.println("boost");
      setpoint = originalSetpoint + 3;
      motorController.move(150, MIN_ABS_SPEED);
      break;
    /*case 'C':
      Serial.println("spinright");
      break;
      case 'S':
      Serial.println("spinleft");
      break;  */
    case 'X':
      Serial.println("brake");//brake
      setpoint = originalSetpoint;
      motorController.stopMoving();
      break;
  }


  if (mySerial.available() < 0)
  {
    //Serial.println("No Bluetooth Data ");
  }
}

void loopAt1Hz()
{
#if MANUAL_TUNING
  setPIDTuningValues();
#endif
}

void loopAt5Hz()
{
#if MOVE_BACK_FORTH
  moveBackForth();
#endif
}

//move back and forth


void moveBackForth()
{
  moveState++;
  if (moveState > 2) moveState = 0;

  if (moveState == 0)
    setpoint = originalSetpoint;
  else if (moveState == 1)
    setpoint = originalSetpoint - movingAngleOffset;
  else
    setpoint = originalSetpoint + movingAngleOffset;
}

//PID Tuning (3 potentiometers)

#if MANUAL_TUNING
void setPIDTuningValues()
{
  readPIDTuningValues();

  if (kp != prevKp || ki != prevKi || kd != prevKd)
  {
#if LOG_PID_CONSTANTS
    Serial.print(kp); Serial.print(", "); Serial.print(ki); Serial.print(", "); Serial.println(kd);
#endif

    pid.SetTunings(kp, ki, kd);
    prevKp = kp; prevKi = ki; prevKd = kd;
  }
}

void readPIDTuningValues()
{
  int potKp = analogRead(A0);
  int potKi = analogRead(A1);
  int potKd = analogRead(A2);

  kp = map(potKp, 0, 1023, 0, 25000) / 100.0; //0 - 250
  ki = map(potKi, 0, 1023, 0, 100000) / 100.0; //0 - 1000
  kd = map(potKd, 0, 1023, 0, 500) / 100.0; //0 - 5
}
#endif

// helper-functions
void PrintFileNameDateTime() {
  Serial.println( F("Code running comes from file ") );
  Serial.println( F(__FILE__) );
  Serial.print( F("  compiled ") );
  Serial.print( F(__DATE__) );
  Serial.print( F(" ") );
  Serial.println( F(__TIME__) );
}


// easy to use helper-function for non-blocking timing
// explanation see here
// https://forum.arduino.cc/t/example-code-for-timing-based-on-millis-easier-to-understand-through-the-use-of-example-numbers-avoiding-delay/974017
boolean TimePeriodIsOver (unsigned long &startOfPeriod, unsigned long TimePeriod) {
  unsigned long currentMillis  = millis();
  if ( currentMillis - startOfPeriod >= TimePeriod ) {
    // more time than TimePeriod has elapsed since last time if-condition was true
    startOfPeriod = currentMillis; // a new period starts right here so set new starttime
    return true;
  }
  else return false;            // actual TimePeriod is NOT yet over
}



void BlinkHeartBeatLED(int IO_Pin, int BlinkPeriod) {
  static unsigned long MyBlinkTimer;
  pinMode(IO_Pin, OUTPUT);

  if ( TimePeriodIsOver(MyBlinkTimer, BlinkPeriod) ) {
    digitalWrite(IO_Pin, !digitalRead(IO_Pin) );
  }
}
1 Like
3

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.