I'm working on a small 2 wheeled robot that has 3 obstacle sensors and drop sensors. Sensors are the FC-51 adjustable model. (sensor link)
The robot only drives when the sensors show an input. If i dial back the potentiometer, the robot stutters and does not work.
Using the adafruit pro trinket 3v
drv8833 motor driver
I think there is a value that is backwards, maybe a high and low switched. The sensors are plugged into A0-A5. Im hoping someone with better coding experience can help identify the issue at hand.
The sensor portion of the code is below. The rest is just motor movement options:
[tr][td]// === INTERRUPT DETECTION ROUTINE ===
// ================================================================
//volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
//void dmpDataReady() {
// mpuInterrupt = true;
//}
void setup() {
Serial.begin(9600);
pinMode(frontSensorPin,INPUT_PULLUP);
pinMode(rightSensorPin,INPUT_PULLUP);
pinMode(leftSensorPin,INPUT_PULLUP);
pinMode(frontSensorPinDown,INPUT_PULLUP);
pinMode(rightSensorPinDown,INPUT_PULLUP);
pinMode(leftSensorPinDown,INPUT_PULLUP);
pinMode(motor00, OUTPUT);
pinMode(motor01, OUTPUT);
pinMode(motor10, OUTPUT);
pinMode(motor11, OUTPUT);
//motor1.run(RELEASE);
//motor2.run(RELEASE);
if(digitalRead(A0))Serial.println("frontSensorPin");
if(digitalRead(A1))Serial.println("rightSensorPin");
if(digitalRead(A2))Serial.println("leftSensorPin");
if(digitalRead(A3))Serial.println("frontSensorPinDown");
if(digitalRead(A4))Serial.println("rightSensorPinDown");
if(digitalRead(A5))Serial.println("leftSensorPinDown");
randomSeed(analogRead(0));
// 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). Comment this line if having compilation difficulties with TWBR.
#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(38400);
while (!Serial); // wait for Leonardo enumeration, others continue immediately
// NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
// Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
// the baud timing being too misaligned with processor ticks. You must use
// 38400 or slower in these cases, or use some kind of external separate
// crystal solution for the UART timer.
// 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"));
// wait for ready
// 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(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1288); // 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();
} 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(")"));
}
*/
// configure LED for output
pinMode(LED_PIN, OUTPUT);
}
long aRandomNumber(){
return random(2000,8000);
}
void doDMP(){
/*#ifdef ACCELEROMETER
if (!dmpReady) return;
//Serial.println("YAY2");
// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {
Serial.println("HOLD");
// other program behavior stuff here
// .
// .
// .
// if you are really paranoid you can frequently test in between other
// stuff to see if mpuInterrupt is true, and if so, "break;" from the
// while() loop to immediately process the MPU data
// .
// .
// .
}
// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & 0x02) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize){
//Serial.print(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;
// display real acceleration, adjusted to remove gravity
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
Serial.print("areal\t");
Serial.print(aaReal.x);
Serial.print("\t");
Serial.print(aaReal.y);
Serial.print("\t");
Serial.println(aaReal.z);
// blink LED to indicate activity
//blinkState = !blinkState;
//digitalWrite(LED_PIN, blinkState);
}
#endif
*/
#ifdef IRSENSORS
if(digitalRead(frontSensorPinDown)){
moveBackward(responseTime, true );
//Serial.println("frontSensorPinDown");
}
if(digitalRead(leftSensorPinDown)){
pivotRightF(responseTime, true );
//Serial.println("leftSensorPinDown");
}
if(digitalRead(rightSensorPinDown)){
pivotLeftF(responseTime, true );
//Serial.println("rightSensorPinDown");
}
//put sensor movement code here
#endif
}
#define MOTORS_ON
void loop() {
doDMP();
//motorsStop(3000);
doDMP();
//wiggleForward(aRandomNumber(),200);
doDMP();
//motorsStop(1000);
doDMP();
#ifdef MOTORS_ON
//wiggleBackward(1000,200);
doDMP();
motorsStop(1000);
doDMP();
moveForward(aRandomNumber(), false);
doDMP();
moveBackward(aRandomNumber(),false);
doDMP();
spinRight(aRandomNumber());
doDMP();
motorsStop(aRandomNumber());
doDMP();
spinLeft(aRandomNumber());
doDMP();
motorsStop(1000);
doDMP();
pivotLeftF(aRandomNumber(),false);
doDMP();
motorsStop(1000);
doDMP();
pivotRightF(aRandomNumber(),false);
doDMP();
motorsStop(1000);
doDMP();
pivotLeftB(aRandomNumber());
doDMP();
motorsStop(1000);
doDMP();
pivotRightB(aRandomNumber());
doDMP();
motorsStop(1000);
#endif
doDMP();
}
[/td]
[/tr]