#include <Arduino.h>
#include <avr/wdt.h>
#include <PID_v1.h>
#include <LMotorController.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#define MANUAL_TUNING 1
#define MOVE_BACK_FORTH 1
double MIN_ABS_SPEED=15;
String id = ""; // a string to hold incoming data
String stvalue = "";
double value;
int qq=0;
MPU6050 mpu; // 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
Quaternion q; // [w, x, y, z] quaternion container, orientation/motion vars
VectorFloat gravity; // [x, y, z] gravity vector
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
double kp=15 , ki=190, kd=0.6; //PID
double prevKp, prevKi, prevKd;
double originalSetpoint = 177;
double setpoint = originalSetpoint;
double movingAngleOffset = 3.3;
double input, output;
int moveState=0; //0 = balance; 1 = back; 2 = forth
#if MANUAL_TUNING
PID pid(&input, &output, &setpoint, 0, 0, 0, DIRECT);
#else
PID pid(&input, &output, &setpoint, 15, 195, 0.6, DIRECT);
#endif
//MOTOR CONTROLLER
int ENA = 6;
int IN1 = 11;
int IN2 = 12;
int IN3 = 10;
int IN4 = 9;
int ENB = 5;
LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, 1,1);
//timers
long time1Hz = 0;
long time5Hz = 0;
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady(){
mpuInterrupt = true;
}
void setup(){
wdt_disable();
pinMode(7,OUTPUT); //pin 7 on-off (motor enable)
digitalWrite(7,HIGH);
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE // join I2C bus (I2Cdev library doesn't do this automatically)
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(38400);
mpu.initialize();
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);
if (devStatus == 0){ // make sure it worked (returns 0 if so)
mpu.setDMPEnabled(true); // turn on the DMP, now that it's ready
attachInterrupt(0, dmpDataReady, RISING); // enable Arduino interrupt detection
mpuIntStatus = mpu.getIntStatus();
dmpReady = true; // set our DMP Ready flag so the main loop() function knows it's okay to use it
packetSize = mpu.dmpGetFIFOPacketSize(); // get expected DMP packet size for later comparison
pid.SetMode(AUTOMATIC); //setup PID
pid.SetSampleTime(5);
pid.SetOutputLimits(-255, 255);
}
}
void loop(){
#if MOVE_BACK_FORTH
moveBackForth();
#endif
if (!dmpReady) return; // if programming failed, don't try to do anything
while (!mpuInterrupt && fifoCount < packetSize){ // wait for MPU interrupt or extra packet(s) available,no mpu data - performing PID calculations and output to motors
if(moveState==3){ //right side
setpoint = originalSetpoint - 1;
pid.Compute();
motorController.turnLeft(output, 120);
delay(1);
}
else if(moveState==4){ // left side
setpoint = originalSetpoint + 1;
pid.Compute();
motorController.turnRight(output, 120);
delay(1);
}
else if(moveState==0||moveState==1||moveState==2){
pid.Compute();
motorController.move(output, MIN_ABS_SPEED);
delay(1);
}
unsigned long currentMillis = millis();
if (currentMillis - time1Hz >= 1000){
loopAt1Hz();
time1Hz = currentMillis;
}
}
mpuInterrupt = false; // reset interrupt flag and get INT_STATUS byte
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount(); // get current FIFO count
if ((mpuIntStatus & 0x10) || fifoCount == 1024){ // check for overflow (this should never happen unless our code is too inefficient)
mpu.resetFIFO(); // reset so we can continue cleanly
} // otherwise, check for DMP data ready interrupt (this should happen frequently)
else if (mpuIntStatus & 0x02){
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); // wait for correct available data length, should be a VERY short wait
mpu.getFIFOBytes(fifoBuffer, packetSize); // read a packet from FIFO, track FIFO count here in case there is > 1 packet available
fifoCount -= packetSize; // (this lets us immediately read more without waiting for an interrupt)
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
input = ypr[1] * 180/M_PI + 180;
}
} // end of the loop
void loopAt1Hz(){
#if MANUAL_TUNING
setPIDTuningValues();
#endif
}
void moveBackForth(){ //move back and forth
if (moveState == 0)
setpoint = originalSetpoint;
else if (moveState == 1)
setpoint = originalSetpoint - movingAngleOffset;
else if (moveState == 2)
setpoint = originalSetpoint + movingAngleOffset;
}
#if MANUAL_TUNING //PID Tuning (3 potentiometers)
void setPIDTuningValues(){
pid.SetTunings(kp, ki, kd);
prevKp = kp; prevKi = ki; prevKd = kd;
}
#endif
/*
SerialEvent occurs whenever a new data comes in the
hardware serial RX. This routine is run between each
time loop() runs, so using delay inside loop can delay
response. Multiple bytes of data may be available.
*/
void serialEvent() {
while (Serial.available()) {
char inChar = Serial.read();
if (inChar == '#') {
if(id=="Kp"){
kp=value;
}
if(id=="Ki"){
ki=value;
}
if(id=="Kd"){
kd=value;
}
if(id=="AS"){
MIN_ABS_SPEED=value;
}
if(id=="on"){
if(value==1){
wdt_enable(WDTO_15MS);
delay(20);
wdt_reset();
}
else{
digitalWrite(7,LOW);
} }
if(id=="mv"){
int mvs;
mvs=value;
moveState=mvs;
}
if(id=="tn"){
int mvs;
mvs=value;
if(value==1){ }
else{
kp=15;
ki=195;
kd=0.6;
MIN_ABS_SPEED=30;
}}
id = "";
stvalue = "";
qq=0;
break;
}
if(qq<2){
id+=inChar;
}
else{
stvalue+=inChar;
}
value=stvalue.toFloat();
qq++;
}}
