|
- #include "Wire.h"
- //#include "I2Cdev.h"
- //#include "MPU6050.h"
- #include "Timer.h"
- #include "pid_v1.h"
- // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
- // for both classes must be in the include path of your project
- #include "I2Cdev.h"
- #include "MPU6050_6Axis_MotionApps20.h"
- //#include "MPU6050.h" // not necessary if using MotionApps include file
- // Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
- // is used in I2Cdev.h
- //#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
- //#include "Wire.h"
- //#endif
- //=================Pins===================
- #define TrigPin 5
- #define EchoPin 6
- #define MAE 3
- #define USHIRO 10
- #define HITARI 9
- #define MIGI 11
- //////////////////////////////////////////
- Timer t;//ʱ����
- // class default I2C address is 0x68
- // specific I2C addresses may be passed as a parameter here
- // AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
- // AD0 high = 0x69
- MPU6050 mpu;
- //MPU6050 mpu(0x69); // <-- use for AD0 high
- //=================mpu dmp==============
- // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
- // 6/21/2012 by Jeff Rowberg <[email][email protected][/email]>
- // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
- //
- // Changelog:
- // 2013-05-08 - added seamless Fastwire support
- // - added note about gyro calibration
- // 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
- // 2012-06-20 - improved FIFO overflow handling and simplified read process
- // 2012-06-19 - completely rearranged DMP initialization code and simplification
- // 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
- // 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
- // 2012-06-05 - add gravity-compensated initial reference frame acceleration output
- // - add 3D math helper file to DMP6 example sketch
- // - add Euler output and Yaw/Pitch/Roll output formats
- // 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
- // 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
- // 2012-05-30 - basic DMP initialization working
- /* ============================================
- I2Cdev device library code is placed under the MIT license
- Copyright (c) 2012 Jeff Rowberg
- Permission is hereby granted, free of charge, to any person obtaining a copy
- of this software and associated documentation files (the "Software"), to deal
- in the Software without restriction, including without limitation the rights
- to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- copies of the Software, and to permit persons to whom the Software is
- furnished to do so, subject to the following conditions:
- The above copyright notice and this permission notice shall be included in
- all copies or substantial portions of the Software.
- THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- THE SOFTWARE.
- ===============================================
- */
- /* =========================================================================
- NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
- depends on the MPU-6050's INT pin being connected to the Arduino's
- external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
- digital I/O pin 2.
- * ========================================================================= */
- /* =========================================================================
- NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
- when using Serial.write(buf, len). The Teapot output uses this method.
- The solution requires a modification to the Arduino USBAPI.h file, which
- is fortunately simple, but annoying. This will be fixed in the next IDE
- release. For more info, see these links:
- http://arduino.cc/forum/index.php/topic,109987.0.html
- http://code.google.com/p/arduino/issues/detail?id=958
- * ========================================================================= */
- // uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
- // quaternion components in a [w, x, y, z] format (not best for parsing
- // on a remote host such as Processing or something though)
- //#define OUTPUT_READABLE_QUATERNION
- // uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
- // (in degrees) calculated from the quaternions coming from the FIFO.
- // Note that Euler angles suffer from gimbal lock (for more info, see
- // http://en.wikipedia.org/wiki/Gimbal_lock)
- //#define OUTPUT_READABLE_EULER
- // uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
- // pitch/roll angles (in degrees) calculated from the quaternions coming
- // from the FIFO. Note this also requires gravity vector calculations.
- // Also note that yaw/pitch/roll angles suffer from gimbal lock (for
- // more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
- #define OUTPUT_READABLE_YAWPITCHROLL
- // uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
- // components with gravity removed. This acceleration reference frame is
- // not compensated for orientation, so +X is always +X according to the
- // sensor, just without the effects of gravity. If you want acceleration
- // compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
- //#define OUTPUT_READABLE_REALACCEL
- // uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
- // components with gravity removed and adjusted for the world frame of
- // reference (yaw is relative to initial orientation, since no magnetometer
- // is present in this case). Could be quite handy in some cases.
- //#define OUTPUT_READABLE_WORLDACCEL
- // uncomment "OUTPUT_TEAPOT" if you want output that matches the
- // format used for the InvenSense teapot demo
- //#define OUTPUT_TEAPOT
- #define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards
- #define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
- bool blinkState = false;
- // MPU control/status vars
- bool dmpReady = false; // set true if DMP init was successful
- uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
- uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
- uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
- uint16_t fifoCount; // count of all bytes currently in FIFO
- uint8_t fifoBuffer[64]; // FIFO storage buffer
- // orientation/motion vars
- Quaternion q; // [w, x, y, z] quaternion container
- VectorInt16 aa; // [x, y, z] accel sensor measurements
- VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
- VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
- VectorFloat gravity; // [x, y, z] gravity vector
- float euler[3]; // [psi, theta, phi] Euler angle container
- float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
- // packet structure for InvenSense teapot demo
- uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
- double angleAy;// , gyroGx;
- double angleAx;// , gyroGy; //�������ĽǶȣ���x���нǣ��ͽ��ٶ�
- //MPU6050 accelgyro;//��������
- //int16_t ax, ay, az, gx, gy, gz;//������ԭʼ���� 3�����ٶ�+3�����ٶ�
- // ================================================================
- // === INTERRUPT DETECTION ROUTINE ===
- // ================================================================
- volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
- void dmpDataReady() {
- mpuInterrupt = true;
- }
- void mpu_Setup() {
- // join I2C bus (I2Cdev library doesn't do this automatically)
- #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
- Wire.begin();
- Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
- #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
- Fastwire::setup(400, true);
- #endif
- // initialize serial communication
- // (115200 chosen because it is required for Teapot Demo output, but it's
- // really up to you depending on your project)
- Serial.begin(115200);
- while (!Serial); // wait for Leonardo enumeration, others continue immediately
- // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
- // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
- // the baud timing being too misaligned with processor ticks. You must use
- // 38400 or slower in these cases, or use some kind of external separate
- // crystal solution for the UART timer.
- // initialize device
- Serial.println(F("Initializing I2C devices..."));
- mpu.initialize();
- pinMode(INTERRUPT_PIN, INPUT);
- // verify connection
- Serial.println(F("Testing device connections..."));
- Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
- // wait for ready
- Serial.println(F("\nSend any character to begin DMP programming and demo: "));
- while (Serial.available() && Serial.read()); // empty buffer
- while (!Serial.available()); // wait for data
- while (Serial.available() && Serial.read()); // empty buffer again
- // load and configure the DMP
- Serial.println(F("Initializing DMP..."));
- devStatus = mpu.dmpInitialize();
- // supply your own gyro offsets here, scaled for min sensitivity
- mpu.setXGyroOffset(220);
- mpu.setYGyroOffset(76);
- mpu.setZGyroOffset(-85);
- mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
- // make sure it worked (returns 0 if so)
- if (devStatus == 0) {
- // turn on the DMP, now that it's ready
- Serial.println(F("Enabling DMP..."));
- mpu.setDMPEnabled(true);
- // enable Arduino interrupt detection
- Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
- attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
- mpuIntStatus = mpu.getIntStatus();
- // set our DMP Ready flag so the main loop() function knows it's okay to use it
- Serial.println(F("DMP ready! Waiting for first interrupt..."));
- dmpReady = true;
- // get expected DMP packet size for later comparison
- packetSize = mpu.dmpGetFIFOPacketSize();
- }
- else {
- // ERROR!
- // 1 = initial memory load failed
- // 2 = DMP configuration updates failed
- // (if it's going to break, usually the code will be 1)
- Serial.print(F("DMP Initialization failed (code "));
- Serial.print(devStatus);
- Serial.println(F(")"));
- }
- // configure LED for output
- pinMode(LED_PIN, OUTPUT);
- }
- void getData() {
- // if programming failed, don't try to do anything
- if (!dmpReady) return;
- // wait for MPU interrupt or extra packet(s) available
- while (!mpuInterrupt && fifoCount < packetSize) {
- // other program behavior stuff here
- // .
- // .
- // .
- // if you are really paranoid you can frequently test in between other
- // stuff to see if mpuInterrupt is true, and if so, "break;" from the
- // while() loop to immediately process the MPU data
- // .
- // .
- // .
- }
- // reset interrupt flag and get INT_STATUS byte
- mpuInterrupt = false;
- mpuIntStatus = mpu.getIntStatus();
- // get current FIFO count
- fifoCount = mpu.getFIFOCount();
- // check for overflow (this should never happen unless our code is too inefficient)
- if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
- // reset so we can continue cleanly
- mpu.resetFIFO();
- Serial.println(F("FIFO overflow!"));
- // otherwise, check for DMP data ready interrupt (this should happen frequently)
- }
- else if (mpuIntStatus & 0x02) {
- // wait for correct available data length, should be a VERY short wait
- while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
- // read a packet from FIFO
- mpu.getFIFOBytes(fifoBuffer, packetSize);
- // track FIFO count here in case there is > 1 packet available
- // (this lets us immediately read more without waiting for an interrupt)
- fifoCount -= packetSize;
- //#ifdef OUTPUT_READABLE_YAWPITCHROLL
- // display Euler angles in degrees
- mpu.dmpGetQuaternion(&q, fifoBuffer);
- mpu.dmpGetGravity(&gravity, &q);
- mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
- //Serial.print("ypr\t");
- //Serial.print(ypr[0] * 180 / M_PI);
- //Serial.print("\t");
- //Serial.print(ypr[1] * 180 / M_PI);
- angleAx = ypr[1] * 180 / M_PI - 0.25;
- //Serial.print("\t");
- //Serial.println(ypr[2] * 180 / M_PI);
- angleAy = ypr[2] * 180 / M_PI - 0.7;
- //#endif
- // blink LED to indicate activity
- blinkState = !blinkState;
- digitalWrite(LED_PIN, blinkState);
- }
- }
- //==================PID================
- int pid_Sample_Time = 10;
- double x_mae_pid_out, x_ushiro_pid_out, y_migi_pid_out, y_hitari_pid_out;
- //double Px,Ix,Dx,Py,Iy,Dy;
- double Px = 0;
- double Ix = 0;
- double Dx = 0;
- //double Px_ = 0.5;
- //double Ix_ = 0;
- //double Dx_ = 0;
- double Py = 0;
- double Iy = 0;
- double Dy = 0;
- //double Py_ = 0.5;
- //double Iy_ = 0;
- //double Dy_ = 0;
- double x_set_angle = 0;
- double y_set_angle = 0;
- PID x_mae_PID(&angleAx, &x_mae_pid_out, &x_set_angle, Px, Ix, Dx, DIRECT);
- //PID x_ushiro_PID(&angleAx, &x_ushiro_pid_out, &x_set_angle, Py_, Iy_, Dy_, REVERSE);
- PID y_hitari_PID(&angleAy, &y_hitari_pid_out, &y_set_angle, Py, Iy, Dy, DIRECT);
- //PID y_migi_PID(&angleAy, &y_migi_pid_out, &y_set_angle, Py_, Iy_, Dy_, REVERSE);
- void pid_Config(int _limit) {
- x_mae_PID.SetMode(AUTOMATIC);//pid mode
- //x_ushiro_PID.SetMode(AUTOMATIC);//pid mode
- x_mae_PID.SetOutputLimits(0, _limit);//range
- //x_ushiro_PID.SetOutputLimits(0, out_limit);//range
- //x_ushiro_PID.SetSampleTime(pid_Sample_Time);
- x_mae_PID.SetSampleTime(pid_Sample_Time);
- y_hitari_PID.SetMode(AUTOMATIC);//pid mode
- //y_migi_PID.SetMode(AUTOMATIC);//pid mode
- y_hitari_PID.SetOutputLimits(0, _limit);//range
- //y_migi_PID.SetOutputLimits(0, out_limit);//range
- y_hitari_PID.SetSampleTime(pid_Sample_Time);
- //y_migi_PID.SetSampleTime(pid_Sample_Time);
- }
- //===================PWM============
- int val1;
- int unlock = 0;
- int myangle1 = 0;
- int out_limit = 40;
- int basic_Power = 0;
- int motor_PWM[4] = { 0,0,0,0 };
- int x_ushiro_fix = 0.7;
- void servopulse(int val1)//����һ�����庯��
- {
- myangle1 = map(val1, 0, 180, 500, 2480);
- digitalWrite(MAE, HIGH);//�������ӿڵ�ƽ����
- digitalWrite(MIGI, HIGH);
- digitalWrite(USHIRO, HIGH);
- digitalWrite(HITARI, HIGH);
- delayMicroseconds(myangle1);//��ʱ����ֵ��΢����
- digitalWrite(MAE, LOW);//�������ӿڵ�ƽ����
- digitalWrite(MIGI, LOW);
- digitalWrite(USHIRO, LOW);
- digitalWrite(HITARI, LOW);
- delay(20 - val1 / 1000);
- }
- void ESC_Prepare(int ESC_mode) {
- //������˵���飬���������г�ʱ��һ��ʼ��Ҫ��ң�����������ߵ㡣i<=110������2�¶�
- if (ESC_mode == 0) {//���������г̣�
- for (int i = 0; i <= 110; i++)
- {
- servopulse(150);//�������庯��
- }
- }
- //�ȵ�������B-B�����󣨾��������󣬴��ž��������ˣ������Ŵ������͵�
- for (int i = 0; i <= 55; i++)
- {
- servopulse(20);//�������庯��
- }
- //������ʾ�󣬾Ϳ��Կ��Ƶ���ת����
- delay(1000);
- }
- void motor_Prepare() {
- pinMode(MAE, OUTPUT);
- pinMode(USHIRO, OUTPUT);
- pinMode(HITARI, OUTPUT);
- pinMode(MIGI, OUTPUT);
- //����analogWrite()��������PWM��Ƶ��Ϊ 122Hz
- TCCR1B = TCCR1B & 0xF8 | 4; // 122.549
- TCCR2B = TCCR2B & 0xF8 | 6; //122.549
- //����analogWrite()��������PWM��Ƶ��Ϊ defaultValue
- // TCCR1B = TCCR1B & 0xF8 | 3; /// 490.196
- // TCCR2B = TCCR2B & 0xF8 | 4; ///490.196
- }
- void PWM_Update() {
- if (angleAx>=0){
- x_mae_PID.SetControllerDirection(REVERSE);
- x_mae_PID.Compute();
- motor_PWM[0] = basic_Power - x_mae_pid_out; //x + mae
- motor_PWM[1] = basic_Power + x_mae_pid_out-x_ushiro_fix;//x - ushiro
- }
- else {
- x_mae_PID.SetControllerDirection(DIRECT);
- x_mae_PID.Compute();
- motor_PWM[0] = basic_Power + x_mae_pid_out; //x + mae
- motor_PWM[1] = basic_Power - x_mae_pid_out+x_ushiro_fix;//x - ushiro
- }
- if (angleAy>=0) {
- y_hitari_PID.SetControllerDirection(REVERSE);
- y_hitari_PID.Compute();
- motor_PWM[2] = basic_Power - y_hitari_pid_out;//y + hitari
- motor_PWM[3] = basic_Power + y_hitari_pid_out;//y - migi
- }
- else {
- y_hitari_PID.SetControllerDirection(DIRECT);
- y_hitari_PID.Compute();
- motor_PWM[2] = basic_Power + y_hitari_pid_out;//y + hitari
- motor_PWM[3] = basic_Power - y_hitari_pid_out;//y - migi
- }
- //calc
- //x_ushiro_PID.Compute();//calc
- //y_migi_PID.Compute();
- //Serial.print(motor_PWM[0]); Serial.println(',');
- // Serial.println(motor_PWM[1]);
- for (size_t i = 0; i < 4; i++)
- {
- if (motor_PWM[i] < 0)
- {
- motor_PWM[i] = 0;//�����޷�
- }
- else if (motor_PWM[i] > 50)
- {
- motor_PWM[i] = 50;//�����޷�
- }
- }
- if (unlock)
- {
-
- analogWrite(MAE, (int)motor_PWM[0]);
- analogWrite(USHIRO, (int)motor_PWM[1]);
- analogWrite(HITARI, (int)motor_PWM[2]);
- analogWrite(MIGI, (int)motor_PWM[3]);
- }
- else
- {
- basic_Power = 0;
- analogWrite(MAE, 0);
- analogWrite(USHIRO, 0);
- analogWrite(HITARI, 0);
- analogWrite(MIGI, 0);
- }
- }
- //------------------DEBUG----------------
- int data = 0;
- int debug_Time = 500;
- void bluetooth_RC()
- {
- int flag = 0;
- while (Serial.available() > 0) {
- data = Serial.parseInt();
- if (Serial.read() == 'X') {
- switch (flag)
- {
- case 0: {
- // vrx = data;
- flag = 1;
- Px = (double)(data / 1000.0);
- //Serial.println(data);
- break;
- }
- case 1: {
- Dx = (double )(data / 1000.0);
- flag = 2;
- // Serial.println(data);
- break;
- }
- case 2: {
- basic_Power = data;
- flag = 3;
- // Serial.println(data);
- break;
- }
- case 3: {
- unlock = data;//Py = data/100;
- flag = 4;
- break;
- }
- case 4: {
- //int Dx = data / 100.0;
- if (data)x_mae_PID.SetTunings(Px, Ix, Dx);
- flag = 5;
- break;
- }
- case 5: {
- Ix =data/1000.0;
- x_mae_PID.SetTunings(Px, Ix, Dx);
- flag = 0;
- break;
- }
- // case 6: {
- // Px = data/100.0;
- // flag = 7;
- // break;
- // }
- // case 7: {
- // Ix = data/100.0;
- // flag = 8;
- // break;
- // }
- // case 8: {
- // Dx = data/100.0;
- // flag = 0;
- // break;
- // }
- }
- }
- }
- }
- //=============================
- void printout()
- {
- Serial.print(angleAx); Serial.print(',');
- Serial.print(angleAy); Serial.print(',');
- Serial.print(Px); Serial.print(',');
- Serial.print(Dx); Serial.println(',');
- Serial.print(motor_PWM[0]); Serial.println(',');
- Serial.print(motor_PWM[1]); Serial.println(" ");
- Serial.print(motor_PWM[2]); Serial.println(',');
- Serial.print(motor_PWM[3]); Serial.println(" ");
- //Serial.print(angle2);Serial.print(',');
- // Serial.print(gx/131.00);Serial.print(',');
- // Serial.println(angle);//Serial.print(',');
- // Serial.println(Output);
- }
- void setup() {
- pid_Config(20);//pid init
- motor_Prepare();
- ESC_Prepare(0);
- int tickEvent1 = t.every(5, getData); //������ִ���Ժ�timeChange����ִ�лص�����getangle
- int tickEvent2 = t.every(50, printout); //������ִ���Ժ�50����ִ�лص�����printout����������
- int tickEvent3 = t.every(pid_Sample_Time, PWM_Update);//1oms һ��pid
- int tickEvent4 = t.every(debug_Time, bluetooth_RC);
- mpu_Setup();
- }
- void loop() {
- t.update();//ʱ������ϵͳ����
- //getData();
- //printout();
- }
复制代码 |
本帖子中包含更多资源
您需要 登录 才可以下载或查看,没有帐号?注册
x
|