Here's an abbreviated version of my program with the basic elements that seize. If I comment out the PWM call ("analogWrite"), the program does not seize.
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_L3GD20_U.h>
#include <Adafruit_LSM303_U.h>
// TIMING
//---------------------------------------------------------
#define MAXSAMPLE 2000 // Number of data samples:
#define CLAW_FS 100 // Control System Sample Rate (long) (Hz)
double DT = 1.0/CLAW_FS; // TIME STEP (sec)
long tstep = DT*1e6;
double deg2rad = 0.01745329;
double pi = 3.14159265;
// IMU SENSORS
//-----------------------------------------------------------------------------
// Assign a unique ID to this sensor at the same time
Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(54321);
// The default constructor uses I2C
Adafruit_L3GD20_Unified gyro = Adafruit_L3GD20_Unified(20);
sensors_event_t eventg, eventa;
// MOTOR PINS
//---------------------------------------------------------
int pin_motor_rte = 6; // green
int pin_motor_rta = 27; // orange
int pin_motor_rtb = 29; // yello
void motor_right_go(double duty)
{
uint32_t dutyx, gainx, gain;
dutyx = abs(duty) * 4095;
gainx = (uint32_t)dutyx;
gain = min(gainx, 4095);
// CALL PWM FUNCTION
//-------------------------------------
analogWrite( pin_motor_rte, gain);
// FORWARD
if (duty>=0.0) {
digitalWrite(pin_motor_rta, LOW);
digitalWrite(pin_motor_rtb, HIGH);
}
// BACK
if (duty<0.0) {
digitalWrite(pin_motor_rta, HIGH);
digitalWrite(pin_motor_rtb, LOW);
}
}
// MAIN SETUP
//--------------------
void setup()
{
int ii;
char nmeac[40];
long tbegin, tspan, tpause;
long time0, time1, time2;
// SENSITIVITIES
//-----------------------------------------------------
double sensitivity_accelx = 0.002 / 1.01793206266836;
double sensitivity_accely = 0.002 / 0.990061393069583;
double sensitivity_accelz = 0.002 / 0.908981888152909;
double sensitivity_gyro = 0.0175;
// CONTROL SYSTEM GAINS
//------------------------------------------
double Kp = 15.0; // Proportional gain
double Kd = 0.01; // Derivative gain
double Ki = 0.0; // Integral gain
long caccx, caccy, caccz, cgyrox, cgyroy, cgyroz; // Counts
double accx, accy, accz, gyrox, gyroy, gyroz; // Engineering Units
double integrate_theta=0.0;
double PID, control_P, control_I, control_D;
double gyroy_clean, gyroy_filt;
double theta, theta_filt;
// INITIALIZE SERIAL
//---------------------------------------
Serial.begin(115200);
Serial.println("Hello.");
// INITIALIZE PWM RESOLUTION
//---------------------------------------
analogWriteResolution(12);
// INITIALIZE MOTOR PINS
//---------------------------------------
pinMode(pin_motor_rte, OUTPUT); digitalWrite(pin_motor_rte, LOW );
pinMode(pin_motor_rta, OUTPUT); digitalWrite(pin_motor_rta, LOW );
pinMode(pin_motor_rtb, OUTPUT); digitalWrite(pin_motor_rtb, LOW );
// INITIALIZE I2C DEVICE
//------------------------------------------------
delay(250);
if (!gyro.begin()) {
Serial.println("Oops ... unable to initialize the L3GD20. Check your wiring!");
while (1);
}
delay(250);
if(!accel.begin()) {
Serial.println("Ooops, no LSM303 detected ... Check your wiring!");
while(1);
}
delay(250);
gyro.write8(GYRO_REGISTER_CTRL_REG1, 0xFF); // 1111 1111; 760hz, 100hz cutoff
gyro.write8(GYRO_REGISTER_CTRL_REG4, 0x10); // 0001 0000; 500 dps
gyro.write8(GYRO_REGISTER_CTRL_REG5, 0x2); // 0000 0010; Out_Sel1=1
delay(250);
accel.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A, 0x97); // 1001 0111; 1.344KHZ
accel.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG4_A, 0x18); // 0001 1000; ±4 g, hi res; Sensitivity=0.002 g/count
delay(2000);
Serial.println("Go...");
// EXAMPLE CONTROL SYSTEM
//------------------------------
Serial.print("DT ");
Serial.println(DT,6);
ii = 0;
theta = 0.0;
integrate_theta = 0.0;
tbegin = micros();
while (ii<MAXSAMPLE)
{
time0 = micros();
// IMU Measurements
//-----------------------
gyro.getEvent(&eventg);
accel.getEvent(&eventa);
caccx = eventa.acceleration.y;
caccy = eventa.acceleration.x;
caccz = eventa.acceleration.z;
cgyrox = eventg.gyro.y;
cgyroy = eventg.gyro.x;
cgyroz = eventg.gyro.z;
accx = sensitivity_accelx * caccx; // units: g's
accy = sensitivity_accely * caccy; // units: g's
accz = sensitivity_accelz * caccz; // units: g's
// ANGULAR VELOCITY
//-------------------------
gyroy = sensitivity_gyro * cgyroy * deg2rad; // units: rad/sec
// ANGLE
//-----------------------
theta = theta + gyroy*DT; // units: deg
// PID CONTROL
//------------------------
control_P = Kp * theta;
control_D = Kd * gyroy;
// MOTOR COMMAND
//-------------------
PID = control_P + control_D;
motor_right_go(PID);
// STREAM RESULTS
//--------------------
sprintf(nmeac, "%10.3lf %10.3lf\r\n", gyroy*180.0/pi, theta*180/pi );
Serial.print(nmeac);
// FORCE TIME STEP WITH MICRO DELAY
//--------------------------------------
ii++;
time1 = micros();
tpause = tstep - (time1 - time0) ;
delayMicroseconds(tpause);
}
tspan = micros() - tbegin;
Serial.println("#");
Serial.println(ii);
Serial.println(tspan);
}
void loop()
{
}