update: I am using the following setup to make changes easily and check , arduino , nrf24L01 , mpu6050, l298n motor driver, 25ga motor 200rpm 12v , jumper wires
so now the pull up resisitor on mpu 6050 module is 2.2k to 3.3v, it still freezes(it takes more time to freeze than in pcb but it does) , could you please suggest what should be my next step , reducing clock speed makes the mpu6050 not functional
#include <Wire.h>
/********** PID **********/
#include <PID_v1.h>
#include <Servo.h>
#define INTERRUPT_PIN_2
double kP = 17.388;
double kI = 35;
double kD = 1;
double setpoint, input, output; // PID variables
PID pid(&input, &output, &setpoint, kP, kI, kD, DIRECT); // PID setup
/********** radio **********/
#include <nRF24L01.h>
#include <SPI.h>
#include <RF24.h>
#include <nRF24L01.h>
RF24 radio(9, 10); // CE, CSN
const byte address[6] = "00001";
float angleV = 0, turnV = 0;int servo = 0; // values from remote
float controlValues[3]; // array to receive both data values
/********** L298N **********/
#include <L298N.h>
// Pin definition
const unsigned int EN_A = 6;
const unsigned int IN1_A = 4;
const unsigned int IN2_A = A2;
const unsigned int IN1_B = 7;
const unsigned int IN2_B = 8;
const unsigned int EN_B = 3;
// Create motor instances
L298N rightMotor(EN_A, IN1_A, IN2_A);
L298N leftMotor(EN_B, IN1_B, IN2_B);
// motor speeds
int speedLeft = 0;
int speedRight = 0;
/********** MPU **********/
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
//AD0 high = 0x69;
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;
// 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
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
float pitch;
long velocity;
float trimPot;
float trimAngle;
int IMUdataReady = 0;
volatile bool mpuInterrupt = false;
int servoPin = 5;
Servo Servo1;
// IMU interrupt service routine
void dmpDataReady() {
IMUdataReady = 1;
}
// function that actually read the angle when the flag is set by the ISR
void readAngles() {
mpuIntStatus = mpu.getIntStatus();
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);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
IMUdataReady = 0;
//count = count + 1;
}
}
/********** SETUP **********/
void setup() {
//
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
Wire.setWireTimeout(3000, true); //timeout value in uSec
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
Serial.begin(115200);
// initialize device
mpu.initialize();
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(54);
mpu.setYGyroOffset(-45);
mpu.setZGyroOffset(10);
mpu.setZAccelOffset(681); // 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
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} 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(")"));
}
pid.SetMode(AUTOMATIC);
pid.SetOutputLimits(-255,255);
pid.SetSampleTime(10);
pinMode(LED_PIN, OUTPUT);
Servo1.attach(servoPin);
radio.begin();
radio.openReadingPipe(0, address);
radio.setPALevel(RF24_PA_MIN);
radio.startListening();
}
void loop() {
// only read remote data if remote is available, otherwise balance normally
if (radio.available()) {
radio.read(&controlValues, sizeof(controlValues)); // read values of array
angleV = controlValues[0]; // assign array values to control variables
turnV = controlValues[1];
servo = controlValues[2];
}
if(servo == 1)
{
Servo1.write(180);
}
else
{
Servo1.write(0);
}
trimPot = analogRead(A3)*-1; // read dial on robot and *-1 for direction
trimAngle = (trimPot/100) + 5 + angleV; // adjust to degrees
if (IMUdataReady == 1) {
readAngles();
}
pitch = (ypr[1] * 180/M_PI); // adjust to degrees
if (abs(turnV) < 15) { // turnV threshold
turnV = 0;
}
if (abs(angleV) < .17) { // angleV threshold
angleV = 0;
}
// PID vars
setpoint = trimAngle + angleV;
input = pitch;
pid.Compute();
// set motor speed with adjusted turn values
speedLeft = output - turnV;
speedRight = output + turnV;
if (pitch > 25 || pitch < -25) { // angle threshold
leftMotor.setSpeed(0);
rightMotor.setSpeed(0);
} else {
leftMotor.setSpeed(abs(speedLeft));
rightMotor.setSpeed(abs(speedRight));
}
// move motors
if (speedLeft < 0) {
leftMotor.forward();
} else {
leftMotor.backward();
}
if (speedRight < 0) {
rightMotor.forward();
} else {
rightMotor.backward();
}
// print some control info
Serial.print("pitch: ");
Serial.print(pitch);
Serial.print(" , trimPot: ");
Serial.print(trimPot);
Serial.print(" , angleV: ");
Serial.print(angleV);
Serial.print(" , turnV: ");
Serial.print(turnV);
Serial.print("\n");
}
This is the complete code, i have tried replacing the 4.7k resisotor to 2.2 in my pcb still it freezed, also after removing the mpu6050 ic rom pcb tried connecting the mpu module through jumper wire it still freezed. Could you please suggest my next step to resolve this
