Trigger Countdown with 433 MHz transmission - time

I would like to use an arduino to read 433 MHz transmission from multiple Soil Moisture Sensors. Since I can never be sure all transmissions reach the receiver I'd like to set a countdown from the moment the first transmission is received. If another transmission is received, the countdown starts again.
After a defined amount of time (e.g. 10 Minutes) without any more signals or if all signals have been received (e.g 4 Sensors) the receiving unit should stop and come to decision based on the data it got to the point.
For transmitting and receiving I am using the <RCSwitch.h>library.
The loop of the receiving unit and one Sensor looks like this:
#include <RCSwitch.h>
RCSwitch mySwitch = RCSwitch();
void Setup(){
Serial.begin(9600);
mySwitch.enableReceive(4);
}
void loop() {
if (mySwitch.available()) {
int value = mySwitch.getReceivedValue();
if (value == 0) {
lcd.clear();
Serial.print("Unknown encoding");
}
else {
Serial.print(mySwitch.getReceivedValue());
Serial.print("%");
}
The full code includes some differentiation mechanism for all sensors but I figured that might not be relevant for my question.
Question:
What's the best way to do this without a real time clock module. As far as I know I can't wait by using delay(...)since then I won't receive any data while the processor waiting.

You can use millis() as a clock. It returns the number of milliseconds since the arduino started.
#define MINUTES(x) ((x) * 60000UL)
unsigned long countStart = 0;
void loop()
{
if (/*read from module ok*/)
{
countStart = millis();
// sanity check, since millis() eventually rolls over
if (countStart == 0)
countStart = 1;
}
if (countStart && ((millis() - countStart) > MINUTES(10)))
{
countStart = 0;
// trigger event
}
}

Arduino's internal timers can also be used in this situation. If a long time period is needed, it's better to use 16bit counter (usually timer1) at 1024 prescaler (largest available). If the largest time interval of timer is greater than time required, then a counter have to be added in order to keep track of 1 minute interval.
For example, for 1-minute interval, initialize registers as:
TCCR1A = 0; //Initially setting every register as 0x0000
TCCR1B = 0;
TCNT1 = 0;
OCR1A = 468750; // compare match register 16MHz/1024/2/frequency(hz)
TCCR1B |= (1 << WGM12); // Timer compare mode
TCCR1B |= (1 << CS10) | (1 << CS10); // 1024 prescaler
TIMSK1 |= (1 << OCIE1A); // enable timer compare interrupt
These configuration of timer will give interrupt time of 1 minute. And upon timer completion ISR TIMER1_COMPA_vect will be run. You can play around with value of OCR1A for different interrupt periods.
Main advantage of using interrupts is that they don't block any task and can will be executed instantaneously (if interrupts are not disabled explicitly).

Related

SPI implementation stuck on “while(!spi_is_tx_empty(WINC1500_SPI));”

I'm currently implementing a driver for the WINC1500 to be used with an ATMEGA32 MCU and it's getting stuck on this line of "while(!spi_is_tx_empty(WINC1500_SPI));". The code builds and runs but it won't clear what's inside in this function to proceed through my code and boot up the Wifi Module. I've been stuck on this problem for weeks now with no progress and don't know how to clear it.
static inline bool spi_is_tx_empty(volatile avr32_spi_t *spi)
{
// 1 = All Transmissions complete
// 0 = Transmissions not complete
return (spi->sr & AVR32_SPI_SR_TXEMPTY_MASK) != 0;
}
Here is my implementation of the SPI Tx/Rx function
void m2mStub_SpiTxRx(uint8_t *p_txBuf,
uint16_t txLen,
uint8_t *p_rxBuf,
uint16_t rxLen)
{
uint16_t byteCount;
uint16_t i;
uint16_t data;
// Calculate the number of clock cycles necessary, this implies a full-duplex SPI.
byteCount = (txLen >= rxLen) ? txLen : rxLen;
// Read / Transmit.
for (i = 0; i < byteCount; ++i)
{
// Wait for transmitter to be ready.
while(!spi_is_tx_ready(WINC1500_SPI));
// Transmit.
if (txLen > 0)
{
// Send data from the transmit buffer
spi_put(WINC1500_SPI, *p_txBuf++);
--txLen;
}
else
{
// No more Tx data to send, just send something to keep clock active.
// Here we clock out a don't care byte
spi_put(WINC1500_SPI, 0x00U);
// Not reading it back, not being cleared 16/1/2020
}
// Reference http://asf.atmel.com/docs/latest/avr32.components.memory.sdmmc.spi.example.evk1101/html/avr32_drivers_spi_quick_start.html
// Wait for transfer to finish, stuck on here
// Need to clear the buffer for it to be able to continue
while(!spi_is_tx_empty(WINC1500_SPI));
// Wait for transmitter to be ready again
while(!spi_is_tx_ready(WINC1500_SPI));
// Send dummy data to slave, so we can read something from it.
spi_put(WINC1500_SPI, 0x00U); // Change dummy data from 00U to 0xFF idea
// Wait for a complete transmission
while(!spi_is_tx_empty(WINC1500_SPI));
// Read or throw away data from the slave as required.
if (rxLen > 0)
{
*p_rxBuf++ = spi_get(WINC1500_SPI);
--rxLen;
}
else
{
spi_get(WINC1500_SPI);
}
}
Debug output log
Disable SPI
Init SPI module as master
Configure SPI and Clock settings
spi_enable(WINC1500_SPI)
InitStateMachine()
INIT_START_STATE
InitStateMachine()
INIT_WAIT_FOR_CHIP_RESET_STATE
m2mStub_PinSet_CE
m2mStub_PinSet_RESET
m2mStub_GetOneMsTimer();
SetChipHardwareResetState (CHIP_HARDWARE_RESET_FIRST_DELAY_1MS)
InitStateMachine()
INIT_WAIT_FOR_CHIP_RESET_STATE
if(m2m_get_elapsed_time(startTime) >= 2)
m2mStub_PinSet_CE(M2M_WIFI_PIN_HIGH)
startTime = m2mStub_GetOneMsTimer();
SetChipHardwareResetState(CHIP_HARDWARE_RESET_SECOND_DELAY_5_MS);
InitStateMachine()
INIT_WAIT_FOR_CHIP_RESET_STATE
m2m_get_elapsed_time(startTime) >= 6
m2mStub_PinSet_RESET(M2M_WIFI_PIN_HIGH)
startTime = m2mStub_GetOneMsTimer();
SetChipHardwareResetState(CHIP_HARDWARE_RESET_FINAL_DELAY);
InitStateMachine()
INIT_WAIT_FOR_CHIP_RESET_STATE
m2m_get_elapsed_time(startTime) >= 10
SetChipHardwareResetState(CHIP_HARDWARE_RESET_COMPLETE)
retVal = true // State machine has completed successfully
g_scanInProgress = false
nm_spi_init();
reg = spi_read_reg(NMI_SPI_PROTOCOL_CONFIG)
Wait for a complete transmission
Wait for transmitter to be ready
SPI_PUT(WINC1500_SPI, *p_txBuf++);
--txLen;
Wait for transfer to finish, stuck on here
Wait for transfer to finish, stuck on here
The ATmega32 is an 8-bit AVR but you seem to be using code for the AVR32, a family of 32-bit AVRs. You're probably just using the totally wrong code and you should consult the datasheet of the ATmega32, and search for SPI for the AVR ATmega family.

I²C Master Write with PIC18F45K50 : keeps SCL low

I'm writing my own I²C Master Write function according to Microchip's datasheet. I'm using MPLAB X. I generated the configuration with the Code Configurator, but here are the interesting bits :
// R_nW write_noTX; P stopbit_notdetected; S startbit_notdetected; BF RCinprocess_TXcomplete; SMP Standard Speed; UA dontupdate; CKE disabled; D_nA lastbyte_address;
SSP1STAT = 0x80;
// SSPEN enabled; WCOL no_collision; CKP Idle:Low, Active:High; SSPM FOSC/4_SSPxADD_I2C; SSPOV no_overflow;
SSP1CON1 = 0x28;
// SBCDE disabled; BOEN disabled; SCIE disabled; PCIE disabled; DHEN disabled; SDAHT 100ns; AHEN disabled;
SSP1CON3 = 0x00;
// Baud Rate Generator Value: SSP1ADD 80;
SSP1ADD = 0x50;
// clear the master interrupt flag
PIR1bits.SSP1IF = 0;
// enable the master interrupt
PIE1bits.SSP1IE = 1;
So : Standard Speed, 100ns hold time, Master Mode, clokck frequency about 50kHz.
I tried to follow the procedure described p238 of the datasheet :
http://ww1.microchip.com/downloads/en/DeviceDoc/30000684B.pdf
Here's my code :
#include "mcc_generated_files/mcc.h"
#include <stdio.h>
#define _XTAL_FREQ 16000000
#define RTS_PIN PORTDbits.RD3
#define CTS_PIN PORTDbits.RD2
#define LED_PIN PORTAbits.RA1
#define RX_FLAG PORTAbits.RA2
uint8_t c;
// Define putch() for printf())
void putch(char c)
{
EUSART1_Write(c);
}
void main(void)
{
// Initialize the device
SYSTEM_Initialize();
while (1)
{
// Generate a START condition by setting Start Enable bit
SSP1CON2bits.SEN = 1;
// Wait for START to be completed
while(!PIR1bits.SSPIF);
// Clear flag
PIR1bits.SSPIF = 0;
// Load the address + RW byte in SSP1BUF
// Address = 85 ; request type = WRITE (0)
SSP1BUF = 0b10101010;
// Wait for ack
while (SSP1CON2bits.ACKSTAT);
// Wait for MSSP interrupt
while (!PIR1bits.SSPIF);
// Load data (0x11) in SSP1BUF
SSP1BUF = 0x11;
// Wait for ack
while (SSP1CON2bits.ACKSTAT);
// Generate a STOP condition
SSP1CON2bits.PEN = 1;
// Wait for STOP to be completed
while(!PIR1bits.SSPIF);
// Clear flag
PIR1bits.SSPIF = 0;
// Wait for 1s before sending the next byte
__delay_ms(1000);
}
}
The slave device is an Arduino which I have tested with another Arduino (Master) to make sure it's working correctly.
My problem is : analysing the SDA/SCL signals with a logic analyser, when I start the PIC I get 2 correct messages, that's with correct address send and byte transmission, but at the end of the second SCL is held LOW, which makes all other writings bad (can't have a proper START condition if SCL is held LOW). BTW, at the end of the first transmission, SCL is held LOW for like 3ms, but then comes HIGH again without any reason.
Can anyone here point what I'm doing wrong ? Did I forget something ?
Thanx in advance.
Best regards.
Eric
PS : when testing the slave with another Arduino as the Master, SCL is set HIGH as soon as the transmission is over.
One thing I'm noticing is that after sending the slave address you are waiting for the ACK (ACKSTAT) then waiting for the SSPIF Interrupt Flag, but you are not checking for SSPIF after the data byte. You are only checking ACKSTAT. Maybe try waiting for and clearing the SSPIF before setting PEN to assert the stop conditon?
Have you checked the state of the SSPCON and SSPSTAT registers when this behavior occurs, that might help narrow down where the problem lies.
Thanx a lot for your answer !
I cleared SSP1IF after loading the data byte, and now it's working fine !
I think I understand now what was happening : the datasheet indicates that ACKSTAT is the only register bit that reacts on the rising edge of SCL, instead of the falling edge for the other bits. So in my code, I generate the STOP condition too early, and that might make it inoperative. Thus no STOP condition is generated, SCL is stuck LOW, and the next transmission cannot be started.
Furthermore, when I wait for the STOP condition to be completed, the SSP1IF flag is still set, so he doesn't actually wait and jumps directly to the delay() function. I don't know if that matters as he waits anyway, but it could matter if ever I tried to send packets one after the other.
So I here's the function I wrote, and which is working :
(BTW it can take up to 255 data bytes)
void MasterWrite(char _size, char* _data)
{
// Generate a START condition by setting Start Enable bit
SSP1CON2bits.SEN = 1;
// Wait for START to be completed
while(!PIR1bits.SSPIF);
// Clear flag
PIR1bits.SSPIF = 0;
// Load the address + RW byte in SSP1BUF
// Address = 85 ; request type = WRITE (0)
SSP1BUF = 0b10101010;
// Wait for ack
while (SSP1CON2bits.ACKSTAT);
// Wait for MSSP interrupt
while (!PIR1bits.SSPIF);
// Clear flag
PIR1bits.SSPIF = 0;
for (int i=0; i<_size; i++)
{
// Load data in SSP1BUF
SSP1BUF = *(_data+i);
// Wait for ack
while (SSP1CON2bits.ACKSTAT);
// Wait for MSSP interrupt
while (!PIR1bits.SSPIF);
// Clear flag
PIR1bits.SSPIF = 0;
}
// Generate a STOP condition
SSP1CON2bits.PEN = 1;
// Wait for STOP to be completed
while(!PIR1bits.SSPIF);
// Clear flag
PIR1bits.SSPIF = 0;
}
Thanx a lot again for your help !
Best regards.
Eric

c: socketCAN connection: read() not fast enough

socketCAN connection: read() not fast enough
Hello,
I use the socket() connection for my CAN communication.
fd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
I'm using 2 threads: one periodic 1ms RT thread to send data and one
thread to read the incoming messages. The read function looks like:
void readCan0Socket(void){
int receivedBytes = 0;
do
{
// set GPIO pin low
receivedBytes = read(fd ,
&receiveCanFrame[recvBufferWritePosition],
sizeof(struct can_frame));
// reset GPIO pin high
if (receivedBytes != 0)
{
if (receivedBytes == sizeof(struct can_frame))
{
recvBufferWritePosition++;
if (recvBufferWritePosition == CAN_MAX_RECEIVE_BUFFER_LENGTH)
{
recvBufferWritePosition = 0;
}
}
receivedBytes = 0;
}
} while (1);
}
The socket is configured in blocking mode, so the read function stays open
until a message arrived. The current implementation is working, but when
I measure the time between reading a message and the next waiting state of
the read function (see set/reset GPIO comment) the time varies between 30 us
(the mean value) and > 200 us. A value greather than 200us means
(CAN has a baud rate of 1000 kBit/s) that packages are not recognized while
the read() handles the previous message. The read() function must be ready within
134 us.
How can I accelerate my implementation? I tried to use two threads which are
separated with Mutexes (lock before the read() function and unlock after a
message reception), but this didn't solve my problem.

AlarmRepeat function in Arduino

I am using the Time.h and TimeAlarms.h libraries in Arduino. I try to call a function on specific times of the day (every day). The function is called on the first day, but then on the next day it seems like the alarm ceases to work, although it should repeat every day. Any ideas what's wrong with my sketch?
#include <Time.h>
#include <TimeAlarms.h>
#define TIME_MSG_LEN 11 // time sync to PC is HEADER followed by Unix time_t as ten ASCII digits
#define TIME_HEADER 'T' // Header tag for serial time sync message
#define TIME_REQUEST 7 // ASCII bell character requests a time sync message
// The constant variables used in the code:
const unsigned long shock_delay = 10; // shock stimulus duration (s)
// Edit below only if you make changes to the hardware configuration:
const int tonePin = 13; // the number of the LED pin
const int shockPin = 12; // the number of the shocker pin
const int buzzerOut = 8; // the number of the tone producer
const int trialButton=2; // the number of the trial button
const int controlButton=3; // the number of the control button
// Define the variables that will change in the code
unsigned int Interval1; // the interval between the beginning of the hour and the first footshock
unsigned int Interval2; // the interval between the first and the second footshock
void setup()
{
Serial.begin(9600); // opens serial port, sets data rate to 9600 bps
setTime(23,59,50,10,4,15); // The current time (HH,MM,SS,DD,MM,YY)
// Define the position of the different digital pins
pinMode(tonePin, OUTPUT); // The tone LED (red)
pinMode(shockPin, OUTPUT); // The shock output, which will coincide with green LED
pinMode(trialButton, INPUT); // The trial input button
pinMode(controlButton, INPUT); // The control input button
Alarm.alarmRepeat(20,00,0,RandomShock); // Initiate the user defined RandomShock function
Alarm.alarmRepeat(21,00,0,RandomShock); // Initiate the user defined RandomShock function
Alarm.alarmRepeat(22,00,0,RandomShock); // Initiate the user defined RandomShock function
Alarm.alarmRepeat(23,00,0,RandomShock); // Initiate the user defined RandomShock function
Alarm.alarmRepeat(0,00,0,RandomShock); // Initiate the user defined RandomShock function
Alarm.alarmRepeat(1,00,0,RandomShock); // Initiate the user defined RandomShock function
Alarm.alarmRepeat(2,00,0,RandomShock); // Initiate the user defined RandomShock function
Alarm.alarmRepeat(3,00,0,RandomShock); // Initiate the user defined RandomShock function
Alarm.alarmRepeat(4,00,0,RandomShock); // Initiate the user defined RandomShock function
Alarm.alarmRepeat(5,00,0,RandomShock); // Initiate the user defined RandomShock function
Alarm.alarmRepeat(6,00,0,RandomShock); // Initiate the user defined RandomShock function
Alarm.alarmRepeat(7,00,0,RandomShock); // Initiate the user defined RandomShock function
}
void loop(){
// Create a message that gives the current time and date on the monitor
if(Serial.available() )
{
processSyncMessage();
}
if(timeStatus() == timeNotSet)
Serial.println("waiting for sync message");
else
digitalClockDisplay(); // The function that calls on the time display
Alarm.delay(1000); // Delay of 1 minute between time display
}
void digitalClockDisplay(){
// digital clock display of the time
Serial.print(hour());
printDigits(minute());
printDigits(second());
Serial.print(" ");
Serial.print(day());
Serial.print(" ");
Serial.print(month());
Serial.print(" ");
Serial.print(year());
Serial.println();
}
void printDigits(int digits){
// utility function for digital clock display: prints preceding colon and leading 0
Serial.print(":");
if(digits < 10)
Serial.print('0');
Serial.print(digits);
}
void processSyncMessage() {
// if time sync available from serial port, update time and return true
while(Serial.available() >= TIME_MSG_LEN ){ // time message consists of header & 10 ASCII digits
char c = Serial.read() ;
Serial.print(c);
if( c == TIME_HEADER ) {
time_t pctime = 0;
for(int i=0; i < TIME_MSG_LEN -1; i++){
c = Serial.read();
if( c >= '0' && c <= '9'){
pctime = (10 * pctime) + (c - '0') ; // convert digits to a number
}
}
setTime(pctime); // Sync Arduino clock to the time received on the serial port
}
}
}
void RandomShock () {
Interval1 = random(0,60); // Random value between 0 and 59 [min]
digitalWrite(tonePin,HIGH); // Indicate the shock program is ON by the red LED
Alarm.delay(Interval1*60000); // Wait for the duration of the first interval
digitalWrite(tonePin,LOW); // Turn the red LED OFF
digitalWrite(shockPin, HIGH); // Apply the first shock (green LED will turn ON)
Serial.println("Applying an electeric shock at:"); // Write a message indicating a shock is applied
digitalClockDisplay(); // Display the time during which the shock was applied
Alarm.delay(shock_delay*1000); // The duration of the shock [10 seconds]
digitalWrite(shockPin, LOW); // Terminate the first shock (green LED will turn OFF)
Interval2 = random(0,(61-Interval1)); // Randomly asign a value to the second interval [min]
digitalWrite(tonePin,HIGH); // Indicate the shock program is ON by the red LED
Alarm.delay(Interval2*60000-20000); // Wait for the duration of the second interval
digitalWrite(tonePin,LOW); // Turn the red LED OFF
digitalWrite(shockPin,HIGH); // Apply the second shock (green LED will turn ON)
Serial.println("Applying an electeric shock at:"); // Write a message indicating a shock is applied
digitalClockDisplay(); // Display the time during which the shock was applied
Alarm.delay(shock_delay*1000); // The duration of the shock [10 seconds]
digitalWrite(shockPin,LOW); // Terminate the second shock
digitalWrite(tonePin,HIGH); // Indicate the shock program is ON by the red LED
Alarm.delay((60-Interval1-Interval2)*60000); // Wait until the hour is completed
digitalWrite(tonePin,LOW); // Turn the red LED OFF
}
There are two problems with the sketch that I wrote:
Alarm.alarmRepeat function does not seem to be able to call my
function exactly at midnight (00:00:00), however, if I schedule it
to any other time (e.g., 00:00:01) it works just fine.
There is a limit to the number of alarms you can schedule: the
maximum number of alarms is 6. The solution is either to change this threshold, or to reduce the number of alarms.
After correcting for these two issues the sketch work smoothly.
Seems that TimeAlarm library is unable to set an alarm to midnight
https://github.com/PaulStoffregen/TimeAlarms/issues/3
Try this pull request https://github.com/PaulStoffregen/TimeAlarms/pull/4
If you need more than 6 alarms just look for that at TimeAlarms.h on the library:
#if defined(__AVR__)
#define dtNBR_ALARMS 6 // max is 255
#else
#define dtNBR_ALARMS 12 // assume non-AVR has more memory
#endif
and change it to your need (example:24)
#if defined(__AVR__)
#define dtNBR_ALARMS 24 // max is 255
#else
#define dtNBR_ALARMS 12 // assume non-AVR has more memory
#endif

Arduino/MPU6050/AdafruitMotorShieldV2: script hangs/freezes when turn on motors

I'm a newby to robotics and electronics in general, so please don't assume I tried anything you might think is obvious.
I'm trying to create a cart which will basically run around by itself (simple AI routines to avoid obstacles, go from pt. A to pt. B around corners, follow lines, etc.). I'm putting together an Adafruit Arduino Uno R3 with the Adafruit Motor Shield v2 and an MPU-6050. I'm using the "breadboard" on the Motor Shield for the circuitry, soldering everything there.
I can get all the pieces working independently with their own scripts: the Motor Shield drives the 4 motors as expected using the Adafruit library; I'm using the "JRowberg" library for the MPU-6050, and started with the example MPU6050_DMP6.ino, which works fine as long as the cart motors are turned off. My only changes in the example script below are motor startup and some simple motor commands.
As long as I leave the switch which powers the motors off, everything seems fine: it outputs to the Serial window continuously with Euler data which, I assume, is correct. However, a few seconds after I turn on the power to the motors (and the wheels start turning), it just hangs/freezes: the output to the Serial window stops (sometimes in mid-line), and the wheels keep turning at the speed of their last change. Sometimes I see "FIFO overflow" errors, but not always. Sometimes I see "nan" for some of the floating point values before it hangs, but not always.
Some things I've tried, all of which changed noting:
* I've swapped out the MPU-6050 board for another from the same manufacturer.
* I've tried moving the MPU-6050 away from the motors using a ribbon cable.
* I've changed the I2C clock using JRowber's advice (a change in a .h file and changing the value of the TWBR variable), but I don't think I've tried all possible values.
* I've changed the speed of the MotorShield in the AFMS.begin() command, although, again, there are probably other values I haven't tried, and I'm not sure how in-sync this and the TWBR value need to be.
And some other things, all to no avail.
Below is an example script which fails for me:
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#include "Adafruit_MotorShield.h"
#include "utility/Adafruit_PWMServoDriver.h"
#define DEBUG 1
MPU6050 mpu;
#define OUTPUT_READABLE_EULER
#define LED_PIN 13
bool blinkState = false;
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
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
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
#define NUM_MOTORS 4
#define MOTOR_FL 0
#define MOTOR_FR 1
#define MOTOR_RR 2
#define MOTOR_RL 3
Adafruit_DCMotor *myMotors[NUM_MOTORS] = {
AFMS.getMotor(1),
AFMS.getMotor(2),
AFMS.getMotor(3),
AFMS.getMotor(4),
};
#define CHANGE_SPEED_TIME 500
long changeSpeedMillis = 0;
int curSpeed = 30;
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
void setup() {
#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(115200);
while (!Serial); // wait for Leonardo enumeration, others continue immediately
// start the motor shield.
AFMS.begin(); // create with the default frequency 1.6KHz
// AFMS.begin(4000); // OR with a different frequency, say 4KHz
// kill all the motors.
myMotors[MOTOR_FL]->run(BRAKE);
myMotors[MOTOR_FL]->setSpeed(0);
myMotors[MOTOR_FR]->run(BRAKE);
myMotors[MOTOR_FR]->setSpeed(0);
myMotors[MOTOR_RR]->run(BRAKE);
myMotors[MOTOR_RR]->setSpeed(0);
myMotors[MOTOR_RL]->run(BRAKE);
myMotors[MOTOR_RL]->setSpeed(0);
Serial.println("Motor Shield ready!");
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
Serial.println(F("\nSend any character to begin DMP programming and demo: "));
while (Serial.available() && Serial.read()); // empty buffer
while (!Serial.available()); // wait for data
while (Serial.available() && Serial.read()); // empty buffer again
// 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(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();
} 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);
}
void loop() {
// if programming failed, don't try to do anything
if (!dmpReady) return;
// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {
// as per Vulpo's post.
delay(10);
if (millis() > changeSpeedMillis) {
curSpeed += 20;
if (curSpeed > 256) {
curSpeed = 30;
}
Serial.print("changing speed to: ");
Serial.println(curSpeed);
myMotors[MOTOR_FL]->run(FORWARD);
myMotors[MOTOR_FL]->setSpeed(curSpeed);
myMotors[MOTOR_FR]->run(FORWARD);
myMotors[MOTOR_FR]->setSpeed(curSpeed);
myMotors[MOTOR_RR]->run(FORWARD);
myMotors[MOTOR_RR]->setSpeed(curSpeed);
myMotors[MOTOR_RL]->run(FORWARD);
myMotors[MOTOR_RL]->setSpeed(curSpeed);
changeSpeedMillis = millis() + CHANGE_SPEED_TIME;
}
}
// 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) 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;
#ifdef OUTPUT_READABLE_EULER
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetEuler(euler, &q);
Serial.print("euler\t");
Serial.print(euler[0] * 180/M_PI);
Serial.print("\t");
Serial.print(euler[1] * 180/M_PI);
Serial.print("\t");
Serial.println(euler[2] * 180/M_PI);
#endif
// blink LED to indicate activity
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
}
}
Have you considered that your troubles are caused by interference from the currents flowing into your motors?
If your motors are DC brush, then more interference may be radiated from the brushes back into your various wires.
As a first step, perhaps let only one motor work and see if hangups diminish in frequency (although, to be sure, you need a 'scope onto a few wires carrying logic signals.

Resources