//////////////////////////////////////////////////////////////////////////////MA

IN//////////////////////////////////////
//#include <Kalman.h>
// Rutine for repeat part of the code every X miliseconds
#define runEvery(t) for (static long _lasttime;\
(uint16_t)((uint16_t)millis() - _lasttime) >= (t);\
_lasttime += (t))
//Set up a timer Variable
uint32_t timer;
double InputRoll; // Roll angle value
double InitialRoll; // Roll initial angle
void setup() {
Serial.begin(9600);
timer = micros(); // Initialize timer

InitSensors(); // Initialize sensors

InitMotors(); // Initialize motors
delay(1000); // Wait until sensors be ready
InitialValues(); // Get the initial angle values
SetSetpoint(0); // Set the PID setpoint to 0
}
double RollAngle =0; // Roll angle variable
void loop() {
runEvery(5){ // Exetutes this part of the code every 10 miliseconds ->
100Hz
RollAngle = 0.95* (RollAngle + getDGyroRoll()) + 0.05* (getAccelRoll());
Serial.println(RollAngle);
timer = micros(); // Reset the timer
// Uncomment this for print data to Graphics
/* Serial.write((byte)57); // security value
Serial.write((byte)getAccelRoll()); // Accel Roll value
Serial.write((byte)getGyroRoll()); // Gyro Roll value
Serial.write((byte)(RollAngle-InitialRoll)); // Real Roll
*/
MotorControl(Compute(RollAngle-InitialRoll)); // Sends the real roll angle
-> Roll - InitialRoll
}
/* ReceiveData(); // Checks Serial for incoming data
}

////////////////////////////////////////////////////////////MOTOR///////////////
/////////////
// Motor Pins
int enablea = 2;
int enableb = 5;
int a1 = 3;
int a2 = 4;
int b1 = 6;
int b2 = 7;
// Prueba set point
double distancia = 0;
void InitMotors(){
// Set pins as outputs
pinMode(enablea, OUTPUT);
pinMode(enableb, OUTPUT);
pinMode(a1, OUTPUT);
pinMode(a2, OUTPUT);
pinMode(b1, OUTPUT);
pinMode(b2, OUTPUT);
// Set direction to none direction
digitalWrite(a1,HIGH);
digitalWrite(a2,HIGH);
digitalWrite(b1,HIGH);
digitalWrite(b2,HIGH);
}

void MotorControl(double out){

// Sets direction
if (out > 0){ // forward
digitalWrite(a1,HIGH);
digitalWrite(a2,LOW);
digitalWrite(b1,LOW);
digitalWrite(b2,HIGH);
}else{ // backward
digitalWrite(a1,LOW);
digitalWrite(a2,HIGH);
digitalWrite(b1,HIGH);
digitalWrite(b2,LOW);
}
byte vel = abs(out); // Absolute value of velocity

/////////////////////////////////////////////////////PID////////////////////////
//////////////
// PID variables
int outMax = 255;
int outMin = -255;
float lastInput = 0;
double ITerm =0;
// PID constants
// You can change this values to adjust the control
double kp = 100; // Proportional value
double ki = 10; // Integral value
double kd = 0; // Derivative value
double Setpoint = 0; // Initial setpoint is 0
// Calculates the PID output
double Compute(double input)
{
double error = Setpoint - input;
ITerm+= (ki * error);
if(ITerm > outMax) ITerm= outMax;
else if(ITerm < outMin) ITerm= outMin;
double dInput = (input - lastInput);
// Compute PID Output
double output = kp * error + ITerm + kd * dInput;
if(output > outMax) output = outMax;
else if(output < outMin) output = outMin;
// Remember some variables for next time
lastInput = input;
return output;
}

// Seters and geters for Setpoint

void SetSetpoint(double d){
Setpoint = d;
}
double GetSetPoint(){
return Setpoint;
}

///////////////////////////////////////////////////////////sensor///////////////
////////////
#include <Wire.h>
#include <TimerOne.h>
#define MPU9250_ADDRESS 0x68
#define MAG_ADDRESS 0x0C
#define GYRO_FULL_SCALE_250_DPS 0x00
#define GYRO_FULL_SCALE_500_DPS 0x08
#define GYRO_FULL_SCALE_1000_DPS 0x10
#define GYRO_FULL_SCALE_2000_DPS 0x18
#define ACC_FULL_SCALE_2_G 0x00
#define ACC_FULL_SCALE_4_G 0x08
#define ACC_FULL_SCALE_8_G 0x10
#define ACC_FULL_SCALE_16_G 0x18

//Set up accelerometer variables

float accBiasX, accBiasY, accBiasZ;
float accAngleX, accAngleY;
//double accRoll = 0;
//Set up gyroscope variables
float gyroBiasX, gyroBiasY, gyroBiasZ;
float gyroRateX, gyroRateY, gyroRateZ;
float gyroRoll = 0;
//double gyro_sensitivity = 70; //From datasheet, depends on Scale, 2000DPS = 7
0, 500DPS = 17.5, 250DPS = 8.75.

int16_t x,y,z;
// Acc Variables
int16_t ax,ay,az;
double Ax,Ay,Az;
double accRoll = 0;
void InitSensors(){
Wire.begin(); // Initialize I2C
rangesetup();
delay(1500);

// Variable for accel data

// Calculate bias for the Gyro and accel, the values it gives when it's not mo
ving
// You have to keep the robot vertically and static for a few seconds
for(int i=1; i < 100; i++){ // Takes 100 values to get more precision
getGyroValues(); // Get gyro data
gyroBiasX += (int)x;
gyroBiasY += (int)y;
gyroBiasZ += (int)z;
getAccValues(); // Get accel data
accBiasX += (int)ax;
accBiasY += (int)ay;
accBiasZ += (int)az;
delay(1);
}
// Final bias values for every axis
 gyroBiasX = gyroBiasX / 100;
gyroBiasY = gyroBiasY / 100;
gyroBiasZ = gyroBiasZ / 100;
accBiasX = accBiasX / 100;
accBiasY = accBiasY / 100;
accBiasZ = accBiasZ / 100;
accBiasZ-=16384 ;

//Get Starting Pitch and Roll

getAccValues();
// accRoll = (atan2(Ay,Az))*RAD_TO_DEG;
// if (accRoll <= 360 & accRoll >= 180){
// accRoll = accRoll - 360;
//}
//gyroRoll = accRoll;
}
void getAccRoll(){
getAccValues();
//Serial.print(accBiasY);
//Serial.print("\t");
//Serial.print(accBiasZ);
//Serial.print("\t");
//Serial.print(atan2(Ay-accBiasY,Az-accBiasZ));
//Serial.print("\t");
accRoll = (atan2(Ay-accBiasY,Az-accBiasZ))*RAD_TO_DEG;
}

void InitialValues(){

double InitialAngle = 0;
double dGyro = 0;
for(int i=1; i < 100; i++){ // Takes 100 values to get more precision
getAccValues();
accRoll = (atan2(Ay-accBiasY,Az-accBiasZ))*RAD_TO_DEG;
getGyroValues();
gyroRateX = ((int)x - gyroBiasX)*.06104;
dGyro = gyroRateX * ((double)(micros() - timer)/1000000);
InitialAngle = 0.95* (InitialAngle + dGyro) + 0.05 * (accRoll);
timer = micros();
delay(1);
}
InitialRoll = InitialAngle;
Serial.print("Roll Inicial: ");
Serial.println(InitialRoll);
}

// Roll from accelerometer

double getAccelRoll(){

getAccValues();
accRoll = (atan2(Ay-accBiasY,(Az-accBiasZ)))*RAD_TO_DEG; // Calculate the va
lue of the angle
if (accRoll <= 360 & accRoll >= 180){
accRoll = accRoll - 360;
}
return accRoll;
}
// Roll from gyroscope
double getGyroRoll(){

getGyroValues(); // Get values from gyro

// read raw angular velocity measurements from device
gyroRateX = ((int)x - gyroBiasX)*.06104;
//gyroRateY = -((int)y - gyroBiasY)*.07;
//gyroRateZ = ((int)z - gyroBiasZ)*.07;
gyroRoll += gyroRateX * ((double)(micros() - timer)/1000000);
return gyroRoll;
}
// Angular velocity of Roll by gyroscope
double getDGyroRoll(){
getGyroValues(); // Get values from gyro
// read raw angular velocity measurements from device
gyroRateX = (int)x - gyroBiasX*.06104;
//gyroRateY = -((int)y - gyroBiasY)*.07;
//gyroRateZ = ((int)z - gyroBiasZ)*.07;

double dgyroRoll = gyroRateX * ((double)(micros() - timer)/1000000);

return dgyroRoll;
}

// This function read Nbytes bytes from I2C device at address Address.
// Put read bytes starting at register Register in the Data array.
// Initializations
void rangesetup()
{
// Arduino initializations

// Set accelerometers low pass filter at 5Hz

I2CwriteByte(MPU9250_ADDRESS,29,0x06);
// Set gyroscope low pass filter at 5Hz
I2CwriteByte(MPU9250_ADDRESS,26,0x06);

// Configure gyroscope range

I2CwriteByte(MPU9250_ADDRESS,27,GYRO_FULL_SCALE_2000_DPS);
// Configure accelerometers range
I2CwriteByte(MPU9250_ADDRESS,28,ACC_FULL_SCALE_2_G);
}
void getGyroValues()
{
// Read accelerometer and gyroscope
uint8_t Buf[14];
I2Cread(MPU9250_ADDRESS,0x3B,14,Buf);
// Create 16 bits values from 8 bits data
// Gyroscope
x=-(Buf[8]<<8 | Buf[9]);
y=-(Buf[10]<<8 | Buf[11]);
z=Buf[12]<<8 | Buf[13];
}

void getAccValues(){
uint8_t Buf[14];
I2Cread(MPU9250_ADDRESS,0x3B,14,Buf);
// Accelerometer
ax=-(Buf[0]<<8 | Buf[1]);
ay=-(Buf[2]<<8 | Buf[3]);
az=Buf[4]<<8 | Buf[5];
Ay=(double)ay;
Az=(double)az;
}

void I2Cread(uint8_t Address, uint8_t Register, uint8_t Nbytes, uint8_t* Data)

{
// Set register address
Wire.beginTransmission(Address);
Wire.write(Register);
Wire.endTransmission();
// Read Nbytes
Wire.requestFrom(Address, Nbytes);
uint8_t index=0;
while (Wire.available())
Data[index++]=Wire.read();
}
// Write a byte (Data) in device (Address) at register (Register)
void I2CwriteByte(uint8_t Address, uint8_t Register, uint8_t Data)
{
// Set register address
Wire.beginTransmission(Address);
Wire.write(Register);
Wire.write(Data);
Wire.endTransmission();
}