Professional Documents
Culture Documents
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
////////////////////////////////////////////////////////////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);
}
/////////////////////////////////////////////////////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;
}
///////////////////////////////////////////////////////////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
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);
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);
}
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(){
// 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
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;
}