1 changed files with 66 additions and 258 deletions
@ -1,258 +1,66 @@ |
|||||
= 4WD Seesaw balancing car |
= 4WD Seesaw balancing car |
||||
|
|
||||
==== *Δημιουργοί: Θεόδωρος Πουρνάρας(cs141128), Αίας-Παναγιώτης Δρακόπουλος(cs141020), Αλέξανδρος Βλαχακης(cs131127)* |
==== *Δημιουργοί: Θεόδωρος Πουρνάρας(cs141128), Αίας-Παναγιώτης Δρακόπουλος(cs141020), Αλέξανδρος Βλαχακης(cs131127)* |
||||
===== Περιγραφή εργασίας |
===== Περιγραφή εργασίας |
||||
|
|
||||
Η εργασία διαπραγματεύται ένα αυτοκίνητο τεσσάρων τροχών το οποίο πρέπει να ισορροπεί αυτόματα σε μια τραμπάλα. |
Η εργασία διαπραγματεύται ένα αυτοκίνητο τεσσάρων τροχών το οποίο πρέπει να ισορροπεί αυτόματα σε μια τραμπάλα. |
||||
|
|
||||
Το πρόβλημα που καλούμαστε να λύσουμε είναι το εξής: |
Το πρόβλημα που καλούμαστε να λύσουμε είναι το εξής: |
||||
Σε μια τραμπάλα ισορροπεί το αυτοκίνητο. Βάζουμε ένα βαρίδιο με σκοπό να χαλάσουμε την ισορροπία αυτήν, |
Σε μια τραμπάλα ισορροπεί το αυτοκίνητο. Βάζουμε ένα βαρίδιο με σκοπό να χαλάσουμε την ισορροπία αυτήν, |
||||
το αυτοκίνητο θα πρέπει να κινηθεί με τέτοιον τρόπο ώστε η τραμπάλα να ξανά ισορροπήσει. |
το αυτοκίνητο θα πρέπει να κινηθεί με τέτοιον τρόπο ώστε η τραμπάλα να ξανά ισορροπήσει. |
||||
Μετά θα βγάλουμε το βαρίδιο και το αμάξι θα πρέπει να κινηθεί για να ισορροπήσει ξανά. |
Μετά θα βγάλουμε το βαρίδιο και το αμάξι θα πρέπει να κινηθεί για να ισορροπήσει ξανά. |
||||
|
|
||||
*Το αμάξι θα πρέπει να ισορροπεί για 30 δευτερόλεπτα.* |
*Το αμάξι θα πρέπει να ισορροπεί για 30 δευτερόλεπτα.* |
||||
|
|
||||
===== Εξαρτήματα: Arduino Uno,MPU-9250,Motor Shield L293D,4WD Cardboard Car Kit |
===== Εξαρτήματα: Arduino Uno,MPU-9250,Motor Shield L293D,4WD Cardboard Car Kit |
||||
|
|
||||
*Arduino Uno*: |
*Arduino Uno*: |
||||
|
|
||||
[#img-Uno] |
[#img-Uno] |
||||
[caption="Figure 1: ",link=https://c.scdn.gr/images/sku_main_images/008846/8846565/large_20190607104956_uno_r3_atmega328p.jpeg] |
[caption="Figure 1: ",link=https://c.scdn.gr/images/sku_main_images/008846/8846565/large_20190607104956_uno_r3_atmega328p.jpeg] |
||||
image::https://c.scdn.gr/images/sku_main_images/008846/8846565/large_20190607104956_uno_r3_atmega328p.jpeg[Uno,300,200] |
image::https://c.scdn.gr/images/sku_main_images/008846/8846565/large_20190607104956_uno_r3_atmega328p.jpeg[Uno,300,200] |
||||
|
|
||||
*MPU-9250*: |
*MPU-9250*: |
||||
|
|
||||
[#img-mpu] |
[#img-mpu] |
||||
[caption="Figure 2: ",link=https://www.cableworks.gr/images/thumbnails/499/437/detailed/255/mpu9250.jpg] |
[caption="Figure 2: ",link=https://www.cableworks.gr/images/thumbnails/499/437/detailed/255/mpu9250.jpg] |
||||
image::https://www.cableworks.gr/images/thumbnails/499/437/detailed/255/mpu9250.jpg[mpu,300,200] |
image::https://www.cableworks.gr/images/thumbnails/499/437/detailed/255/mpu9250.jpg[mpu,300,200] |
||||
|
|
||||
*Motor Shield L293D*: |
*Motor Shield L293D*: |
||||
[#img-l293d] |
[#img-l293d] |
||||
[caption="Figure 3: ",link=https://www.cableworks.gr/images/thumbnails/400/350/detailed/254/l293d_motor_shield.jpg] |
[caption="Figure 3: ",link=https://www.cableworks.gr/images/thumbnails/400/350/detailed/254/l293d_motor_shield.jpg] |
||||
image::https://www.cableworks.gr/images/thumbnails/400/350/detailed/254/l293d_motor_shield.jpg[l293d,300,200] |
image::https://www.cableworks.gr/images/thumbnails/400/350/detailed/254/l293d_motor_shield.jpg[l293d,300,200] |
||||
|
|
||||
*Τελικό προϊόν*: |
*Τελικό προϊόν*: |
||||
|
|
||||
image::https://cdn.discordapp.com/attachments/327935497630515200/667414169162612746/IMG_20200116_185600.jpg[final1,300,200] |
image::https://cdn.discordapp.com/attachments/327935497630515200/667414169162612746/IMG_20200116_185600.jpg[final1,300,200] |
||||
|
|
||||
image::https://cdn.discordapp.com/attachments/327935497630515200/667413990594314270/IMG_20200116_185611.jpg[final2,300,200] |
image::https://cdn.discordapp.com/attachments/327935497630515200/667413990594314270/IMG_20200116_185611.jpg[final2,300,200] |
||||
|
|
||||
image::https://cdn.discordapp.com/attachments/327935497630515200/667413833874407447/IMG_20200116_185617.jpg[final3,300,200] |
image::https://cdn.discordapp.com/attachments/327935497630515200/667413833874407447/IMG_20200116_185617.jpg[final3,300,200] |
||||
|
|
||||
video::video-1579697375.mp4[width=640] |
video::video-1579697375.mp4[width=640] |
||||
|
|
||||
*Επειδή το powerbank δεν δίνει αρκετό ρεύμα δεν λειτουργούν όλοι οι τροχοί.* |
*Επειδή το powerbank δεν δίνει αρκετό ρεύμα δεν λειτουργούν όλοι οι τροχοί.* |
||||
|
|
||||
*Και επειδή το γυροσκόπιο δεν είναι σταθερό το αμάξι κουνιέται λίγο.* |
*Και επειδή το γυροσκόπιο δεν είναι σταθερό το αμάξι κουνιέται λίγο.* |
||||
|
|
||||
==== *Συνδεσμολογία*: |
==== *Συνδεσμολογία*: |
||||
|
|
||||
Arduino με το γυροσκόπιο (MPU-9250): |
Arduino με το γυροσκόπιο (MPU-9250): |
||||
|
|
||||
image:https://lucidar.me/en/inertial-measurement-unit/files/wiring-mpu-9250-arduino-mega.png[wiring,300,200] |
image:https://lucidar.me/en/inertial-measurement-unit/files/wiring-mpu-9250-arduino-mega.png[wiring,300,200] |
||||
|
|
||||
Arduino με το L293D: |
Arduino με το L293D: |
||||
|
|
||||
Επειδή το L293D είναι τύπου shield τοποθετείται πάνω στο Arduino. |
Επειδή το L293D είναι τύπου shield τοποθετείται πάνω στο Arduino. |
||||
|
|
||||
image:https://udvabony.com/wp-content/uploads/2019/05/L293D-V1-Motor-Driver-Shield-on-Uno.jpg[wiring1,300,200] |
image:https://udvabony.com/wp-content/uploads/2019/05/L293D-V1-Motor-Driver-Shield-on-Uno.jpg[wiring1,300,200] |
||||
|
|
||||
L293D με τους τροχούς: |
L293D με τους τροχούς: |
||||
|
|
||||
image:https://i.pinimg.com/originals/65/24/a7/6524a7409e7cd6b023f1877ce30376e0.jpg[wiring2,300,200] |
image:https://i.pinimg.com/originals/65/24/a7/6524a7409e7cd6b023f1877ce30376e0.jpg[wiring2,300,200] |
||||
|
|
||||
|
|
||||
==== Κώδικας |
==== Κώδικας |
||||
|
https://git.swarmlab.io:3000/Theodore/Seesaw_balancing_4WD_robot/src/branch/master/Measure_angle_Arduino_MPU6050.ino |
||||
|
|
||||
|
|
||||
#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(); |
|
||||
} |
|
||||
Loading…
Reference in new issue