friends,
I am doing a project to control a car (simply) consisting of 4 dc motors by the gestures of hand. The hand orientation is detected by MPU6050 sensor and is processed by arduino nano. I have connected the output to dc motor data pins at D7 through D10 pins of the arduino nano. I used digitalWrite to write high/low values at the pins for motor rotation. For simplicity, i have connected 4 LEDs to the data out pins (D7-D10) instead of motors in common ground fashion. But, when i change the orientation of the sensor, the Leds glow randomly or even doesn't glow. Is this due to the digital pins of nano? What should i do to make the motors work effectievely? Please suggest methods at the earliest. Deadline for my project is closing.
CRIS
My program (main parts):
// ================================================================INITIAL SETUP
void setup() {
// 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
Serial.begin(19200);
//Serial.println(F("Initializing I2C devices..."));
mpuaccel.initialize();
mpugyro.initialize();
//Serial.println(F("Testing device connections..."));
Serial.println(mpuaccel.testConnection() ? F("MPUaccel connection successful") : F("MPUaccel connection failed"));
Serial.println(mpugyro.testConnection() ? F("MPUgyro connection successful") : F("MPUgyro connection failed"));
// load and configure the DMP
devStatus1 = mpuaccel.dmpInitialize();
devStatus2 = mpugyro.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpuaccel.setXGyroOffset(220);
mpuaccel.setYGyroOffset(76);
mpuaccel.setZGyroOffset(-85);
mpuaccel.setZAccelOffset(1788); // 1688 factory default for my test chip
mpugyro.setXGyroOffset(220);
mpugyro.setYGyroOffset(76);
mpugyro.setZGyroOffset(-85);
mpugyro.setZAccelOffset(1788);
// make sure it worked (returns 0 if so)
if (devStatus1 == 0 ) {
// turn on the DMP, now that it's ready
//Serial.println(F("Enabling DMP..."));
mpuaccel.setDMPEnabled(true);
//Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
attachInterrupt(0, dmpDataReady_accel, RISING);
mpuIntStatus1 = mpuaccel.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..."));
dmpReady1 = true;
// get expected DMP packet size for later comparison
packetSize1 = mpuaccel.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("DMPaccel Initialization failed (code "));
Serial.print(devStatus1);
Serial.println(F(")"));
}
if (devStatus2 == 0)
{
mpugyro.setDMPEnabled(true);
attachInterrupt(1, dmpDataReady_gyro, RISING);
mpuIntStatus2 = mpugyro.getIntStatus();
dmpReady2 = true;
packetSize2 = mpugyro.dmpGetFIFOPacketSize();
}
else
{
Serial.print(F("DMPgyro Initialization failed (code "));
Serial.print(devStatus2);
Serial.println(F(")"));
}
s1.attach(5);
s2.attach(6);
pinMode(7,'OUTPUT'); //MOTORS
pinMode(8,'OUTPUT');
pinMode(9,'OUTPUT');
pinMode(10,'OUTPUT');
digitalWrite(7,'LOW');
digitalWrite(8,'LOW');
digitalWrite(9,'LOW');
digitalWrite(10,'LOW');
pinMode(11,'OUTPUT');
digitalWrite(11,'LOW');
pinMode(12,'OUTPUT');
digitalWrite(12,'HIGH');
pinMode(4,'OUTPUT'); //LED
}
// ================================================================MAIN PROGRAM LOOP
void loop() {
// if programming failed, don't try to do anything
if (!dmpReady1)
{
Serial.println("dmp 1 not ready ");
return;
}
if (!dmpReady2)
{
Serial.println("dmp 2 not ready ");
return;
}
// wait for MPU interrupt or extra packet(s) available
while ((!mpuInterrupt0 && fifoCount1 < packetSize1) || (!mpuInterrupt1 && fifoCount2 < packetSize2))// other program behavior stuff here
{
}
// reset interrupt flag and get INT_STATUS byte
mpuInterrupt0 = false;
mpuIntStatus1 = mpuaccel.getIntStatus();
mpuInterrupt1 = false;
mpuIntStatus2 = mpugyro.getIntStatus();
// get current FIFO count
fifoCount1 = mpuaccel.getFIFOCount();
fifoCount2 = mpugyro.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus1 & 0x10) || (fifoCount1 == 1024))
{
// reset so we can continue cleanly
mpuaccel.resetFIFO();
delay(50);
//Serial.println(F("FIFO overflow!"));
}
else if (mpuIntStatus1 & 0x02)
{
// wait for correct available data length, should be a VERY short wait
while (fifoCount1 < packetSize1)
{
fifoCount1 = mpuaccel.getFIFOCount();
}
}
if ((mpuIntStatus2 & 0x10) || (fifoCount2 == 1024))
{
mpugyro.resetFIFO();
delay(50);
}
else if (mpuIntStatus2 & 0x02)
{
while (fifoCount2 < packetSize2)
{
fifoCount2 = mpugyro.getFIFOCount();
}
}
// read a packet from FIFO
mpuaccel.getFIFOBytes(fifoBuffer1, packetSize1);
mpugyro.getFIFOBytes(fifoBuffer2, packetSize2);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount1 -= packetSize1;
fifoCount2 -= packetSize2;
//OUTPUT_READABLE_YAWPITCHROLL
// display Euler angles in degrees
mpuaccel.dmpGetQuaternion(&q1, fifoBuffer1);
mpuaccel.dmpGetGravity(&gravity1, &q1);
mpuaccel.dmpGetYawPitchRoll(ypr1, &q1, &gravity1);
yprr1[0] = ypr1[0] * 180/M_PI;
yprr1[1] = ypr1[1] * 180/M_PI;
yprr1[2] = ypr1[2] * 180/M_PI;
cons1[0] = constrain(yprr1[0],-60,60);
cons1[1] = constrain(yprr1[1],-60,60);
cons1[2] = constrain(yprr1[2],-60,60);
mapv1[0] = map(cons1[0],-60,60,30,150);
mapv1[1] = map(cons1[1],-60,60,30,150);
mapv1[2] = map(cons1[2],-60,60,30,150);
Serial.print("\n");
//Serial.print(mapv1[0]);
//Serial.print("\t");
Serial.print(mapv1[1]);
Serial.print("\t");
Serial.print(mapv1[2]);
Serial.print("\t");
Serial.print("\t");
mpugyro.dmpGetQuaternion(&q2, fifoBuffer2);
mpugyro.dmpGetGravity(&gravity2, &q2);
mpugyro.dmpGetYawPitchRoll(ypr2, &q2, &gravity2);
yprr2[0] = ypr2[0] * 180/M_PI;
yprr2[1] = ypr2[1] * 180/M_PI;
yprr2[2] = ypr2[2] * 180/M_PI;
cons2[0] = constrain(yprr2[0],-60,60);
cons2[1] = constrain(yprr2[1],-60,60);
cons2[2] = constrain(yprr2[2],-60,60);
mapv2[0] = map(cons2[0],-60,60,30,150);
mapv2[1] = map(cons2[1],-60,60,30,150);
mapv2[2] = map(cons2[2],-60,60,30,150);
Serial.print(mapv2[0]);
Serial.print("\t");
Serial.print(mapv2[1]);
Serial.print("\t");
//Serial.print(mapv2[2]);
Serial.print("\t");
delay(5);
s1.write(mapv2[0]);
s2.write(mapv2[1]);
if(mapv1[1]>85 && mapv1[1]<95 || mapv1[2]>85 && mapv1[2]<95 ) //motor steady
{
digitalWrite(7,'LOW');
digitalWrite(8,'LOW');
digitalWrite(9,'LOW');
digitalWrite(10,'LOW');
}
else if(mapv1[1]>30 && mapv1[1]<85) //forward
{
digitalWrite(7,'HIGH');
digitalWrite(8,'LOW');
digitalWrite(9,'HIGH');
digitalWrite(10,'LOW');
}
else if(mapv1[1]>95 && mapv1[1]<150) //backward
{
digitalWrite(7,'LOW');
digitalWrite(8,'HIGH');
digitalWrite(9,'LOW');
digitalWrite(10,'HIGH');
}
else if(mapv1[2]>95 && mapv1[2]<150) //right
{
digitalWrite(7,'HIGH');
digitalWrite(8,'LOW');
digitalWrite(9,'LOW');
digitalWrite(10,'HIGH');
}
else if(mapv1[2]>30 && mapv1[2]<85) //left
{
digitalWrite(7,'LOW');
digitalWrite(8,'HIGH');
digitalWrite(9,'HIGH');
digitalWrite(10,'LOW');
}
delay(1000);
}