johncc:
What do you mean by this, it stopped working altogether? If you post your sketch it would be helpful...
as you see on the picture the OUT value goes down to zero and the minimum value should be 1, but it doesn't do that everytime i run the program, but sometimes is does.
The code is a bit messy but here you go
and i deleted the valueread function because it was to many characters in the reply if i had that in
//Include Servo.h for servo motors
#include <Servo.h>
//Include regulator lib
#include <PID_v1.h>
//Include gyro stuff
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu;
#define OUTPUT_READABLE_YAWPITCHROLL
#define OUTPUT_READABLE_REALACCEL
//#define OUTPUT_READABLE_WORLDACCEL
//Gyro stuff
// 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 ROUTINE ===
// ================================================================
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
//Declare all motors
Servo motorfront;
Servo motorback;
Servo motorright;
Servo motorleft;
//Variables
int braap;
double pitch;
double roll;
double pospitchkvot;
double negpitchkvot;
double firstnegpitchkvot;
double posrollkvot;
double negrollkvot;
double pitchkvot;
double notpitchkvot;
double rollkvot;
double notrollkvot;
double height;
double heightfilter;
double heightout;
double heightoutf;
double SPheight=4280;
double mfront;
double mback;
double mright;
double mleft;
//Declare regulators
PID pospitchPID(&pitch, &pospitchkvot, 0, 1, 0, 0, REVERSE);
PID negpitchPID(&pitch, &firstnegpitchkvot, 0, 1, 0, 0, DIRECT);
PID posrollPID(&roll, &posrollkvot, 0, 0.002, 0, 0, DIRECT);
PID negrollPID(&roll, &negrollkvot, 0, 0.002, 0, 0, REVERSE);
PID heightPID(&heightfilter, &heightout, &SPheight, 0.002, 0, 0, DIRECT);
//Räkne sätt (1-X)+1=!X
void setup() {
//Start I2C and Serial
Wire.begin();
Serial.begin(115200);
while(!Serial);
// initialize
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();
// 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(")"));
error();
}
//Set up all motors
motorfront.attach(3);
motorback.attach(5);
motorright.attach(6);
motorleft.attach(9);
delay(2000);
//Set output limit so it'll calculate right
pospitchPID.SetOutputLimits(1,2);
negpitchPID.SetOutputLimits(0,1);
posrollPID.SetOutputLimits(1,2);
negrollPID.SetOutputLimits(0,1);
heightPID.SetOutputLimits(60,170);
//Set LED-pin as output
pinMode(13, OUTPUT);
//Call motor begin function to initiate the motors
Serial.println("MOTORS STARTING!!");
Motor_begin();
//Activate PID regulators
heightPID.SetMode(AUTOMATIC);
pospitchPID.SetMode(AUTOMATIC);
negpitchPID.SetMode(AUTOMATIC);
posrollPID.SetMode(AUTOMATIC);
negrollPID.SetMode(AUTOMATIC);
}
void loop() {
//Stop logging
if(Serial.available()){
char reead;
reead = Serial.read();
if(reead = 1){
while(0==0);
}
}
//Call function to get values from gyro+accelometer w/ filter
/*
int heightstorage[5];
for(int b;b<5;b++){
valueread(pitch, roll, height);
heightstorage[b] = height;
}
heightfilter = (heightstorage[0]+heightstorage[1]+heightstorage[2]+heightstorage[3]+heightstorage[4])/5;
*/
valueread(pitch, roll, height);
/*Serial.print("pitch=");
Serial.println(pitch);
Serial.print("roll=");
Serial.println(roll);*/
//height = height - 4200;
braap = braap++;
//Compute PID regulators
heightPID.Compute();
pospitchPID.Compute();
negpitchPID.Compute();
posrollPID.Compute();
negrollPID.Compute();
//Turn negative quota
//negpitchkvot = map_double(firstnegpitchkvot,0,1,1,0);
negpitchkvot = (firstnegpitchkvot - 0) * (0 - 1) / (1 - 0) + 1;
//Calculate quota
pitchkvot = pospitchkvot * negpitchkvot;
rollkvot = posrollkvot * negrollkvot;
notpitchkvot = (1 - pitchkvot) + 1;
notrollkvot = (1 - rollkvot) + 1;
//saftey variable
heightoutf = heightout;
//test
heightoutf = 80;
//Multiply quota and height speed
mfront = pitchkvot * heightoutf;
mback = notpitchkvot * heightoutf;
mright = rollkvot * heightoutf;
mleft = notrollkvot * heightoutf;
//Speed limit
/*
if(mfront>90)
{
mfront=90;
}
if(mback>90)
{
mback=90;
}
if(mright>90)
{
mright=90;
}
if(mleft>90)
{
mleft=90;
}
*/
//if(mfront > 90 || mback > 90 || mright > 90 || mleft > 90)
//{
// error();
//}
motorfront.write(mfront);
motorback.write(mback);
//motorright.write(mright);
//motorleft.write(mleft);
Serial.print(braap);
Serial.print(";");
Serial.print(pospitchkvot);
Serial.print(";");
Serial.print(negpitchkvot);
Serial.print(";");
Serial.print(mfront);
Serial.print(";");
Serial.print(mback);
Serial.print(";");
Serial.println(pitch);
}
void Motor_begin() {
//Show Motors are starting by blinking LED
digitalWrite(13, HIGH);
//Get motors to go to on mode
motorfront.write(60);
motorback.write(60);
motorright.write(60);
motorleft.write(60);
delay(500);
//Blink
digitalWrite(13, LOW);
delay(500);
//Blink
digitalWrite(13, HIGH);
//Get motors to go to drive mode
motorfront.write(70);
motorback.write(70);
motorright.write(70);
motorleft.write(70);
delay(500);
//Blink
digitalWrite(13, LOW);
delay(500);
//Blink
digitalWrite(13, HIGH);
//Test motors
for(int x=60; x<=80; x++)
{
motorfront.write(x);
motorback.write(x);
motorright.write(x);
motorleft.write(x);
delay(100);
}
//Blink
digitalWrite(13, LOW);
//Go back to 0 speed
motorfront.write(60);
motorback.write(60);
motorright.write(60);
motorleft.write(60);
delay(500);
//Blink
digitalWrite(13, HIGH);
delay(500);
//End of test, shut LED off
digitalWrite(13, LOW);
}
void error() {
while(0==0) {
digitalWrite(13, HIGH);
Serial.println("ERROR!!!!");
}
}
