1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56
| #include <Wire.h>
#include <MPU6050.h>
#include <Servo.h>
#include <PID_v1.h>
#define _DEBUG false
//-------------Declarations capteur et servo----------------
Servo servo1, servo2;
int servo1_pin = 11;
int servo2_pin = 10;
MPU6050 sensor ;
int16_t ax, ay, az ;
int16_t gx, gy, gz ;
//---------PID--------------
double input;
double output;
double setpoint;
double Kp=0, Ki=10, Kd=0;
PID myPID(&input, &output, &setpoint, Kp, Ki, Kd, REVERSE);
void setup ( )
{
servo1.attach ( servo1_pin );
servo2.attach ( servo2_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" );
setpoint=90;
myPID.SetMode(AUTOMATIC);
myPID.SetTunings(Kp,Ki,Kd);
myPID.SetOutputLimits(0, 180);
delay (1000);
}
void loop ( )
{
sensor.getMotion6 (&ax, &ay, &az, &gx, &gy, &gz);
input=map (ay, 17000, -17000, 0, 180) ;
myPID.Compute();
servo1.write(output);
Serial.println(output);
//delay(200);
} |
Partager