Mechanical Engineer & Tinkerer of Things. (Project Leader)
· New YorkU.S.
QUESTION
Open

# It's A New Week. We Are on the Lookout for Promising Projects. Ask Us Anything!

Hey All – it’s a bright beautiful week to earn funding for your Project. And it’s not that hard! Show us progress, leadership, and a clear game-plan – and funding will come if that effort is put in :).

12 likes
Like
Award Contribution
Designer special solution && automation , Software engneering student , Robotics developer
``````
// 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 = 2;           // 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;

}
``````
Like
1 like
Award Contribution
Designer special solution && automation , Software engneering student , Robotics developer

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

}

Like
1 like
Award Contribution
Designer special solution && automation , Software engneering student , Robotics developer

#include <Wire.h>

// Gyro variables
#define CTRL_REG1 0x20
#define CTRL_REG2 0x21
#define CTRL_REG3 0x22
#define CTRL_REG4 0x23
#define CTRL_REG5 0x24

// Accel

//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 = 70, 500DPS = 17.5, 250DPS = 8.75.

// Gyro Variables
int x;
int y;
int z;

void InitSensors(){

Wire.begin(); // Initialize I2C

// ¿200?
setupL3G4200D(2000); // Configure L3G4200 - 250, 500 or 2000 deg/sec
delay(1500);

if(!accel.begin()) // Initialize Accelerometer
{
/* There was a problem detecting the ADXL345 … check your connections */
while(1);
}

sensors_event_t event; // Variable for accel data

// Calculate bias for the Gyro and accel, the values it gives when it’s not moving
// 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;
Serial.println( gyroBiasX);
gyroBiasY += (int)y;
Serial.println( gyroBiasY);
gyroBiasZ += (int)z;
Serial.println( gyroBiasZ);

accel.getEvent(&event);       // Get accel data
accBiasX += event.acceleration.x;
accBiasY += event.acceleration.y;
accBiasZ += event.acceleration.z;

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;

//Get Starting Pitch and Roll
accel.getEvent(&event);

if (accRoll <= 360 & accRoll >= 180){
accRoll = accRoll - 360;
}

//gyroRoll = accRoll;

}

void InitialValues(){

// Accelerometer
sensors_event_t event;
double InitialAngle = 0;
double dGyro = 0;
for(int i=1; i < 100; i++){ // Takes 100 values to get more precision

``````accel.getEvent(&event);

getGyroValues();
gyroRateX = ((int)x - gyroBiasX)*.07;

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

InitialAngle = 0.98* (InitialAngle + dGyro) + 0.02 * (accRoll);
timer = micros();

delay(1);
``````

}

InitialRoll = InitialAngle;

Serial.print("Roll Inicial: ");
Serial.println(InitialRoll);

}

// Roll from accelerometer
double getAccelRoll(){

sensors_event_t event;

accRoll = (atan2(event.acceleration.y-accBiasY,-(event.acceleration.z-accBiasZ))+PI)*RAD_TO_DEG; // Calculate the value 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).07;
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).07;
gyroRateY = -((int)y - gyroBiasY)
.07;
gyroRateZ = ((int)z - gyroBiasZ)*.07;

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

return dgyroRoll;

}

/////////////////////////////////////////////////////////////
////////////// Gyro Sensor Code //////////////////
/////////////////////////////////////////////////////////////

// This part of code if from Jim Lindblom of Sparkfun’s code
// used for read the data from the gyro sensor: L3G4200D

void getGyroValues(){

x = ((xMSB << 8) | xLSB);

y = ((yMSB << 8) | yLSB);

z = ((zMSB << 8) | zLSB);
}

int setupL3G4200D(int scale){

//From Jim Lindblom of Sparkfun’s code

// Enable x, y, z and turn off power down:

// If you’d like to adjust/use the HPF, you can edit the line below to configure CTRL_REG2:

// Configure CTRL_REG3 to generate data ready interrupt on INT2
// No interrupts used on INT1, if you’d like to configure INT1
// or INT2 otherwise, consult the datasheet:

// CTRL_REG4 controls the full-scale range, among other things:

if(scale == 250){
}else if(scale == 500){
}else{
}

// CTRL_REG5 controls high-pass filtering of outputs, use it
// if you’d like:

}

Wire.beginTransmission(deviceAddress); // start transmission to device
Wire.write(val); // send value to write
Wire.endTransmission(); // end transmission
}

``````int v;
Wire.endTransmission();

``````

file 4 sensors
while(!Wire.available()) {
// waiting
}

``````v = Wire.read();
return v;
``````

}

Like
Award Contribution
Designer special solution && automation , Software engneering student , Robotics developer

file 4

#include <Wire.h>

// Gyro variables
#define CTRL_REG1 0x20
#define CTRL_REG2 0x21
#define CTRL_REG3 0x22
#define CTRL_REG4 0x23
#define CTRL_REG5 0x24

// Accel

//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 = 70, 500DPS = 17.5, 250DPS = 8.75.

// Gyro Variables
int x;
int y;
int z;

void InitSensors(){

Wire.begin(); // Initialize I2C

// ¿200?
setupL3G4200D(2000); // Configure L3G4200 - 250, 500 or 2000 deg/sec
delay(1500);

if(!accel.begin()) // Initialize Accelerometer
{
/* There was a problem detecting the ADXL345 … check your connections */
while(1);
}

sensors_event_t event; // Variable for accel data

// Calculate bias for the Gyro and accel, the values it gives when it’s not moving
// 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;
Serial.println( gyroBiasX);
gyroBiasY += (int)y;
Serial.println( gyroBiasY);
gyroBiasZ += (int)z;
Serial.println( gyroBiasZ);

accel.getEvent(&event);       // Get accel data
accBiasX += event.acceleration.x;
accBiasY += event.acceleration.y;
accBiasZ += event.acceleration.z;

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;

//Get Starting Pitch and Roll
accel.getEvent(&event);

if (accRoll <= 360 & accRoll >= 180){
accRoll = accRoll - 360;
}

//gyroRoll = accRoll;

}

void InitialValues(){

// Accelerometer
sensors_event_t event;
double InitialAngle = 0;
double dGyro = 0;
for(int i=1; i < 100; i++){ // Takes 100 values to get more precision

``````accel.getEvent(&event);

getGyroValues();
gyroRateX = ((int)x - gyroBiasX)*.07;

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

InitialAngle = 0.98* (InitialAngle + dGyro) + 0.02 * (accRoll);
timer = micros();

delay(1);
``````

}

InitialRoll = InitialAngle;

Serial.print("Roll Inicial: ");
Serial.println(InitialRoll);

}

// Roll from accelerometer
double getAccelRoll(){

sensors_event_t event;

accRoll = (atan2(event.acceleration.y-accBiasY,-(event.acceleration.z-accBiasZ))+PI)*RAD_TO_DEG; // Calculate the value 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).07;
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).07;
gyroRateY = -((int)y - gyroBiasY)
.07;
gyroRateZ = ((int)z - gyroBiasZ)*.07;

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

return dgyroRoll;

}

/////////////////////////////////////////////////////////////
////////////// Gyro Sensor Code //////////////////
/////////////////////////////////////////////////////////////

// This part of code if from Jim Lindblom of Sparkfun’s code
// used for read the data from the gyro sensor: L3G4200D

void getGyroValues(){

x = ((xMSB << 8) | xLSB);

y = ((yMSB << 8) | yLSB);

z = ((zMSB << 8) | zLSB);
}

int setupL3G4200D(int scale){

//From Jim Lindblom of Sparkfun’s code

// Enable x, y, z and turn off power down:

// If you’d like to adjust/use the HPF, you can edit the line below to configure CTRL_REG2:

// Configure CTRL_REG3 to generate data ready interrupt on INT2
// No interrupts used on INT1, if you’d like to configure INT1
// or INT2 otherwise, consult the datasheet:

// CTRL_REG4 controls the full-scale range, among other things:

if(scale == 250){
}else if(scale == 500){
}else{
}

// CTRL_REG5 controls high-pass filtering of outputs, use it
// if you’d like:

}

Wire.beginTransmission(deviceAddress); // start transmission to device
Wire.write(val); // send value to write
Wire.endTransmission(); // end transmission
}

``````int v;
Wire.endTransmission();

while(!Wire.available()) {
// waiting
}

return v;
``````

}

Like
1 like
Award Contribution
Designer special solution && automation , Software engneering student , Robotics developer

sorry for this mess
all codes have been taken from this adress

http://www.instructables.com/id/Self-Balancing-Robot/

Like
Award Contribution
Designer special solution && automation , Software engneering student , Robotics developer
Like
Award Contribution