#include<Wire.h>
#include <PID_v1.h>
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
//Define the aggressive and conservative Tuning Parameters
double aggKp=4, aggKi=0.2, aggKd=1;
double consKp=1, consKi=0.05, consKd=0.25;
//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, DIRECT);
const long interval = 1000;
unsigned long previousMillis = 0;
const int MPU_addr=0x68;
int16_t axis_X,axis_Y,axis_Z;
int minVal=265;
int maxVal=402;
double x;
double y;
double z;
int Stepping = false;
int Stepping1 = false;
void setup(){
pinMode(8, OUTPUT);
pinMode(7, OUTPUT);
pinMode(3, OUTPUT);
pinMode(4, OUTPUT);
//digitalWrite(7, LOW); //clockwise
//digitalWrite(8, HIGH); //clockwise
digitalWrite(3, HIGH); //disable stepper 1
digitalWrite(4, HIGH); //disable stepper 2
pinMode(12, OUTPUT);
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
Serial.begin(9600);
int xAng = map(axis_X,minVal,maxVal,-90,90);
int yAng = map(axis_Y,minVal,maxVal,-90,90);
int zAng = map(axis_Z,minVal,maxVal,-90,90);
Input = y;
Setpoint = 177;
//turn the PID on
myPID.SetMode(AUTOMATIC);
}
void loop(){
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true);
axis_X=Wire.read()<<8|Wire.read();
axis_Y=Wire.read()<<8|Wire.read();
axis_Z=Wire.read()<<8|Wire.read();
int xAng = map(axis_X,minVal,maxVal,-90,90);
int yAng = map(axis_Y,minVal,maxVal,-90,90);
int zAng = map(axis_Z,minVal,maxVal,-90,90);
//x= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI);
y= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);
//z= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);
/Serial.print("Angle of inclination in X axis = ");
Serial.print(x);
Serial.println((char)176);/
//Serial.print("Angle of inclination in Y axis= ");
//Serial.print(y);
//Serial.println((char)176);
/Serial.print("Angle of inclination in Z axis= ");
Serial.print(z);
Serial.println((char)176);/
//Serial.println("-------------------------------------------");
unsigned long currentMillis = millis();
if (currentMillis - previousMillis >= interval) {
// save the last time you blinked the LED
previousMillis = currentMillis;
Serial.println(y);
}
Input = y;
double gap = abs(Setpoint-Input);
if (gap<0){
//this is for led
digitalWrite(7, LOW); //this is for direction pin of stepper 1
digitalWrite(8, HIGH); //this is for direction pin of stepper 2
Stepping = true;
}
else if (gap>10){
digitalWrite(7,HIGH);
digitalWrite(8,LOW);
Stepping = true;
}
else{
Stepping=false;
digitalWrite(12,LOW);
digitalWrite(3, HIGH);
digitalWrite(4, HIGH);
}
if (Stepping == true){
digitalWrite(12,HIGH);
digitalWrite(3, LOW);
digitalWrite(4, LOW);
}
}