//PID-kirjaston tuonti ja saatimen maarittely

#include <PID_v1.h>

//Define Variables we'll be connecting to

double Setpoint, Input, Output;

//Specify the links and initial tuning parameters

//parametrit muodossa (&Input, &Output, &Setpoint, Kp, Ki, Kd, Direction)
PID balancePID(&Input, &Output, &Setpoint,0,0,0, DIRECT); //LAITETAAN SAATOARVOIKSI
TASSA VAIHEESSA NOLLAT, MAARITETAAN ARVOT SETUPISSA

//PID-kirjasto tuotu ja saadin maaritelty

#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation

// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif

// class default I2C address is 0x68

// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high

// uncomment "OUTPUT_PITCH_PID" if you want to see only pitch angle

// (in degrees) calculated from the quaternions coming from the FIFO.
// Note that angles suffer from gimbal lock (for more info, see
// http://en.wikipedia.org/wiki/Gimbal_lock)
#define OUTPUT_PITCH_PID

//uncomment "OUTPUT_VOLTAGE" if you want to see the input voltage measured

//by arduino in both bits (as seen by the analog input, range 0-1023) and
millivolts.
#define OUTPUT_VOLTAGE

#define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards

#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
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

//omat muuttujat
float kallistuskulma;
double motor_r, motor_l, pwm_r, pwm_l, last_motor_r, last_motor_l;
float Kp, Ki, Kd;
char * newvalue;
float newvalues[3];

char saatoarvochar[17];
String saatoarvostring, Kp_new, Ki_new, Kd_new;

int phaseA1Last = LOW;

int n1 = LOW;

int analogInPin = A1;

int voltage_in;
float voltage;

//MAARITELLAAN MOOTTORIEN PINNIT

// connect motor controller pins to Arduino digital pins
// motor one
int enA = 6;
int in1 = 8;
int in2 = 7;
// motor two
int enB = 3;
int in3 = 5;
int in4 = 4;

//MAARITELLAAN ENKOODERIEN PINNIT HUOM HUOM HUOM HUOM !!!! ONE-TWO JAKO SAATTAA
MENNA PUOLITTAIN RISTIIN MOOTTORIEN KANSSA -> TARKISTA KUN OFFSETTAAT!
//encoder one
int encoder1Pos = 0;
int phaseA1 = 10;
int phaseB1 = 11;
//encoder two
int phaseA2 = 12;
int phaseB2 = 13;

// ================================================================
// === INTERRUPT DETECTION ROUTINE ===
// ================================================================

volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has
gone high
void dmpDataReady() {
mpuInterrupt = true;
}
// ================================================================
// === INITIAL SETUP ===
// ================================================================

void setup() {

{
// set all the motor control pins to outputs
pinMode(enA, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
}

// join I2C bus (I2Cdev library doesn't do this automatically)

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having
compilation difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif

// initialize serial communication

Serial.begin(115200);
while (!Serial); // wait for Leonardo enumeration, others continue immediately

// initialize device
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);

// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") :
F("MPU6050 connection failed"));

// 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(109);
mpu.setYGyroOffset(1);
mpu.setZGyroOffset(135);
mpu.setXAccelOffset(317); // These offset-values are values for this very
robot.
mpu.setYAccelOffset(2120); // The values have been calculated with a specific
calibration
mpu.setZAccelOffset(1107); // software in the MPU6050 library package.

// 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(digitalPinToInterrupt(INTERRUPT_PIN), 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);

//PID PID PID PID PID PID PID PID PID PID PID PID PID PID PID PID PID PID PID

Kp=20 ;
Ki=5 ; //TASSA SYOTETAAN SAATOARVOT JA PRINTATAAN NE SERIALIIN JOTTA PYSYTAAN
KARRYILLA
Kd=0.67 ;

balancePID.SetTunings(Kp, Ki, Kd);

Serial.print("Kp: "); Serial.print(Kp);
Serial.print(" Ki: "); Serial.print(Ki);
Serial.print(" Kd: "); Serial.println(Kd);

delay(1500); //VIIVE, JOTTA SAATOARVOT EHTII LUKEMAAN

//initialize the variables we're linked to

Input = kallistuskulma;
Setpoint = 0;

//turn the PID on

balancePID.SetOutputLimits(-255, 255);
balancePID.SetMode(AUTOMATIC);
//PID PID PID PID PID PID PID PID PID PID PID PID PID PID PID PID PID PID PID

last_motor_r=0;
last_motor_l=0;
}
// ================================================================
// === MAIN PROGRAM LOOP ===
// ================================================================

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) {
// other program behavior stuff here
// .
// .
// .
// if you are really paranoid you can frequently test in between other
// stuff to see if mpuInterrupt is true, and if so, "break;" from the
// while() loop to immediately process the MPU data
// .
// .
// .
}

// 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;

//LASKETAAN ROBOTIN KALLISTUSKULMA

kallistuskulma = ypr[1] * 180/M_PI;

// display Euler angles in degrees

mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

#ifdef OUTPUT_PITCH_PID
Serial.print("kallistuskulma: ");
 Serial.print(kallistuskulma);
#endif

// blink LED to indicate activity

blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);

//MITATAAN JANNITE
//TAHAN TAYTYIS KOODATA MOVINGAVERAGE-FILTTERI JOTTA VIRHEMITTAUKSET EI
SEKOTTAIS SAATOA
voltage_in = analogRead(analogInPin);
voltage = map(voltage_in, 0, 1023, 0, 4760)* 200/119;
#ifdef OUTPUT_VOLTAGE
//Serial.print(" voltage (bitteina): ");
//Serial.print(voltage_in);
Serial.print(" voltage (mV): ");
Serial.print(voltage);
#endif

//PID PID PID PID PID PID PID PID PID PID PID PID PID PID PID PID PID PID
PID

//UUSIEN SAATOARVOJEN (Kp, Ki, Kd) SYOTTAMINEN SARJALIIKENTEENA

if ( Serial.available() > 0 ) {
saatoarvostring = Serial.readString(); //Serialiin syotetty rivi muodossa
"Kp Ki Kd;" kahdella desimaalilla, esim "25.00 12.00 03.50;"
saatoarvostring.toCharArray(saatoarvochar, 17);
newvalue = strtok(saatoarvochar," ");
int i = 0;
while (saatoarvochar != NULL && i<=2) {
//Serial.println(newvalue);
newvalues[i] = atof(newvalue);
newvalue = (strtok(NULL," "));
//Serial.println(newvalues[i]);
i = i+1;
}

//SIJOITETAAN UUDET ARVOT PIDIIN

Kp = newvalues[0]; Ki = newvalues[1]; Kd = newvalues[2];
balancePID.SetTunings(Kp, Ki, Kd);

Serial.println("/");Serial.println("/");Serial.println("/");
Serial.println("////////////////////////////////////////////////");
Serial.println("SAATOARVOT PAIVITETTY: ");
Serial.print("Kp= "); Serial.println(balancePID.GetKp());
Serial.print("Ki= "); Serial.println(balancePID.GetKi());
Serial.print("Kd= "); Serial.println(balancePID.GetKd());
Serial.println("////////////////////////////////////////////////");
Serial.println("/");Serial.println("/");Serial.println("/");

analogWrite(enA, 0); analogWrite(enB, 0); //PYSAYTETAAN MOOTTORIT SAADON

AJAKSI KOSKA DELAY, JOTTA KUITTAUKSEN N�KEE
delay(3000);
}

//saatoarvorivi = " ";

 Input = kallistuskulma;
balancePID.Compute();

#ifdef OUTPUT_PITCH_PID
Serial.print(" PID-saatimen arvo: ");
Serial.println(Output); //Output valilla [ -255, +255 ]
#endif

//PID PID PID PID PID PID PID PID PID PID PID PID PID PID PID PID PID PID
PID

//luovutuskulma (kulma, jonka ylittyessa robotti ei enaa yrita suoristaa

itseaan)
if (abs(kallistuskulma) >= 45) {
Output = 0;
}

// //JANNITEKOMPENSAATIO
// if (voltage >= 7400) {
// Output = Output * ( 1-0.000125*(voltage-7400));
// }

// MOOTTOREILLE SUUNNAT JA NOPEUDET(PWM) OUTPUTISTA, R/L ERIKSEEN JOTTA

VALMIUS OFFSETILLE (ELI KAANTYMISELLE)
motor_l = Output;
motor_r = Output;

//maaritetaan moottorien suunnat

if (motor_l < 0) {digitalWrite(in1, LOW); digitalWrite(in2, HIGH);} //vasen
moottori eteen
else {digitalWrite(in1, HIGH); digitalWrite(in2, LOW);} //vasen
moottori taakse
if (motor_r < 0) {digitalWrite(in3, HIGH); digitalWrite(in4, LOW);} //oikea
moottori eteen
else {digitalWrite(in3, LOW); digitalWrite(in4, HIGH);} //oikea
moottori taakse

//SUUNNANVAIHTOPULSSI
if (last_motor_l * motor_l < 0) {pwm_l = 255;}
if (last_motor_r * motor_r < 0) {pwm_r = 255;}

//maaritetaan moottorien nopeudet (pwm, valill� 0-255)

pwm_l = abs(motor_l);
if (pwm_l > 10 && pwm_l < 70) {pwm_l=70;} //Moottori starttaa about 70:ssa,
joten mapataan v�lille 70-255
analogWrite(enA, pwm_l);
pwm_r = abs(motor_r);
if (pwm_r > 10 && pwm_r < 70) {pwm_r=70;} //Moottori starttaa about 70:ssa,
joten mapataan v�lille 70-255
analogWrite(enB, pwm_r);
//
// //LUETAAN ENKOODERI 1
// n1 = digitalRead(phaseA1);
// if ((phaseA1Last == LOW) && (n1 == HIGH)) {
// if (digitalRead(phaseB1) == LOW) {
// encoder1Pos--;
// } else {
// encoder1Pos++;
// }
// Serial.print("enkooderi: ");
// Serial.println(encoder1Pos);

//SUUNNANVAIHTOPULSSISYSTEEMIN MUUTTUJASIJOITUS
last_motor_l = motor_l;
last_motor_r = motor_r;

Serial.print("/");

delay(2); // TESTATAAN, VAKAUTTAISIKO PARIN MILLISEKUNNIN VIIVE SYSTEEMIA

}
}