LAMPIRAN 1
PROGRAM LENGKAP
#include "Wire.h"
#include "MPU6050.h"
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
#define Gry_offset 0
#define Gyr_Gain 0.00763358
#define Angle_offset 0
#define RMotor_offset 20
#define LMotor_offset 20
#define pi 3.14159
long data;
int x, y;
char recev;
float kp, ki, kd;
float r_angle, f_angle, omega;
float Turn_Speed = 0, Turn_Speed_K = 0;
float Run_Speed = 0, Run_Speed_K = 0, Run_Speed_T = 0;
float LOutput,ROutput;
unsigned long preTime = 0;
float SampleTime = 0.08;
unsigned long lastTime;
float Input, Output;
float errSum, dErr, error, lastErr;
int timeChange;
int ENC=10;
int END=5;
void setup() {
Serial.begin(9600);
Wire.begin();
accelgyro.initialize();
pinMode(ENA,OUTPUT);
pinMode(ENB,OUTPUT);
pinMode(ENC,OUTPUT);
pinMode(END,OUTPUT);
}
void loop() {
Recive();
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
r_angle = (atan2(ay, az) * 180 / pi + Angle_offset);
omega = Gyr_Gain * (gx + Gry_offset);
if (abs(r_angle)<30){
myPID();
PWMControl();
}
else{
analogWrite(ENC, 0);
analogWrite(END,0);
analogWrite(ENA, 0);
analogWrite(ENB, 0);
}
void Recive(){
void Analog(){
kp = analogRead(A1)*0.1;
kd = analogRead(A2);
ki =0;
}
void myPID(){
Analog();
unsigned long now = millis();
float K = 0.8;
float A = K / (K + dt);
f_angle = A * (f_angle + omega * dt) + (1 - A) * r_angle; //complemtari filter //
Serial.print("*D");Serial.print(f_angle);Serial.print("*" );
timeChange = (now - lastTime);
if(timeChange >= SampleTime){
Input = f_angle;
void PWMControl(){
if(LOutput > 0){
analogWrite(ENA, min(255, abs(LOutput) + LMotor_offset));
analogWrite(ENB, 0);
}
else if(LOutput < 0){
analogWrite(ENA, 0);
analogWrite(ENB, min(255, abs(LOutput) + LMotor_offset));
}
analogWrite(ENA, 0);
analogWrite(ENB, 0);
}
if(ROutput > 0){
analogWrite(ENC, min(255, abs(LOutput) + LMotor_offset));
analogWrite(END, 0);
}
else if(ROutput < 0){
analogWrite(ENC, 0);
analogWrite(END, min(255, abs(LOutput) + LMotor_offset));
}
else{
analogWrite(ENC, 0);
analogWrite(END,0);
}
LAMPIRAN 2
