//MPU6050 stepper motor left n right
#include <Wire.h>
#include <MPU6050.h>
#include <AccelStepper.h>
// Define the pins for the A4988 stepper motor driver.
#define DIR_PIN 2
#define STEP_PIN 3
// Instantiate an AccelStepper object, providing the motor interface type and pin numbers.
AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);
// Instantiate an MPU6050 object for reading accelerometer and gyroscope data.
MPU6050 mpu;
void setup() {
// Initiate serial communication at 9600 baud rate.
Serial.begin(9600);
// Initialize the MPU6050 sensor.
mpu.initialize();
// Set the motor speed and acceleration.
stepper.setMaxSpeed(500);
stepper.setAcceleration(100);
}
void loop() {
// Read the accelerometer data from the MPU6050 sensor.
int16_t ax, ay, az;
mpu.getAcceleration(&ax, &ay, &az);
// Calculate the tilt angle in degrees using the accelerometer data.
float tiltAngleX = atan2(ay, az) * 180 / PI;
float tiltAngleY = atan2(ax, az) * 180 / PI;
// Print the tilt angles to the Serial Monitor.
Serial.print("Tilt Angle X: ");
Serial.print(tiltAngleX);
Serial.print(" degrees");
Serial.print("\t");
Serial.print("Tilt Angle Y: ");
Serial.print(tiltAngleY);
Serial.println(" degrees");
// Control the stepper motor based on the tilt angles.
// Adjust the motor speed and direction based on the tilt angles.
if (tiltAngleX > 5) {
stepper.setMaxSpeed(100);
stepper.move(100);
} else if (tiltAngleX < -5) {
stepper.setMaxSpeed(-100);
stepper.move(-100);
} else {
stepper.setMaxSpeed(0);
}
// Move the stepper motor by a specified number of steps.
stepper.runSpeedToPosition();
}
// // error for wokwi - no adafruit library for DC motor with mpu6050....
// #include<Wire.h>
// #include<Servo.h>
// #include<Adafruit_MotorShield.h>
// Adafruit_MotorShield AFMS = Adafruit_MotorShield();
// //declaring motors
// Adafruit_DCMotor *UL = AFMS.getMotor(4);
// Adafruit_DCMotor *UR = AFMS.getMotor(3);
// //variables for PID control
// float target = 0;
// float error = 0;
// float integral = 0;
// float derivative = 0;
// float last_error = 0;
// //the 'k' values are the ones you need to fine tune before your program will work. Note that these are arbitrary values that you just need to experiment with one at a time.
// float Kp = 11;
// float Ki = 0.09;
// float Kd = 10;
// int mtrSpd = 127.5;
// float angle = 0;
// //Variables for Gyroscope
// int gyro_x, gyro_y, gyro_z;
// long gyro_x_cal, gyro_y_cal, gyro_z_cal;
// boolean set_gyro_angles;
// long acc_x, acc_y, acc_z, acc_total_vector;
// float angle_roll_acc, angle_pitch_acc;
// float angle_pitch, angle_roll;
// int angle_pitch_buffer, angle_roll_buffer;
// float angle_pitch_output, angle_roll_output;
// // Setup timers and temp variables
// long loop_timer;
// int temp;
// int state = 0;
// void setup() {
// Wire.begin();
// //Start the motor shield
// AFMS.begin();
// //Setup the registers of the MPU-6050
// setup_mpu_6050_registers();
// //Read the raw acc and gyro data from the MPU-6050 1000 times
// for (int cal_int = 0; cal_int < 1000 ; cal_int ++){
// read_mpu_6050_data();
// //Add the gyro x offset to the gyro_x_cal variable
// gyro_x_cal += gyro_x;
// //Add the gyro y offset to the gyro_y_cal variable
// gyro_y_cal += gyro_y;
// //Add the gyro z offset to the gyro_z_cal variable
// gyro_z_cal += gyro_z;
// //Delay 3us to have 250Hz for-loop
// delay(3);
// }
// // Divide all results by 1000 to get average offset
// gyro_x_cal /= 1000;
// gyro_y_cal /= 1000;
// gyro_z_cal /= 1000;
// // Start Serial Monitor
// Serial.begin(9600);
// // Init Timer
// loop_timer = micros();
// }
// void loop() {
// // Get data from MPU-6050
// read_mpu_6050_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
// //0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
// angle_roll += gyro_y * 0.0000611;
// //If the IMU has yawed transfer the roll angle to the pitch angle
// angle_pitch += angle_roll * sin(gyro_z * 0.000001066);
// //If the IMU has yawed transfer the pitch angle to the roll angle
// 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;
// //Accelerometer calibration value for pitch
// angle_pitch_acc -= 0.0;
// //Accelerometer calibration value for roll
// angle_roll_acc -= 0.0;
// if(set_gyro_angles){
// //If the IMU has been running
// //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{
// //IMU has just started
// //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 the IMU started flag
// set_gyro_angles = true;
// }
// //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 * 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 * 0.1;
// //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop
// //-----------------done with mpu6050 calibration--------------------------------------//
// error = target - angle_pitch_output;// proportional
// integral = integral + error; //integral
// derivative = error - last_error; //derivative
// angle = (error * Kp) + (integral * Ki) + (derivative * Kd);
// Serial.println(angle_pitch_output);
// //using the pid control outputs
// //setting the deafault speed of the motors
// UR->setSpeed(mtrSpd);
// UL->setSpeed(mtrSpd);
// UR->run(FORWARD);
// UL->run(FORWARD);
// //setting the steering command if it is veering to the right
// if(angle_pitch_output > 0){
// UL->setSpeed(mtrSpd-abs(angle));
// UR->setSpeed(mtrSpd+abs(angle));
// UR->run(FORWARD);
// UL->run(FORWARD);
// }
// else if(angle_pitch_output < 0){ //setting the steering command if it is veering to the left
// UL->setSpeed(mtrSpd+abs(angle));
// UR->setSpeed(mtrSpd-abs(angle));
// UR->run(FORWARD);
// UL->run(FORWARD);
// }
// error = last_error;
// }
// void setup_mpu_6050_registers(){
// //Activate the MPU-6050
// //Start communicating with the MPU-6050
// Wire.beginTransmission(0x68);
// //Send the requested starting register
// Wire.write(0x6B);
// //Set the requested starting register
// Wire.write(0x00);
// //End the transmission
// Wire.endTransmission();
// //Configure the accelerometer (+/-8g)
// //Start communicating with the MPU-6050
// Wire.beginTransmission(0x68);
// //Send the requested starting register
// Wire.write(0x1C);
// //Set the requested starting register
// Wire.write(0x10);
// //End the transmission
// Wire.endTransmission();
// //Configure the gyro (500dps full scale)
// //Start communicating with the MPU-6050
// Wire.beginTransmission(0x68);
// //Send the requested starting register
// Wire.write(0x1B);
// //Set the requested starting register
// Wire.write(0x08);
// //End the transmission
// Wire.endTransmission();
// }
// void read_mpu_6050_data(){
// //Read the raw gyro and accelerometer data
// //Start communicating with the MPU-6050
// Wire.beginTransmission(0x68);
// //Send the requested starting register
// Wire.write(0x3B);
// //End the transmission
// Wire.endTransmission();
// //Request 14 bytes from the MPU-6050
// Wire.requestFrom(0x68,14);
// //Wait until all the bytes are received
// while(Wire.available() < 14);
// //Following statements left shift 8 bits, then bitwise OR.
// //Turns two 8-bit values into one 16-bit value
// 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();
// }
// // MPU6050 left and right stepper motor, constant speed
// //https://www.quora.com/How-can-I-control-a-stepper-motor-and-an-MPU-6050-through-Arduino
// #include <Wire.h>
// #include <AccelStepper.h>
// #include <MPU6050.h>
// // Define the pins for the stepper motor driver
// #define dirPin 2
// #define stepPin 3
// #define enablePin 8 // connect reset to sleep also
// // Initialize the stepper motor driver
// AccelStepper stepper(AccelStepper::DRIVER, stepPin, dirPin);
// // Initialize the MPU-6050 sensor
// MPU6050 mpu;
// void setup() {
// // Set the stepping mode for the stepper motor
// stepper.setSpeed(3);
// // Set the maximum speed and acceleration for the stepper motor
// stepper.setMaxSpeed(100); // keep 200, more stable - if 200 unstable, sliding. higher speed, more sensitive & fast.
// stepper.setAcceleration(500); // keep 500, more stable - if 100 unstable, sliding
// // Initialize the MPU-6050 sensor
// Wire.begin();
// mpu.initialize();
// }
// void loop() {
// // Read the MPU-6050 sensor data
// int16_t ax, ay, az, gx, gy, gz;
// mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
// // Use the sensor data to control the stepper motor's movement
// int targetPos = map(ax, -32768, 32767, 0, 1000);
// stepper.moveTo(targetPos);
// stepper.run();
// }
// // basic control servo left n right speed
// #include <Wire.h>
// #include <MPU6050.h>
// #include <Servo.h>
// Servo sg90;
// int servo_pin = 2;
// MPU6050 sensor ;
// int16_t ax, ay, az ;
// int16_t gx, gy, gz ;
// void setup ( )
// {
// sg90.attach ( servo_pin );
// Wire.begin ( );
// Serial.begin (9600);
// Serial.println ( "Initializing the sensor" );
// sensor.initialize ( );
// Serial.println (sensor.testConnection ( ) ? "Successfully Connected" : "Connection failed");
// delay (1000);
// Serial.println ( "Taking Values from the sensor" );
// delay (1000);
// }
// void loop ( )
// {
// sensor.getMotion6 (&ax, &ay, &az, &gx, &gy, &gz);
// ax = map (ax, -17000, 17000, 0, 180) ;
// Serial.println (ax);
// sg90.write (ax);
// delay (200);
// }
// // Example sketch for the MPU-6050 ---- play led display cube graphic
// //
// // Version 1, 9 August 2021, by Koepel with help from Wokwi community.
// //
// // Start the simulation and click on the MPU-6050 module to change the values.
// //
// // Note:
// // This example does not use a library with quaternations for the 3D orientation.
// // The values are directly read from the MPU-6050 and are used to rotate a cube.
// // It is just a fun example. The rotation of the cube is not according to the
// // physical values of the accelerometer and the gyro.
// //
// // Based on:
// // (1)
// // 3D_Cube for Arduino OLED module by Colin Ord, 9/1/2015
// // A port of my (Colin Ord) original JustBasic Cube_3D demo to the Arduino Uno using U8G library.
// // http://colinord.blogspot.com/2015/01/arduino-oled-module-with-3d-demo.html
// // (no known copyrights)
// //
// // (2)
// // MPU-6050 Short Example Sketch
// // By Arduino User JohnChi
// // August 17, 2014
// // Public Domain
// // https://playground.arduino.cc/Main/MPU-6050/#short
// //
// // (3)
// // The Adafruit GFX library with the SSD1306 driver.
// //
// #include <Wire.h>
// #include <Adafruit_SSD1306.h>
// #include <Adafruit_GFX.h>
// #include <Adafruit_NeoPixel.h>
// Adafruit_SSD1306 display( 128, 64); // 128 pixels width, 64 pixels height
// #define LED_PIN 6
// #define LED_COUNT 1
// Adafruit_NeoPixel neoPixel( LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800);
// const int mpuAddress = 0x68; // I2C address of the MPU-6050
// float xByGyro, yByGyro, zByGyro; // Global variables for the rotation by gyro
// // Set the origin in the middle of the display
// const int xOrigin = 64;
// const int yOrigin = 32;
// const float viewDistance = 150.0; // higher for less perspective, lower for more.
// // Vertices for a cube
// // A cube has 8 corners and each coordinate has x,y,z values.
// #define NUM_VERTICES 8
// const int cube_vertex[NUM_VERTICES][3] =
// {
// { -20, -20, 20 }, // x, y, z
// { 20, -20, 20 },
// { 20, 20, 20 },
// { -20, 20, 20 },
// { -20, -20, -20 },
// { 20, -20, -20 },
// { 20, 20, -20 },
// { -20, 20, -20 }
// };
// // The wirefram is to display the lines on the OLED display
// // It contains the corners of the shape in 2D coordinates
// int wireframe[NUM_VERTICES][2];
// void setup()
// {
// Serial.begin( 115200);
// Wire.begin();
// // Initialize the OLED display and test if it is connected.
// if( !display.begin( SSD1306_SWITCHCAPVCC, 0x3C))
// {
// Serial.println(F( "SSD1306 allocation failed"));
// for(;;); // halt the sketch if error encountered
// }
// // Initialize the MPU-6050 and test if it is connected.
// Wire.beginTransmission( mpuAddress);
// Wire.write( 0x6B); // PWR_MGMT_1 register
// Wire.write( 0); // set to zero (wakes up the MPU-6050)
// auto error = Wire.endTransmission();
// if( error != 0)
// {
// Serial.println(F( "Error, MPU-6050 not found"));
// for(;;); // halt the sketch if error encountered
// }
// // Initialize the NeoPixel
// neoPixel.begin();
// }
// void loop()
// {
// Wire.beginTransmission( mpuAddress);
// Wire.write( 0x3B); // Starting with register 0x3B (ACCEL_XOUT_H)
// Wire.endTransmission( false); // No stop condition for a repeated start
// // The MPU-6050 has the values as signed 16-bit integers.
// // There are 7 values in 14 registers.
// int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
// Wire.requestFrom( mpuAddress, 14); // request a total of 14 bytes
// AcX = Wire.read()<<8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
// AcY = Wire.read()<<8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
// AcZ = Wire.read()<<8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
// Tmp = Wire.read()<<8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
// GyX = Wire.read()<<8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
// GyY = Wire.read()<<8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
// GyZ = Wire.read()<<8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
// // The acceleration is directly mapped into the angles.
// // That is rather artificial.
// // The combined gravity could be used for an angle, while ignoring the strength.
// //
// // The gyro sets the rotation speed.
// // The angle created by the rotation speed is added to angle by the accelerometer.
// //
// // The conversion from the sensor values to the rotation is just a value
// // that makes it look good on the display.
// float xByAccel = (float) AcX * 0.0001; // static angle by accelerometer
// float yByAccel = (float) AcY * 0.0001;
// float zByAccel = (float) AcZ * 0.0001;
// xByGyro += (float) GyX * 0.00001; // moving angle by gyro
// yByGyro += (float) GyY * 0.00001;
// zByGyro += (float) GyZ * 0.00001;
// float x = xByAccel + xByGyro; // combine both angles
// float y = yByAccel + yByGyro;
// float z = zByAccel + zByGyro;
// // Keep the radians in range (although the cos/sin functions accept every value)
// if( x < 0.0)
// x += 2.0 * M_PI;
// else if( x > 2.0 * M_PI)
// x -= 2.0 * M_PI;
// if( y < 0.0)
// y += 2.0 * M_PI;
// else if( y > 2.0 * M_PI)
// y -= 2.0 * M_PI;
// if( z < 0.0)
// z += 2.0 * M_PI;
// else if( z > 2.0 * M_PI)
// z -= 2.0 * M_PI;
// // Draw 3D picture
// for (int i = 0; i < NUM_VERTICES; i++)
// {
// // Rotate Y
// float rotx = cube_vertex[i][2] * sin(y) + cube_vertex[i][0] * cos(y);
// float roty = cube_vertex[i][1];
// float rotz = cube_vertex[i][2] * cos(y) - cube_vertex[i][0] * sin(y);
// // Rotate X
// float rotxx = rotx;
// float rotyy = roty * cos(x) - rotz * sin(x);
// float rotzz = roty * sin(x) + rotz * cos(x);
// // Rotate Z
// float rotxxx = rotxx * cos(z) - rotyy * sin(z);
// float rotyyy = rotxx * sin(z) + rotyy * cos(z);
// float rotzzz = rotzz;
// // Add depth perspective
// rotxxx *= viewDistance / (viewDistance + rotzzz);
// rotyyy *= viewDistance / (viewDistance + rotzzz);
// // Bring to middle of screen
// rotxxx += (float) xOrigin;
// rotyyy += (float) yOrigin;
// // Store new vertices values for wireframe drawing
// wireframe[i][0] = (int) rotxxx;
// wireframe[i][1] = (int) rotyyy;
// wireframe[i][2] = (int) rotzzz;
// }
// draw_wireframe();
// // Set the color of the NeoPixel according to the temperature
// // Temperature by the MPU-6050 is -40 to 85.
// // According to the datasheet:
// // Temperature in Celsius = (raw_value / 340) + 36.53
// // The Hue range for the NeoPixel is the full uint16_t range.
// float Celsius = ((float) Tmp / 340.00) + 36.53;
// float hue = (Celsius + 40.0) / 125.0 * 65535.0;
// uint32_t rgbcolor = neoPixel.ColorHSV( (uint16_t) hue);
// neoPixel.setPixelColor( 0, rgbcolor);
// neoPixel.show(); // update new values to NeoPixel
// }
// void draw_wireframe(void)
// {
// // Start with a empty buffer
// display.clearDisplay();
// // A cube has 8 points and 12 sides.
// // The wireframe contains the 8 points, and the 12 lines are drawn here.
// display.drawLine( wireframe[0][0], wireframe[0][1], wireframe[1][0], wireframe[1][1], SSD1306_WHITE);
// display.drawLine( wireframe[1][0], wireframe[1][1], wireframe[2][0], wireframe[2][1], SSD1306_WHITE);
// display.drawLine( wireframe[2][0], wireframe[2][1], wireframe[3][0], wireframe[3][1], SSD1306_WHITE);
// display.drawLine( wireframe[3][0], wireframe[3][1], wireframe[0][0], wireframe[0][1], SSD1306_WHITE);
// display.drawLine( wireframe[4][0], wireframe[4][1], wireframe[5][0], wireframe[5][1], SSD1306_WHITE);
// display.drawLine( wireframe[5][0], wireframe[5][1], wireframe[6][0], wireframe[6][1], SSD1306_WHITE);
// display.drawLine( wireframe[6][0], wireframe[6][1], wireframe[7][0], wireframe[7][1], SSD1306_WHITE);
// display.drawLine( wireframe[7][0], wireframe[7][1], wireframe[4][0], wireframe[4][1], SSD1306_WHITE);
// display.drawLine( wireframe[0][0], wireframe[0][1], wireframe[4][0], wireframe[4][1], SSD1306_WHITE);
// display.drawLine( wireframe[1][0], wireframe[1][1], wireframe[5][0], wireframe[5][1], SSD1306_WHITE);
// display.drawLine( wireframe[2][0], wireframe[2][1], wireframe[6][0], wireframe[6][1], SSD1306_WHITE);
// display.drawLine( wireframe[3][0], wireframe[3][1], wireframe[7][0], wireframe[7][1], SSD1306_WHITE);
// // Extra cross face on one side
// display.drawLine( wireframe[1][0], wireframe[1][1], wireframe[3][0], wireframe[3][1], SSD1306_WHITE);
// display.drawLine( wireframe[0][0], wireframe[0][1], wireframe[2][0], wireframe[2][1], SSD1306_WHITE);
// // Write the new picture to the display
// display.display();
// }