Theodore
/
Seesaw_balancing_4WD_robot
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
Browse Source

Update 'Asciidoc/IoT.adoc'

master
Theodore 5 years ago
parent
87d1e3c65e
commit
14e19e19dd
1 changed files with 66 additions and 258 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 194
      Asciidoc/IoT.adoc

194
Asciidoc/IoT.adoc
View File

@ -63,196 +63,4 @@ image:https://i.pinimg.com/originals/65/24/a7/6524a7409e7cd6b023f1877ce30376e0.j
==== Κώδικας
#include <Wire.h>
#include <AFMotor.h>
//Declaring some global variables
int gyro_x, gyro_y, gyro_z = 0;
long gyro_x_cal, gyro_y_cal, gyro_z_cal = 0.0;
boolean set_gyro_angles;
long acc_x, acc_y, acc_z, acc_total_vector = 0.0;
float angle_roll_acc, angle_pitch_acc = 0.0;
float angle_pitch, angle_roll = 0.0;
int angle_pitch_buffer, angle_roll_buffer = 0;
float angle_pitch_output, angle_roll_output = 0.0;
long loop_timer = 0.0;
int temp = 0;
AF_DCMotor motor1(1);
AF_DCMotor motor2(2);
AF_DCMotor motor3(3);
AF_DCMotor motor4(4);
void setup() {
Wire.begin(); //Start I2C as master
setup_mpu_9250_registers(); //Setup the registers of the MPU-9250
//Read the raw acc and gyro data from the MPU-9250 for 1000 times
for (int cal_int = 0; cal_int < 1000 ; cal_int ++) {
read_mpu_9250_data();
gyro_x_cal += gyro_x; //Add the gyro x offset to the gyro_x_cal variable
gyro_y_cal += gyro_y; //Add the gyro y offset to the gyro_y_cal variable
gyro_z_cal += gyro_z; //Add the gyro z offset to the gyro_z_cal variable
delay(3); //Delay 3us to have 250Hz for-loop
motor1.setSpeed(200); //set the motors speed to 200
motor2.setSpeed(200);
motor3.setSpeed(200);
motor4.setSpeed(200);
}
// divide by 1000 to get avarage offset
gyro_x_cal /= 1000;
gyro_y_cal /= 1000;
gyro_z_cal /= 1000;
Serial.begin(9600);
loop_timer = micros(); //Reset the loop timer
}
void loop() {
read_mpu_9250_data();
//Subtract the offset values from the raw gyro values
gyro_x -= gyro_x_cal;
gyro_y -= gyro_y_cal;
gyro_z -= gyro_z_cal;
//Gyro angle calculations . Note 0.0000611 = 1 / (250Hz x 65.5)
//Calculate the traveled pitch angle and add this to the angle_pitch variable
angle_pitch += gyro_x * 0.0000611;
//Calculate the traveled roll angle and add this to the angle_roll variable
angle_roll += gyro_y * 0.0000611;
//0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
//If the IMU has yawed transfer the roll angle to the pitch angel
angle_pitch += angle_roll * sin(gyro_z * 0.000001066);
//If the IMU has yawed transfer the pitch angle to the roll angel
angle_roll -= angle_pitch * sin(gyro_z * 0.000001066);
//Accelerometer angle calculations
//Calculate the total accelerometer vector
acc_total_vector = sqrt((acc_x * acc_x) + (acc_y * acc_y) + (acc_z * acc_z));
//57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
//Calculate the pitch angle
angle_pitch_acc = asin((float)acc_y / acc_total_vector) * 57.296;
//Calculate the roll angle
angle_roll_acc = asin((float)acc_x / acc_total_vector) * -57.296;
angle_pitch_acc -= 0.0; //Accelerometer calibration value for pitch
angle_roll_acc -= 0.0; //Accelerometer calibration value for roll
if (set_gyro_angles) {//If the IMU is already started
//Correct the drift of the gyro pitch angle with the accelerometer pitch angle
angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004;
//Correct the drift of the gyro roll angle with the accelerometer roll angle
angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004;
}
else { //At first start
//Set the gyro pitch angle equal to the accelerometer pitch angle
angle_pitch = angle_pitch_acc;
//Set the gyro roll angle equal to the accelerometer roll angle
angle_roll = angle_roll_acc;
set_gyro_angles = true; //Set the IMU started flag
}
//To dampen the pitch and roll angles a complementary filter is used
//Take 90% of the output pitch value and add 10% of the raw pitch value
angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch_acc * 0.1;
//Take 90% of the output roll value and add 10% of the raw roll value
angle_roll_output = angle_roll_output * 0.9 + angle_roll_acc * 0.1;
Serial.print(" | Angle = "); Serial.println(angle_pitch_output);
while (micros() - loop_timer < 10);
{//Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop
loop_timer = micros();//Reset the loop timer
}
if (angle_pitch_output > 3)
// if the pitch output is greater than 3 move forward
{
motor1.run(FORWARD);
motor2.run(FORWARD);
motor3.run(FORWARD);
motor4.run(FORWARD);
}
else if (angle_pitch_output < -3)
// if the pitch output is less than -3 move backward
{
motor1.run(BACKWARD);
motor2.run(BACKWARD);
motor3.run(BACKWARD);
motor4.run(BACKWARD);
}
else if (angle_pitch_output >= -3 || angle_pitch_output <= 3)
// if the pitch output is between -3 and 3 stop moving
{
motor1.run(RELEASE);
motor2.run(RELEASE);
motor3.run(RELEASE);
motor4.run(RELEASE);
}
}
void setup_mpu_9250_registers() {
//Activate the MPU-9250
Wire.beginTransmission(0x68); //Start communicating with the MPU-9250
Wire.write(0x6B); //Send the requested starting register
Wire.write(0x00); //Set the requested starting register
Wire.endTransmission();
//Configure the accelerometer (+/-8g)
Wire.beginTransmission(0x68); //Start communicating with the MPU-9250
Wire.write(0x1C); //Send the requested starting register
Wire.write(0x10); //Set the requested starting register
Wire.endTransmission();
//Configure the gyro (500dps full scale)
Wire.beginTransmission(0x68); //Start communicating with the MPU-9250
Wire.write(0x1B); //Send the requested starting register
Wire.write(0x08); //Set the requested starting register
Wire.endTransmission();
}
void read_mpu_9250_data() {
//Subroutine for reading the raw gyro and accelerometer data
Wire.beginTransmission(0x68); //Start communicating with the MPU-9250
Wire.write(0x3B); //Send the requested starting register
Wire.endTransmission(); //End the transmission
Wire.requestFrom(0x68, 14); //Request 14 bytes from the MPU-9250
while (Wire.available() < 14); //Wait until all the bytes are received
acc_x = Wire.read() << 8 | Wire.read();
acc_y = Wire.read() << 8 | Wire.read();
acc_z = Wire.read() << 8 | Wire.read();
temp = Wire.read() << 8 | Wire.read();
gyro_x = Wire.read() << 8 | Wire.read();
gyro_y = Wire.read() << 8 | Wire.read();
gyro_z = Wire.read() << 8 | Wire.read();
}
https://git.swarmlab.io:3000/Theodore/Seesaw_balancing_4WD_robot/src/branch/master/Measure_angle_Arduino_MPU6050.ino
Write Preview
Loading…
Cancel
Save