I am trying to tune quad copter (single axis) via string setup.
For pitch axis it is some what ok (I think), quad stabilized with P alone (is it weird ?).
But for Roll - i'm unable to tune it. it just rolls as soon small throttle applied. (I dont even see oscillations)
I changed X config to + Config but same reaction.
I am thinking there may be some bug in my code for roll (or pitch ) PID calculation (I'm not sure) which is not visible to me.
So If you guys found anything weird in attached code let me know.
/*
ref - http://ardupilot.org/dev/docs/apmcopter-programming-attitude-control-2.html and ymfc-al code
angle error (difference between target error and actual angle) is converted in to desired rotation rate followed by
previous rate pid from ksrmQuadRateMode
The input pulses from RC receiver are converted to the +/- rate of rotation.
The pilot input acts as the SetPoint to roll,pitch and yaw rate PID's. Measured input to PID comes from
gyroscope
Motor directions for Quad + configuration
5 (cw)
|
|
(ccw) 6 ------|------ (ccw)4
|
|
7 (cw)
Arduino Pin
m0 right - ccw 4
m1 top - cw 5
m2 left - ccw 6
m3 bottom - cw 7
MPU6050 Connections
SCL - A5
SDA - A4
Rx connections
Enable PCMSK0 Register - PCMSK0 - portB (D8-D13) (PCINT0-PCINT6)
ArduinoPin PCINTx
ch1 - 8 PCINT0 - Roll
ch2 - 9 PCINT1 - Pitch
ch3 - 10 PCINT2 - Throttle
ch4 - 11 PCINT3 - YAW
Arm and Disarm mechanism
To Arm - Throttle Min && Yaw Max
To DisArm - Throttle Min && Roll Max
*/
// Dont use servo - it is limited to 50Hz and conflict with Software serial
// If you are planning for more serial devices go for arduino Mega
// I use blutooth to get flight parameters - it will be used with hardware serial
#include<PID_v1.h>
#include<Wire.h>
#include<math.h>
//used to send debug info via bluetooth
//#define DEBUG
#define DATA_INTERVAL 200 //send debug info every 200 milli seconds
unsigned long btDataStartMillis;
//motor connections
#define m0 4
#define m1 5
#define m2 6
#define m3 7
//receiver connections
#define ch0 8
#define ch1 9
#define ch2 10
#define ch3 11
#define motorMinValue 1150
#define motorMaxValue 1800
//variables used for MPU6050 Angle calculation
const float GYRO_SENSITIVITY_SCALE_FACTOR = 65.5; //for 500 degrees/sec full scale
//MPU6050 I2C address
const int MPU6050_ADDR = 0b1101000;
//MPU6050 Register Addresses
const int PWR_REG_ADDR = 0x6B;
const int GYRO_CONFIG_REG_ADDR = 0x1B;
const int GYRO_CONFIG_REG_VALUE = 0x08;//for 500 degrees / sec
const int ACCR_CONFIG_REG_ADDR = 0x1C;
const int ACCR_CONFIG_REG_VALUE = 0x10; //for +/- 8g
const int ACCR_READ_START_ADDR = 0x3B;
//variables used for angular rate and angle calculations
const float radToDegreeConvert = 180.0 / PI;
int16_t accX, accY, accZ, gyroX, gyroY, gyroZ, tmp; //used to store the data from MPU6050 registers
float rotX, rotY, rotZ; //used to store the angular rotation from gyro
float accAngleX = 0.0, accAngleY = 0.0, accrAngleZ = 0.0;
float compAngleX = 0.0, compAngleY = 0.0;
float pitchAngle = 0.0, rollAngle = 0.0;
float pitchAngleAdjust = 0.0, rollAngleAdjust = 0.0;
unsigned long prevTime = 0, currentTime = 0; //used to calculate the dt for gyro integration
unsigned long loopTimer = 0, now = 0, difference = 0; //used to calculate the loop execution time
//gyro offset variables
float gyroOffsetValX = 0, gyroOffsetValY = 0, gyroOffsetValZ = 0;
float accrOffsetValX = 0, accrOffsetValY = 0;
const int noOfSamplesForOffset = 400;
//variables used in pinChange ISR
volatile boolean armMotors = false; //used to arm and disarm quad
volatile int pwmDuration[4]; //used to store pwm duration
volatile unsigned long pwmStart[4];
//port status
volatile byte prevPortState[] = {0, 0, 0, 0};
volatile byte presentPortState[4];
//pinDeclarations for Rx
const byte rxCh[] = {ch0, ch1, ch2, ch3};
const byte noOfChannels = sizeof(rxCh);
//PID Constants //0.5
const float pitchRatePGain = 1.0;
const float pitchRateIGain = 0.0;
const float pitchRateDGain = 0.0;
const float rollRatePGain = 1.0;
const float rollRateIGain = 0.0;
const float rollRateDGain = 0.0;
const float yawRatePGain = 0.0;
const float yawRateIGain = 0.0;
const float yawRateDGain = 0.0;
const int pidOutMax = 400;
// PID variables
float pidPitchRateIn = 0.0, pidPitchRateOut = 0.0, pidPitchRateSetPoint = 0.0;
float pidRollRateIn = 0.0, pidRollRateOut = 0.0, pidRollRateSetPoint = 0.0;
float pidYawRateIn = 0.0, pidYawRateOut = 0.0, pidYawRateSetPoint = 0.0;
float pitchError = 0.0, rollError = 0.0, yawError = 0.0;
float pitchPrevError = 0.0, rollPrevError = 0.0, yawPrevError = 0.0;
float pitchErrorSum = 0.0, rollErrorSum = 0.0, yawErrorSum = 0.0; //for integral coefficient
float pitchErrorDelta = 0.0 , rollErrorDelta = 0.0 , yawErrorDelta = 0.0; //for differntial coeffcient
const int pidMax = 400;
//variables to store the motor values calculated using pid and throttle
int m0Value, m1Value, m2Value, m3Value;
//Interrupt Service Routine will fire when for PinChange in PortB
ISR(PCINT0_vect) {
for (int ch = 0; ch < noOfChannels ; ch++) {
presentPortState[ch] = digitalRead(rxCh[ch]);
}//end of for looptr
for (int c = 0; c < noOfChannels ; c++) {
if (prevPortState[c] == 0 & presentPortState[c] == 1) {
//if previous state is 0 and present state is 1 (Raising Edge) then take the time stamp
pwmStart[c] = micros();
prevPortState[c] = 1; //update the prevPort State
} else if (prevPortState[c] == 1 & presentPortState[c] == 0) {
//if previous state is 1 and present state is 0 (Falling Edge) then calculate the width based on the change
pwmDuration[c] = micros() - pwmStart[c];
prevPortState[c] = 0; //update Present PortState
}
}//end of for loop
//ARM motors
if (pwmDuration[2] < 1250 && pwmDuration[3] > 1700) {
//Throttle Min And Yaw Max
armMotors = true;
}
//DISARM Motors
if (pwmDuration[2] < 1250 && pwmDuration[0] > 1600) {
//Throttle Min and Roll Max
armMotors = false;
}
}//end of ISR Fcn
void initReceiver() {
cli(); //Clear all interrupts
PCICR |= 1 << PCIE0; //Enable port B Registers i.e D8-D13
PCMSK0 |= 1 << PCINT3 | 1 << PCINT2 | 1 << PCINT1 | 1 << PCINT0 ; // Pin11,10,9,8
sei(); //enable all interrupts
for (int i = 0 ; i < noOfChannels ; i++) {
pinMode(rxCh[i], INPUT);
digitalWrite(rxCh[i], HIGH); //enable pullup
}//end of for loop
}//end of initReceiver Fcn
void updateMotors() {
int throttle = pwmDuration[2];
m0Value = throttle - pidRollRateOut + pidYawRateOut; //right
m1Value = throttle - pidPitchRateOut - pidYawRateOut; //top
m2Value = throttle + pidRollRateOut + pidYawRateOut; //left
m3Value = throttle + pidPitchRateOut - pidYawRateOut; //bottom
//dont send values more than motorMaxValue - ESC may get into trouble
if (m0Value > motorMaxValue) m0Value = motorMaxValue;
if (m1Value > motorMaxValue) m1Value = motorMaxValue;
if (m2Value > motorMaxValue) m2Value = motorMaxValue;
if (m3Value > motorMaxValue) m3Value = motorMaxValue;
//dont send values less than motorMinValue - Keep motors running
if (m0Value < motorMinValue) m0Value = motorMinValue + 10;
if (m1Value < motorMinValue) m1Value = motorMinValue + 10;
if (m2Value < motorMinValue) m2Value = motorMinValue + 10;
if (m3Value < motorMinValue) m3Value = motorMinValue + 10;
// Refresh rate is 250Hz: send ESC pulses every 4000µs
while ((now = micros()) - loopTimer < 4000);
// Update loop timer
loopTimer = now;
// Set pins #4 #5 #6 #7 HIGH
PORTD |= B11110000;
// Wait until all pins #4 #5 #6 #7 are LOW
while (PORTD >= 16) {
now = micros();
difference = now - loopTimer;
if (difference >= m0Value) PORTD &= B11101111; // Set pin #4 LOW
if (difference >= m1Value) PORTD &= B11011111; // Set pin #5 LOW
if (difference >= m2Value) PORTD &= B10111111; // Set pin #6 LOW
if (difference >= m3Value) PORTD &= B01111111; // Set pin #7 LOW
}//end of while loop
}//end of updateMotors Fcn
void initializeMotors() {
pinMode(m0, OUTPUT);
pinMode(m1, OUTPUT);
pinMode(m2, OUTPUT);
pinMode(m3, OUTPUT);
}//end of initializeMotors Fcn
/*
This function will convert Rx inputs into angular rate
which are used as SetPoints for PID
*/
void setPointUpdate() {
pidRollRateSetPoint = 0.0;
pidPitchRateSetPoint = 0.0;
pidYawRateSetPoint = 0.0;
if (pwmDuration[0] > 1508) {
pidRollRateSetPoint = pwmDuration[0] - 1508;
} else if (pwmDuration[0] < 1492) {
pidRollRateSetPoint = pwmDuration[0] - 1492;
}
rollAngleAdjust = rollAngle * 15; // convert 0 - 30 to 0 -450
pidRollRateSetPoint = pidRollRateSetPoint - rollAngleAdjust; //calculate angleError
pidRollRateSetPoint = pidRollRateSetPoint / 3.0 ;// the max roll rate is aprox 164 degrees per second (500/3 = 165 d/s )
if (pwmDuration[1] > 1508) {
pidPitchRateSetPoint = pwmDuration[1] - 1508;
} else if (pwmDuration[1] < 1492) {
pidPitchRateSetPoint = pwmDuration[1] - 1492;
}
pitchAngleAdjust = pitchAngle * 15;
pidPitchRateSetPoint = pidPitchRateSetPoint - pitchAngleAdjust;
pidPitchRateSetPoint = pidPitchRateSetPoint / 3.0;
if (pwmDuration[3] > 1508) {
pidYawRateSetPoint = pwmDuration[3] - 1508;
} else if (pwmDuration[3] < 1492) {
pidYawRateSetPoint = pwmDuration[3] - 1492;
}
pidYawRateSetPoint = pidYawRateSetPoint / 3.0;
}//end of SetPointUpdate Fcn
void initPID() {
//nothing to initialize here
}//end of initPID Fcn
/*
In this fuction we configure mpu6050 and calculate offsets i.e calibration
*/
void initMPU6050() {
configureMPU();
calculateOffsets();
}//end of initMPU6050 Fcn
void calculateOffsets() {
float gyroTotalValX = 0, gyroTotalValY = 0, gyroTotalValZ = 0;
float accrTotalValX = 0, accrTotalValY = 0;
for (int i = 0 ; i < noOfSamplesForOffset ; i++) {
readMPU();
delayMicroseconds(10);
gyroTotalValX = gyroTotalValX + rotX;
gyroTotalValY = gyroTotalValY + rotY;
gyroTotalValZ = gyroTotalValZ + rotZ;
accAngleY = atan(accX / sqrt(pow(accY, 2) + pow(accZ, 2))) * radToDegreeConvert;
accAngleX = atan(accY / sqrt(pow(accX, 2) + pow(accZ, 2))) * radToDegreeConvert;
accrTotalValX = accrTotalValX + accAngleX;
accrTotalValY = accrTotalValY + accAngleY;
// Serial.println("..........");
}//end of for loop
gyroOffsetValX = gyroTotalValX / noOfSamplesForOffset;
gyroOffsetValY = gyroTotalValY / noOfSamplesForOffset;
gyroOffsetValZ = gyroTotalValZ / noOfSamplesForOffset;
accrOffsetValX = accrTotalValX / noOfSamplesForOffset;
accrOffsetValY = accrTotalValY / noOfSamplesForOffset;
//read again to get current values
readMPU();
accAngleY = atan(accX / sqrt(pow(accY, 2) + pow(accZ, 2))) * radToDegreeConvert;
accAngleX = atan(accY / sqrt(pow(accX, 2) + pow(accZ, 2))) * radToDegreeConvert;
//apply offsets to accr values
accAngleX = accAngleX - accrOffsetValX;
accAngleY = accAngleY - accrOffsetValY;
//set initial values for gyro and complementary filter
rotX = accAngleX;
rotY = accAngleY;
compAngleX = accAngleX;
compAngleY = accAngleY;
}//end of calculateGyroOffsets Fcn
void readMPU() {
Wire.beginTransmission(MPU6050_ADDR);
Wire.write(ACCR_READ_START_ADDR); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU6050_ADDR, 14, true);
while (Wire.available() < 14) {
//wait until data is available
#ifdef DEBUG
Serial.println(F("MPU6050 waiting"));
#endif
}
accX = Wire.read() << 8 | Wire.read();
accY = Wire.read() << 8 | Wire.read();
accZ = Wire.read() << 8 | Wire.read();
tmp = Wire.read() << 8 | Wire.read();
gyroX = Wire.read() << 8 | Wire.read();
gyroY = Wire.read() << 8 | Wire.read();
gyroZ = Wire.read() << 8 | Wire.read();
//apply scale factor for gyro reading
rotX = gyroX / GYRO_SENSITIVITY_SCALE_FACTOR;
rotY = gyroY / GYRO_SENSITIVITY_SCALE_FACTOR;
rotZ = gyroZ / GYRO_SENSITIVITY_SCALE_FACTOR;
}//end of readGyroX Fcn
/*
This function will apply complementary filer to gyro angular rates and
assign them as PID inputs
*/
void calculateRotationRates() {
//apply the offset
rotX = rotX - gyroOffsetValX;
rotY = rotY - gyroOffsetValY;
rotZ = rotZ - gyroOffsetValZ;
//complemenrary filter for gyro readings
pidPitchRateIn = (pidPitchRateIn * 0.8) + (rotX * 0.2);
pidRollRateIn = (pidRollRateIn * 0.8) + (rotY * 0.2);
pidYawRateIn = (pidYawRateIn * 0.8) + (rotZ * 0.2);
}//end of calculateRotationRates Fcn
/*
This Fcn will calculate the angles using complementary filter
*/
void calculateAngles() {
//angles from accr
accAngleY = atan(accX / sqrt(pow(accY, 2) + pow(accZ, 2))) * radToDegreeConvert;
accAngleX = atan(accY / sqrt(pow(accX, 2) + pow(accZ, 2))) * radToDegreeConvert;
//apply offsets to accr values
accAngleX = accAngleX - accrOffsetValX;
accAngleY = accAngleY - accrOffsetValY;
//double dt = 0.004 = 1/250
//The Mighty Complementary filter
compAngleX = 0.98 * (compAngleX + rotX * 0.004) + 0.02 * accAngleX;
compAngleY = 0.98 * (compAngleY + rotY * 0.004) + 0.02 * accAngleY;
// assigning pitch and roll
pitchAngle = compAngleX;
rollAngle = compAngleY;
// //if imu yawed convert roll to pitch //0.004 = 1/250
//copied from ymfc-al code
pitchAngle = pitchAngle - rollAngle * sin(rotZ * 0.004 * ((3.142 * PI) / 180));
rollAngle = rollAngle + pitchAngle * sin(rotZ * 0.004 * ((3.142 * PI) / 180));
}//end of calculateAngles Fcn
//get register values from mpu6050 and calculate angles based on complementary filter
void updateAnglesFromMPU() {
readMPU();
calculateRotationRates();
calculateAngles();
}//end of updateAnglesFromMPU Fcn
void configureMPU() {
//Power Register
Wire.beginTransmission(MPU6050_ADDR);
Wire.write(PWR_REG_ADDR);//Access the power register
Wire.write(0b00000000);//check datasheet
Wire.endTransmission();
//Gyro Config
Wire.beginTransmission(MPU6050_ADDR);
Wire.write(ACCR_CONFIG_REG_ADDR);
Wire.write(ACCR_CONFIG_REG_VALUE);//check data sheet for more info
Wire.endTransmission();
//Accr Config
Wire.beginTransmission(MPU6050_ADDR);
Wire.write(GYRO_CONFIG_REG_ADDR);
Wire.write(GYRO_CONFIG_REG_VALUE);//check datasheet for more info
Wire.endTransmission();
}//end of setUpMPU Fcn
void computePID() {
//input updated in updateAngles Fcn
//update SetPoint according to throttle
setPointUpdate();
//calculate errors
pitchError = pidPitchRateIn - pidPitchRateSetPoint;
rollError = pidRollRateIn - pidRollRateSetPoint;
yawError = pidYawRateIn - pidYawRateSetPoint;
//calculate sum of errors
pitchErrorSum = pitchError + pitchPrevError;
rollErrorSum = rollError + rollPrevError;
yawErrorSum = yawError + yawPrevError;
//calculate delta of errors
pitchErrorDelta = pitchError - pitchPrevError;
rollErrorDelta = rollError - rollPrevError;
yawErrorDelta = yawError - yawPrevError;
//save current error as prev error for next iteration
pitchPrevError = pitchError;
rollPrevError = rollError;
yawPrevError = yawError;
//PID Calculation
pidPitchRateOut = (pitchError * pitchRatePGain) + (pitchErrorSum * pitchRateIGain) + (pitchErrorDelta * pitchRateDGain);
pidRollRateOut = (rollError * rollRatePGain) + (rollErrorSum * rollRateIGain) + (rollErrorDelta * rollRateDGain);
pidYawRateOut = (yawError * yawRatePGain) + (yawErrorSum * yawRateIGain) + (yawErrorDelta * yawRateDGain);
if (pidPitchRateOut > pidMax) pidPitchRateOut = pidMax;
else if (pidPitchRateOut < -pidMax) pidPitchRateOut = pidMax * -1;
if (pidRollRateOut > pidMax) pidRollRateOut = pidMax;
else if (pidRollRateOut < -pidMax) pidRollRateOut = pidMax * -1;
if (pidYawRateOut > pidMax) pidYawRateOut = pidMax;
else if (pidYawRateOut < -pidMax ) pidYawRateOut = pidMax * -1;
}//end of computePID Fcn
void resetPID() {
}//end of resetPID Fcn
void sendBTOutput() {
if (millis() - btDataStartMillis > DATA_INTERVAL) {
btDataStartMillis = millis();
Serial.print(F("y")); Serial.print(pidYawRateIn); Serial.print(F(">"));
Serial.print(F("p")); Serial.print(pitchAngle); Serial.print(F(">"));
Serial.print(F("r")); Serial.print(rollAngle); Serial.print(F(">"));
Serial.print(F("ps")); Serial.print(pidPitchRateSetPoint); Serial.print(F(">"));
Serial.print(F("rs")); Serial.print(pidRollRateSetPoint); Serial.print(F(">"));
Serial.print(F("ys")); Serial.print(pidYawRateSetPoint); Serial.print(F(">"));
Serial.print(F("po")); Serial.print(pidPitchRateOut); Serial.print(F(">"));
Serial.print(F("ro")); Serial.print(pidRollRateOut); Serial.print(F(">"));
Serial.print(F("yo")); Serial.print(pidYawRateOut); Serial.print(F(">"));
Serial.print(F("m0-")); Serial.print(m0Value); Serial.print(F(">"));
Serial.print(F("m1-")); Serial.print(m1Value); Serial.print(F(">"));
Serial.print(F("m2-")); Serial.print(m2Value); Serial.print(F(">"));
Serial.print(F("m3-")); Serial.print(m3Value); Serial.print(F(">"));
Serial.print(F("c0")); Serial.print(pwmDuration[0]); Serial.print(F(">"));
Serial.print(F("c1")); Serial.print(pwmDuration[1]); Serial.print(F(">"));
Serial.print(F("c2")); Serial.print(pwmDuration[2]); Serial.print(F(">"));
Serial.print(F("c3")); Serial.print(pwmDuration[3]); Serial.print(F(">"));
Serial.println("");
}//end of IF
}//end of sendBTOutput Fcn
void setup() {
pinMode(13, OUTPUT);
digitalWrite(13, LOW); //used to indicate FC is ready
initReceiver();
#ifdef DEBUG
Serial.begin(115200); //HC-05 is using hardware serial, 38400 is HC-05 default baud rate
btDataStartMillis = millis();
Serial.println("DEBUG mode ON");
#endif
initMPU6050();
initPID();
initializeMotors();
//prevTime = millis(); //used to calculate dt for gyro integration
loopTimer = micros();//used to keep track of loop time
digitalWrite(13, HIGH); //FC is Ready
}//end of setup Fcn
void loop() {
updateAnglesFromMPU();
if (armMotors == true) {
computePID();
updateMotors();
}//end of armMotors
#ifdef DEBUG
sendBTOutput();
#endif
}//end of loop Fcn
Edit :
These are the directions for angle and angular rate
Roll and Roll Rate directions are inverted.
Angle
Roll
Right Down --> -ve
Left Down --> +ve
Pitch
Nose Up --> +ve
Node Down --> -ve
Angular Rate
Roll
Right Down --> + ve
Left Down --> - ve
Pitch
Nose Up --> + ve
Nose Down --> -ve
are these directions are ok ? Or DId I mess up something in the code ?
Changing
rotY = rotY - gyroOffsetValY;
to
rotY = rotY - gyroOffsetValY;
rotY = -1 * rotY
seems to solve this problem for me
Related
Im using the sensor MPU6050 to control movements of my robotic arm. The codes work fine when it is a standalone program but i keep encountering 'FIFO overflow' when the codes are complied into the main program. This is the code that i am using.
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
RF24 radio(9, 8); // CE, CSN
const byte address[6] = "00001";
const int AccReadings = 10;
//Wrist Roll
int DataX[AccReadings];
int WRIndex = 0;
int WRtotal = 0;
int WRaverage = 0;
//Wrist Pitch
int DataY[AccReadings];
int WPIndex = 0;
int WPtotal = 0;
int WPaverage = 0;
//Shoulder Lift
int DataY2[AccReadings];
int SLIndex = 0;
int SLtotal = 0;
int SLaverage = 0;
//Elbow Lift
int ELaverage = 0;
//Arm Rotation
int ARaverage = 0;
float correct;
#define OUTPUT_READABLE_YAWPITCHROLL
#define INTERRUPT_PIN 2
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
struct Sensor_Data
{
int WristRoll;
int WristPitch;
int ShoulderLift;
int ElbowLift;
int ArmRotation;
};
Sensor_Data data;
//Interrupt Detection
volatile bool mpuInterrupt = false;
void dmpDataReady()
{
mpuInterrupt = true;
}
void setup()
{
radio.begin();
radio.openWritingPipe(address);
radio.setPALevel(RF24_PA_MIN);
radio.stopListening();
//Zero-fill Arrays
for (int i = 0; i < AccReadings; i++)
{
DataX[i] = 0;
}
for (int j = 0; j < AccReadings; j++)
{
DataY[j] = 0;
}
for (int k = 0; k < AccReadings; k++)
{
DataY2[k] = 0;
}
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000);
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
Serial.begin(115200);
while (!Serial);
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);
devStatus = mpu.dmpInitialize();
mpu.setXGyroOffset(49);
mpu.setYGyroOffset(-18);
mpu.setZGyroOffset(9);
mpu.setZAccelOffset(4427);
if (devStatus == 0)
{
mpu.setDMPEnabled(true);
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
dmpReady = true;
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(")"));
}
}
void loop()
{
/*smoothWR();
movementWR();
smoothWP();
movementWP();
smoothSL();
movementSL();*/
ElbowMovement();
radio.write(&data, sizeof(Sensor_Data));
}
void smoothWR()
{
WRtotal = WRtotal - DataX[WRIndex];
DataX[WRIndex] = analogRead(A0);
WRtotal = WRtotal + DataX[WRIndex];
WRIndex = WRIndex + 1;
if (WRIndex >= AccReadings)
{
WRIndex = 0;
}
WRaverage = WRtotal / AccReadings;
//Serial.println(WRaverage);
}
void movementWR()
{
WRaverage = map(WRaverage, 278, 419, 0, 180);
data.WristRoll = constrain(WRaverage, 0, 180);
//Serial.println(data.WristRoll);
}
void smoothWP()
{
WPtotal = WPtotal - DataY[WPIndex];
DataY[WPIndex] = analogRead(A1);
WPtotal = WPtotal + DataY[WPIndex];
WPIndex = WPIndex + 1;
if (WPIndex >= AccReadings)
{
WPIndex = 0;
}
WPaverage = WPtotal / AccReadings;
//Serial.println(WPaverage);
}
void movementWP()
{
WPaverage = map(WPaverage, 280, 421, 0 , 135);
data.WristPitch = constrain(WPaverage, 0, 135);
//Serial.println(data.WristPitch);
}
void smoothSL()
{
SLtotal = SLtotal - DataY2[SLIndex];
DataY2[SLIndex] = analogRead(A2);
SLtotal = SLtotal + DataY2[SLIndex];
SLIndex = SLIndex + 1;
if (SLIndex >= AccReadings)
{
SLIndex = 0;
}
SLaverage = SLtotal / AccReadings;
//Serial.println(SLaverage);
}
void movementSL()
{
SLaverage = map(SLaverage, 410, 270, 0 , 180);
data.ShoulderLift = constrain(SLaverage, 35, 180);
//Serial.println(data.ShoulderLift);
}
void ElbowMovement()
{
// 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)
{
if (mpuInterrupt && fifoCount < packetSize)
{
// try to get out of the infinite loop
fifoCount = mpu.getFIFOCount();
}
}
// 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 & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024)
{
// reset so we can continue cleanly
mpu.resetFIFO();
fifoCount = mpu.getFIFOCount();
Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)
}
else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT))
{
// 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;
// Get Yaw, Pitch and Roll values
#ifdef OUTPUT_READABLE_YAWPITCHROLL
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
// Yaw, Pitch, Roll values - Radians to degrees
ypr[0] = ypr[0] * 180 / M_PI;
ypr[1] = ypr[1] * 180 / M_PI;
ypr[2] = ypr[2] * 180 / M_PI;
// Skip 300 readings (self-calibration process)
if (int l = 0; l <= 300) {
correct = ypr[0]; // Yaw starts at random value, so we capture last value after 300 readings
l++;
}
// After 300 readings
else {
ypr[0] = ypr[0] - correct; // Set the Yaw to 0 deg - subtract the last random Yaw value from the currrent value to make the Yaw 0 degrees
// Map the values of the MPU6050 sensor from -90 to 90 to values suatable for the servo control from 0 to 180
ELaverage = map(ypr[0], -90, 90, 0, 180);
data.ElbowLift = constrain(ELaverage, 30, 110);
ARaverage = map(ypr[1], -90, 90, 0, 180);
data.ArmRotation = constrain(ARaverage, 0, 180);
//Serial.println(data.ElbowLift);
Serial.println(ypr[1]);
}
#endif
}
}
Is there any ways to get rid of the FIFO overflow? Also When i tried to used Jeff Rowberg's example codes MPU6050_DMP6 the program will freeze after a few seconds. Is there any solution to that? These are the example codes that i am using.
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
float correct;
int j = 0;
#define OUTPUT_READABLE_YAWPITCHROLL
#define INTERRUPT_PIN 2
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
//Interrupt Detection
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();
Wire.setClock(400000);
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
Serial.begin(38400);
while (!Serial);
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);
devStatus = mpu.dmpInitialize();
mpu.setXGyroOffset(17);
mpu.setYGyroOffset(-69);
mpu.setZGyroOffset(27);
mpu.setZAccelOffset(1551);
if (devStatus == 0)
{
mpu.setDMPEnabled(true);
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
dmpReady = true;
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(")"));
}
}
void loop()
{
if (!dmpReady) return;
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer))
{
#ifdef OUTPUT_READABLE_YAWPITCHROLL
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
if (j <= 300)
{
correct = ypr[0]; // Yaw starts at random value, so we capture last value after 300 readings
j++;
}
else
{
ypr[0] = ypr[0] - correct;
Serial.print("ypr\t");
Serial.print(ypr[0] * 180/M_PI);
Serial.print("\t");
Serial.print(ypr[1] * 180/M_PI);
Serial.print("\t");
Serial.println(ypr[2] * 180/M_PI);
}
#endif
}
}
You're using DMP (Digital Motion Processor) with mean calculation running on the MPU itself, this gives more precise and less CPU consumption but you need to update the FIFO frequently or the track goes wrong.
Encountering 'FIFO overflow' means your loop code was too slow, you should increase the speed of another task in your loop code. Or just use other code that not use the DMP.
I've been working on my doppler radar speed project for a while. I found this very helpful link and the code below:
// Based on the Adafruit Trinket Sound-Reactive LED Color Organ
// http://learn.adafruit.com/trinket-sound-reactive-led-color-organ/code
#define RADAR A5 // RADAR inut is attached to A7
#define MICRODELAY 100 // 100microseconds ~10000hz
#define MAXINDEX 1024 // 10 bits
#define TOPINDEX 1023 // 10 bits
byte collect[MAXINDEX];
int mean;
int minimum;
int maximum;
int hysteresis; // 1/16 of max-min
bool currentphase; // are value above mean + hysteresis;
int lastnull; // index for last null passing value
int prevnull; // index for previous null passing value
int deltaindex;
int deltadeltaindex;
int index;
bool phasechange = false;
void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
while (!Serial) {}
index = 0;
mean = 0;
maximum = 255;
minimum = 0;
hysteresis = 0;
currentphase = false;
lastnull = 0;
prevnull = 0;
Serial.print("deltadeltaindex");
Serial.print("\t");
Serial.print("deltaindex");
Serial.print("\t");
Serial.println("delta");
}
void loop() {
int newVal = analogRead(RADAR); // Raw reading from amplified radar
mean -= (collect[index] >> 2);
mean += (newVal >> 2);
collect[index]= newVal;
minimum = newVal < minimum ? newVal : minimum + 1;
maximum = newVal > maximum ? newVal : maximum - 1;
hysteresis = abs(maximum - minimum) >> 5;
if(newVal > (mean + hysteresis))
{
if(false == currentphase)
{
currentphase = true;
phasechange = true;
}
}
else if(newVal < (mean - hysteresis))
{
if(currentphase)
{
currentphase = false;
phasechange = true;
}
}
if(phasechange)
{
prevnull = lastnull;
lastnull = index;
int delta = (prevnull > lastnull) ?
(lastnull - prevnull + MAXINDEX) :
(lastnull - prevnull);
deltadeltaindex = abs(deltaindex - delta);
deltaindex = delta;
Serial.print(deltadeltaindex);
Serial.print("\t");
Serial.print(deltaindex);
Serial.print("\t");
Serial.println(delta);
}
index = index == TOPINDEX ? 0 : index + 1;
phasechange = false;
//delayMicroseconds(10);
}
I tried it out on my Arduino with HB100(model with breakout board), and it works just fine.
However, what I really wanted to do was to understand the mechanism behind the code. I read some articles on hysteresis and bit-shifting, but I simply cannot understand why the programmer here used bit-shifting.
What would mean -= (collect[index] >> 2); and mean += (newVal >> 2); do to the values exactly?
Help will be appreciated.
I am trying to control a few (8 for now) servo motors using this 16-channel board. I am running to some issues about accuracy, for example, when moving a couple of motors do draw a diagonal line, because of the delay between each servo, each motor will move in different timing resulting in incorrect drawings.
I am not sure about how to drive the motors in the fastest way in therms of code.
Where to set delays, the baud rate settings for this application, etc. I couldn't find a good example using all channels with minimum delay. In my case, messages are coming from serial, as explained in the code comment.
Is this the right way to drive this board channels?
I am using an arduino uno, but I would like to check if using a Teensy 3.2 results in best performances for this application.
Thanks in advance for any suggestions.
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
//#define SERVOMIN 150
//#define SERVOMAX 600
// temporary setting pins for 4 lights - it will be controlled by some decade counter...
//#define L1 4
//#define L2 7
//#define L3 8
//#define L4 10
#define L1 9
#define L2 10
#define L3 11
#define L4 12
/*
* a "pointer" device includes a light and 2 servos. Parameters from serial are:
* index,light,servo1,servo2; <- parameters separated by ',' end of pointer is ';'
*
* example of how serial is coming containing instructions for 4 pointers;
0,0,180,180;1,0,0,0;2,0,180,180;3,0,0,0;
0,0,90,90;1,0,90,90;2,0,90,90;3,0,90,90;
**most of the time these instructions doesn't come all for 4 pointers.
ex:
1,0,12,12;4,255,100,100;
**sometimes it comes only id and light parameter.
0,255;1,0;
(instructions only to turn light on/off)
*/
//values for 8 servos:
const uint8_t SERVOMIN[] = {150, 130, 150, 130, 150, 130, 150, 130};
const uint8_t SERVOMAX[] = {600, 500, 600, 500, 600, 500, 600, 500};
//boards (for now, only one board = 16 servos)
Adafruit_PWMServoDriver pwm [] = {
Adafruit_PWMServoDriver(0x40)
};
uint8_t servonum = 0;
uint8_t activeServos = 4; //not being used now
char buf [4]; //maybe too long
uint16_t currentPointer [4]; //index//light//servo1//servo2
byte lightPin [4] = {L1, L2, L3, L4};
uint8_t lightstatus [4] = {0, 0, 0, 0};
//debug
String inputString = ""; // a string to hold incoming data
boolean stringComplete = false; // whether the string is complete
boolean feedback = false;
void setup() {
//temporally as digital outputs
pinMode(L1, OUTPUT);
pinMode(L2, OUTPUT);
pinMode(L3, OUTPUT);
pinMode(L4, OUTPUT);
Serial.begin(115200);//230400 //115200 //57600 //38400 ?
for ( uint8_t i = 0; i < sizeof(pwm); i++) {
pwm[i].begin();
pwm[i].setPWMFreq(60);
}
}
void loop() {
reply();
}
void reply() {
if (stringComplete) {
if (feedback) Serial.println(inputString);
// clear the string:
inputString = "";
stringComplete = false;
for ( int i = 0; i < sizeof(buf); ++i ) buf[i] = (char)0;
}
}
void serialEvent() {
static byte ndx = 0;
static int s = 0;
while (Serial.available()) {
char rc = (char)Serial.read();
inputString += rc;
//(2) setting pointer parameter
if ( rc == ',') {
setPointer(s);
s++;
for ( int i = 0; i < sizeof(buf); ++i ) buf[i] = (char)0;
ndx = 0;
}
//(3) end of this pointer instruction
else if (rc == ';') {
setPointer(s);
//executePointer(); //plan B
ndx = 0;
s = 0;
for ( int i = 0; i < sizeof(buf); ++i ) buf[i] = (char)0;
}
//(4) end of command line
else if (rc == '\n') {
//p = 0;
s = 0;
stringComplete = true;
}
//(1) buffering
else {
buf[ndx] = rc;
ndx++;
}
}
}
void setPointer(int s) {
//index//light//servo1//servo2
int value;
value = atoi(buf);
//index
if (s == 0) {
if (feedback) {
Serial.print("index:");
Serial.print(value);
Serial.print(", buf:");
Serial.println(buf);
}
currentPointer[0] = value;
}
//light
else if (s == 1) {
int index = currentPointer[0];
currentPointer[s] = value;
//Serial.println(index);
digitalWrite(lightPin[index], (value > 0) ? HIGH : LOW);
// analogWrite( lightPin[currentPointer[0]], currentPointer[1]); // implement later
if (feedback) {
Serial.print("light: ");
Serial.println(value);
}
//servos
} else {
int index = currentPointer[0];
if (feedback) {
Serial.print("servo ");
Serial.print(index * 2 + s - 2);
Serial.print(": ");
Serial.println(value);
}
uint16_t pulselen = map(value, 0, 180, SERVOMIN[index], SERVOMAX[index]);
currentPointer[s] = pulselen;
pwm[0].setPWM(index * 2 + (s - 2), 0, pulselen); //current pointer id * 2 + s (s is 2 or 3)
//delay(20);
}
}
// this was plan B - not using
void executePointer() {
int index = currentPointer[0];
analogWrite( lightPin[index], currentPointer[1]);
pwm[0].setPWM(index * 2, 0, currentPointer[2]);
pwm[0].setPWM(index * 2 + 1, 0, currentPointer[3]);
delay(20);
}
I'm getting wrong output on the pins 9, 10 and 11. They are meant to be inputs for my RGB strip driver circuit. Which is basically an array of NPN transistors pulling current from the strip.
The basic idea is to get only 2 controls to set R, G, B and brightness. I'm using a button and a potenciometer. Potenciometer is used to set the value and the button to skip to next value setting. There is one setting state which is like the default one. It is the one to set the brightness and I will be using this most of the time. The othe ones are for setting the colors and when in one of those setting the strip will be blinking in between only the color I'm currently setting and the result with alle three colors set. The whole code was working just fine until I added the brightness setting and I think that I am kinda lost in my own code.
I even added a serial to read the outputs but I don't understand why are the numbers the way they are :(
int pinR = 9;
int pinG = 10;
int pinB = 11;
int potPin = A0;
const int buttonPin = 2;
int brightR = 0;
int brightG = 0;
int brightB = 0;
int brightness = 50; //
int R;
int G;
int B;
int potValue = 0;
int blinky = 0;
boolean blinking = false;
int buttonState;
int lastButtonState = LOW;
long lastDebounceTime = 0;
long debounceDelay = 50;
int setting = 0; //0=R 1=G 2=B 3=Brightness
void setup() {
// put your setup code here, to run once:
pinMode(pinR, OUTPUT);
pinMode(pinG, OUTPUT);
pinMode(pinB, OUTPUT);
Serial.begin(9600);
}
void RGBset(int r, int g, int b){
analogWrite(pinR, r);
analogWrite(pinG, g);
analogWrite(pinB, b);
}
void loop() {
// put your main code here, to run repeatedly:
potValue = analogRead(potPin);
potValue = map(potValue, 0, 1023, 0, 255); //read pot --> map to values from 0 - 255
int reading = digitalRead(buttonPin);
if (reading != lastButtonState) {
lastDebounceTime = millis();
}
if ((millis() - lastDebounceTime) > debounceDelay) {
if (reading != buttonState) {
buttonState = reading;
if (buttonState == HIGH) {
setting++;
}
}
}
lastButtonState = reading;
if(setting > 3){ // 0=R 1=G 2=B 3=Brightness
setting = 0; // cant get 4 cause there is no 4
}
if(setting == 0){
brightR = potValue;
if(blinking){
RGBset(brightR, brightG, brightB);
}else{
RGBset(brightR, 0, 0);
}
}
if(setting == 1){
brightG = potValue;
if(blinking){
RGBset(brightR, brightG, brightB);
}else{
RGBset(0, brightG, 0);
}
}
if(setting == 2){
brightB = potValue;
if(blinking){
RGBset(brightR, brightG, brightB);
}else{
RGBset(0, 0, brightB);
}
}
if(setting == 3){
brightness = potValue;
brightness = map(brightness, 0, 255, 1, 100); //mapping brightness to values from 1 - 100
R = brightR * brightness / 100; //set value * brightness / 100
G = brightG * brightness / 100; //that leads to get % of set value
B = brightB * brightness / 100; //255 * 50 / 100 = 127,5 ==> 128
RGBset(R, G, B); //it wont blink in thiss setting
}
if(setting != 3){
blinky++;
if(blinky > 1000){
blinking = !blinking;
blinky = 0;
}
}
String output = (String(brightR) + " " + String(R) + " " + String(brightG) + " " + String(G) + " " + String(brightB) + " " + String(B) + " " + String(brightness) + " " + String(potValue) + " " + String(blinking));
Serial.println(output);
delay(1);
}
First, in setup:
pinMode(buttonPin , INPUT);
Second, what are you expected when setting==3? You aren't reloading/updating the variables for brightR brightG brightB. So, when you change setting, you will lost the change of brightness
I am trying to make a Arduino program where it receives signals from the Serial monitor and then lights LED's accordingly, i have it set up with RGB. But I have a problem where all three LED's light like the blue one only should. here is my code:
#define SENSOR 0
#define R_LED 11
#define G_LED 10
#define B_LED 9
#define BUTTON 12
int val = 0;
int times = 0;
int btn = LOW;
int old_btn = LOW;
int state = 0;
int r = 0;
int g = 0;
int b = 0;
byte inByte = 0;
char buffer[5];
void setup() {
Serial.begin(9600);
pinMode(BUTTON, INPUT);
}
void loop() {
val = analogRead(SENSOR);
Serial.println(val);
if (Serial.available() > 0) {
inByte = Serial.read();
if (inByte == '#') {
r = Serial.read() + 1;
r = r * 25;
g = Serial.read() + 1;
g = g * 25;
b = Serial.read() + 1;
b = b * 25;
}
}
btn = digitalRead(BUTTON);
if ((btn == HIGH) && (old_btn == LOW)){
state = 1 - state;
}
old_btn = btn;
if (state == 1){
analogWrite(R_LED, r);
analogWrite(G_LED, g);
analogWrite(B_LED, b);
}else{
analogWrite(R_LED, 0);
analogWrite(G_LED, 0);
analogWrite(B_LED, 0);
}
delay(100);
}
Note: I am sure it is a coding issue, not a mechanical one,for your information.
If I understand what you're doing correctly, some code such as #987 will be sent on the serial monitor every time you want the color to change, correct? In that case, the issue is that when you do r = Serial.read() + 1; (and the same goes for b and g), Serial.read() is returning the ASCII code for a character from '0' through '9' (48 - 57). Try this:
r = Serial.read() - '0' + 1;
r = r * 25;
g = Serial.read() - '0' + 1;
g = g * 25;
b = Serial.read() - '0' + 1;
b = b * 25;
This will subtract the ASCII code for '0' first, so the numbers will be in the proper 0-9 range.
In your code, you're doing the identical thing to all three lights.
Why would you expect blue to behave differently?
analogWrite(R_LED, r);
analogWrite(G_LED, g);
analogWrite(B_LED, b);