1 changed files with 66 additions and 258 deletions
@ -1,258 +1,66 @@ |
|||
= 4WD Seesaw balancing car |
|||
|
|||
==== *Δημιουργοί: Θεόδωρος Πουρνάρας(cs141128), Αίας-Παναγιώτης Δρακόπουλος(cs141020), Αλέξανδρος Βλαχακης(cs131127)* |
|||
===== Περιγραφή εργασίας |
|||
|
|||
Η εργασία διαπραγματεύται ένα αυτοκίνητο τεσσάρων τροχών το οποίο πρέπει να ισορροπεί αυτόματα σε μια τραμπάλα. |
|||
|
|||
Το πρόβλημα που καλούμαστε να λύσουμε είναι το εξής: |
|||
Σε μια τραμπάλα ισορροπεί το αυτοκίνητο. Βάζουμε ένα βαρίδιο με σκοπό να χαλάσουμε την ισορροπία αυτήν, |
|||
το αυτοκίνητο θα πρέπει να κινηθεί με τέτοιον τρόπο ώστε η τραμπάλα να ξανά ισορροπήσει. |
|||
Μετά θα βγάλουμε το βαρίδιο και το αμάξι θα πρέπει να κινηθεί για να ισορροπήσει ξανά. |
|||
|
|||
*Το αμάξι θα πρέπει να ισορροπεί για 30 δευτερόλεπτα.* |
|||
|
|||
===== Εξαρτήματα: Arduino Uno,MPU-9250,Motor Shield L293D,4WD Cardboard Car Kit |
|||
|
|||
*Arduino Uno*: |
|||
|
|||
[#img-Uno] |
|||
[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] |
|||
|
|||
*MPU-9250*: |
|||
|
|||
[#img-mpu] |
|||
[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] |
|||
|
|||
*Motor Shield L293D*: |
|||
[#img-l293d] |
|||
[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://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/667413833874407447/IMG_20200116_185617.jpg[final3,300,200] |
|||
|
|||
video::video-1579697375.mp4[width=640] |
|||
|
|||
*Επειδή το powerbank δεν δίνει αρκετό ρεύμα δεν λειτουργούν όλοι οι τροχοί.* |
|||
|
|||
*Και επειδή το γυροσκόπιο δεν είναι σταθερό το αμάξι κουνιέται λίγο.* |
|||
|
|||
==== *Συνδεσμολογία*: |
|||
|
|||
Arduino με το γυροσκόπιο (MPU-9250): |
|||
|
|||
image:https://lucidar.me/en/inertial-measurement-unit/files/wiring-mpu-9250-arduino-mega.png[wiring,300,200] |
|||
|
|||
Arduino με το L293D: |
|||
|
|||
Επειδή το L293D είναι τύπου shield τοποθετείται πάνω στο Arduino. |
|||
|
|||
image:https://udvabony.com/wp-content/uploads/2019/05/L293D-V1-Motor-Driver-Shield-on-Uno.jpg[wiring1,300,200] |
|||
|
|||
L293D με τους τροχούς: |
|||
|
|||
image:https://i.pinimg.com/originals/65/24/a7/6524a7409e7cd6b023f1877ce30376e0.jpg[wiring2,300,200] |
|||
|
|||
|
|||
==== Κώδικας |
|||
|
|||
|
|||
|
|||
#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(); |
|||
} |
|||
= 4WD Seesaw balancing car |
|||
|
|||
==== *Δημιουργοί: Θεόδωρος Πουρνάρας(cs141128), Αίας-Παναγιώτης Δρακόπουλος(cs141020), Αλέξανδρος Βλαχακης(cs131127)* |
|||
===== Περιγραφή εργασίας |
|||
|
|||
Η εργασία διαπραγματεύται ένα αυτοκίνητο τεσσάρων τροχών το οποίο πρέπει να ισορροπεί αυτόματα σε μια τραμπάλα. |
|||
|
|||
Το πρόβλημα που καλούμαστε να λύσουμε είναι το εξής: |
|||
Σε μια τραμπάλα ισορροπεί το αυτοκίνητο. Βάζουμε ένα βαρίδιο με σκοπό να χαλάσουμε την ισορροπία αυτήν, |
|||
το αυτοκίνητο θα πρέπει να κινηθεί με τέτοιον τρόπο ώστε η τραμπάλα να ξανά ισορροπήσει. |
|||
Μετά θα βγάλουμε το βαρίδιο και το αμάξι θα πρέπει να κινηθεί για να ισορροπήσει ξανά. |
|||
|
|||
*Το αμάξι θα πρέπει να ισορροπεί για 30 δευτερόλεπτα.* |
|||
|
|||
===== Εξαρτήματα: Arduino Uno,MPU-9250,Motor Shield L293D,4WD Cardboard Car Kit |
|||
|
|||
*Arduino Uno*: |
|||
|
|||
[#img-Uno] |
|||
[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] |
|||
|
|||
*MPU-9250*: |
|||
|
|||
[#img-mpu] |
|||
[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] |
|||
|
|||
*Motor Shield L293D*: |
|||
[#img-l293d] |
|||
[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://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/667413833874407447/IMG_20200116_185617.jpg[final3,300,200] |
|||
|
|||
video::video-1579697375.mp4[width=640] |
|||
|
|||
*Επειδή το powerbank δεν δίνει αρκετό ρεύμα δεν λειτουργούν όλοι οι τροχοί.* |
|||
|
|||
*Και επειδή το γυροσκόπιο δεν είναι σταθερό το αμάξι κουνιέται λίγο.* |
|||
|
|||
==== *Συνδεσμολογία*: |
|||
|
|||
Arduino με το γυροσκόπιο (MPU-9250): |
|||
|
|||
image:https://lucidar.me/en/inertial-measurement-unit/files/wiring-mpu-9250-arduino-mega.png[wiring,300,200] |
|||
|
|||
Arduino με το L293D: |
|||
|
|||
Επειδή το L293D είναι τύπου shield τοποθετείται πάνω στο Arduino. |
|||
|
|||
image:https://udvabony.com/wp-content/uploads/2019/05/L293D-V1-Motor-Driver-Shield-on-Uno.jpg[wiring1,300,200] |
|||
|
|||
L293D με τους τροχούς: |
|||
|
|||
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 |
|||
Loading…
Reference in new issue