Welcome! Share code as fast as possible.
- Use the language picker to change the language manually.
- The previous link will get modified. You can then share this new link with others.
- Visit
https://code-dump.vercel.app/<extension>
- For example, to create a link which for a JavaScript code you will visit
https://code-dump.vercel.app/js
Any link generated does not have an expiry.
GitHub
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <PID_v1.h>
#define en_a 12
#define motor1_pin1 14
#define motor1_pin2 27
#define en_b 33
#define motor2_pin1 25
#define motor2_pin2 26
Adafruit_MPU6050 mpu;
float currentTime, elapsedTime, previousTime;
float acclAngleY;
float angle;
struct Motor {
int pin1;
int pin2;
int spd;
char dir; //Still, Fowrard, Backward
};
Motor motor1 = {motor1_pin1, motor1_pin2, en_a, 's'};
Motor motor2 = {motor2_pin1, motor2_pin2, en_b, 's'};
double p = 25, i = 0.9, d = 0.1;
double setpoint = 131;
double input, output;
PID pidController(&input, &output, &setpoint, p, i, d, P_ON_E, DIRECT);
void setup() {
Serial.begin(115200);
/* Wait for serial to begin */
while (!Serial)
delay(10);
/* MPU6050 SETUP */
/* Attempt to connect to MPU6050 */
if (!mpu.begin()) {
Serial.println("Failed to find MPU6050 chip");
while (1) {
delay(10);
}
}
Serial.println("MPU6050 Found!");
mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
mpu.setGyroRange(MPU6050_RANGE_500_DEG);
/* MOTOR SETUP */
/* Motor 1 */
pinMode(motor1.spd, OUTPUT);
pinMode(motor1.pin1, OUTPUT);
pinMode(motor1.pin2, OUTPUT);
/* Motor 2 */
pinMode(motor2.spd, OUTPUT);
pinMode(motor2.pin1, OUTPUT);
pinMode(motor2.pin2, OUTPUT);
/* Start Motors off */
digitalWrite(motor1.pin1, LOW);
digitalWrite(motor1.pin2, LOW);
digitalWrite(motor2.pin1, LOW);
digitalWrite(motor2.pin2, LOW);
/* PID SETUP */
pidController.SetMode(AUTOMATIC);
}
void loop() {
input = map(double(get_angle()), -55, 55, 0, 255); // double(smoothed_angle(15));
Serial.print("INPUT: ");
Serial.print(input);
Serial.print(" ");
Serial.print("OUTPUT: ");
Serial.print(output);
Serial.print(" ");
pidController.Compute();
angle = get_angle();
Serial.print("ANGLE: ");
Serial.print(angle);
Serial.println(" ");
if (angle < 0)
{
set_motor(motor1, false, output);
set_motor(motor2, false, output);
}
else if (angle > 0)
{
set_motor(motor1, true, output);
set_motor(motor2, true, output);
}
// else
// {
// digitalWrite(motor1.pin1, LOW);
// digitalWrite(motor1.pin2, LOW);
// digitalWrite(motor2.pin1, LOW);
// digitalWrite(motor2.pin2, LOW);
// }
delay(10);
}
void set_motor(Motor motor, bool forward, int spd) {
if (forward && motor.dir != 'f')
{
digitalWrite(motor.pin1, LOW);
digitalWrite(motor.pin2, HIGH);
motor.dir = 'f';
}
else if (!forward && motor.dir != 'b')
{
digitalWrite(motor.pin1, HIGH);
digitalWrite(motor.pin2, LOW);
motor.dir = 'b';
}
analogWrite(motor.spd, spd);
}
float smoothed_angle(int n) {
float sum = 0;
for (int i = 0; i < n; i++) {
previousTime = currentTime;
currentTime = millis();
elapsedTime = (currentTime - previousTime) / 1000;
sum += get_angle();
}
return sum / n;
}
float get_angle() {
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);
float gyroAngleY = (atan(-1 * a.acceleration.x / sqrt(pow(a.acceleration.y, 2) + pow(a.acceleration.z, 2))) * 180 / PI);
float acclAngleY = acclAngleY + g.gyro.y * elapsedTime;
float angle = 0.96 * gyroAngleY + 0.04 * a.acceleration.z;
return angle;
}