= 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();
}