- #include "Wire.h"`
- const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB
- const uint16_t I2C_TIMEOUT = 100; // Used to check for errors in I2C communication
- uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {
- return i2cWrite(registerAddress, &data, 1, sendStop); // Returns 0 on success
- }
- uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) {
- Wire.beginTransmission(IMUAddress);
- Wire.write(registerAddress);
- Wire.write(data, length);
- uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success
- if (rcode) {
- Serial.print(F("i2cWrite failed: "));
- Serial.println(rcode);
- }
- return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
- }
- uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {
- uint32_t timeOutTimer;
- Wire.beginTransmission(IMUAddress);
- Wire.write(registerAddress);
- uint8_t rcode = Wire.endTransmission(false); // Don't release the bus
- if (rcode) {
- Serial.print(F("i2cRead failed: "));
- Serial.println(rcode);
- return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
- }
- Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated start and then release the bus after reading
- for (uint8_t i = 0; i < nbytes; i++) {
- if (Wire.available())
- data[i] = Wire.read();
- else {
- timeOutTimer = micros();
- while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());
- if (Wire.available())
- data[i] = Wire.read();
- else {
- Serial.println(F("i2cRead timeout"));
- return 5; // This error value is not already taken by endTransmission
- }
- }
- }
- return 0; // Success
- }
- /******************************************************/
- //2560 pin map 引腳定義好即可，然后改變一下PID的幾個值（kp，kd，ksp，ksi）即可，剩下的全部都是固定的程序，
- //可能小車會有一點重心不在中點的現象，加一下角度值或者減一點即可
- //至于每個MPU6050的誤差，自己調節一下即可，不是很難
- //調試時先將速度環的ksp，ksi=0，調到基本可以站起來，然后可能會出現倒，或者自動跑起來的時候加上速度環
- //這時就會很穩定的站起來，然后用小力氣的手推不會倒。
- int ENA=11;
- int ENB=12;
- int IN1=4;
- int IN2=5;
- int IN3=6;
- int IN4=7;
- int MAS,MBS;
- /* IMU Data */
- double accX, accY, accZ;
- double gyroX, gyroY, gyroZ;
- int16_t tempRaw;
- double gyroXangle, gyroYangle; // Angle calculate using the gyro only
- double compAngleX, compAngleY; // Calculated angle using a complementary filter
- double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
- uint8_t i2cData[14]; // Buffer for I2C data
- uint32_t timer;
- unsigned long lastTime;
- /***************************************/
- double P[2][2] = {{ 1, 0 },{ 0, 1 }};
- double Pdot[4] ={ 0,0,0,0};
- static const double Q_angle=0.001, Q_gyro=0.003, R_angle=0.5,dtt=0.005,C_0 = 1;
- double q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
- double angle,angle_dot,aaxdot,aax;
- double position_dot,position_dot_filter,positiono;
- /*-------------Encoder---------------*/
- #define LF 0
- #define RT 1
- //The balance PID
- float kp,kd,ksp,ksi;
- int Lduration,Rduration;
- boolean LcoderDir,RcoderDir;
- const byte encoder0pinA = 2;
- const byte encoder0pinB = 8;
- byte encoder0PinALast;
- const byte encoder1pinA = 3;
- const byte encoder1pinB = 9;
- byte encoder1PinALast;
- int RotationCoder[2];
- int turn_flag=0;
- float move_flag=0;
- float right_need = 0, left_need = 0;;
- int pwm;
- int pwm_R,pwm_L;
- float range;
- float range_error_all;
- float wheel_speed;
- float last_wheel;
- float error_a=0;
- void Kalman_Filter(double angle_m,double gyro_m)
- {
- angle+=(gyro_m-q_bias) * dtt;
- Pdot[0]=Q_angle - P[0][1] - P[1][0];
- Pdot[1]=- P[1][1];
- Pdot[2]=- P[1][1];
- Pdot[3]=Q_gyro;
- P[0][0] += Pdot[0] * dtt;
- P[0][1] += Pdot[1] * dtt;
- P[1][0] += Pdot[2] * dtt;
- P[1][1] += Pdot[3] * dtt;
- angle_err = angle_m - angle;
- PCt_0 = C_0 * P[0][0];
- PCt_1 = C_0 * P[1][0];
- E = R_angle + C_0 * PCt_0;
- K_0 = PCt_0 / E;
- K_1 = PCt_1 / E;
- t_0 = PCt_0;
- t_1 = C_0 * P[0][1];
- P[0][0] -= K_0 * t_0;
- P[0][1] -= K_0 * t_1;
- P[1][0] -= K_1 * t_0;
- P[1][1] -= K_1 * t_1;
- angle+= K_0 * angle_err;
- q_bias += K_1 * angle_err;
- angle_dot = gyro_m-q_bias;//也許應該用last_angle-angle
- }
-
- void setup() {
- Wire.begin();
- Serial.begin(9600);
- pinMode(IN1, OUTPUT);
- pinMode(IN2, OUTPUT);
- pinMode(IN3, OUTPUT);
- pinMode(IN4, OUTPUT);
- pinMode(ENA, OUTPUT);
- pinMode(ENB, OUTPUT);
- TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz
-
- i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
- i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
- i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
- i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
- while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
- while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
-
- while (i2cRead(0x75, i2cData, 1));
- if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
- Serial.print(F("Error reading sensor"));
- while (1);
- }
- delay(20); // Wait for sensor to stabilize
- while (i2cRead(0x3B, i2cData, 6));
- accX = (i2cData[0] << 8) | i2cData[1];
- accY = (i2cData[2] << 8) | i2cData[3];
- accZ = (i2cData[4] << 8) | i2cData[5];
- double roll = atan2(accX, accZ) * RAD_TO_DEG;
- EnCoderInit();
- timer = micros();
- //The balance PID
- kp= 42;//24.80;43
- kd= 1.9;//9.66;1.4
- ksp= 8.5;//4.14;
- ksi= 2.1;//0.99; 0.550
- }
- void EnCoderInit()
- {
- pinMode(8,INPUT);
- pinMode(9,INPUT);
- attachInterrupt(LF, LwheelSpeed, RISING);
- attachInterrupt(RT, RwheelSpeed, RISING);
- }
- void pwm_calculate()
- {
- unsigned long now = millis(); // 當前時間(ms)
- int Time = now - lastTime;
- int range_error;
- range += (Lduration + Rduration) * 0.5;
- range *= 0.9;
- range_error = Lduration - Rduration;
- range_error_all += range_error;
-
- wheel_speed = range - last_wheel;
- //wheel_speed = constrain(wheel_speed,-800,800);
- //Serial.println(wheel_speed);
- last_wheel = range;
- pwm = (angle + 0.825) * kp + angle_dot * kd + range * ksp + wheel_speed * ksi;
- if(pwm > 255)pwm = 255;
- if(pwm < -255)pwm = -255;
-
- if(turn_flag == 0)
- {
- pwm_R = pwm + range_error_all;
- pwm_L = pwm - range_error_all;
- }
- else if(turn_flag == 1) //左轉
- {
- pwm_R = pwm ; //Direction PID control
- pwm_L = pwm + left_need * 68;
- range_error_all = 0; //clean
- }
- else if(turn_flag == 2)
- {
- pwm_R = pwm + right_need * 68; //Direction PID control
- pwm_L = pwm ;
- range_error_all = 0; //clean
- }
-
- Lduration = 0;//clean
- Rduration = 0;
- lastTime = now;
- }
- void PWMD()
- {
- if(pwm>0)
- {
- digitalWrite(IN1, HIGH);
- digitalWrite(IN2, LOW);
- digitalWrite(IN3, LOW);
- digitalWrite(IN4, HIGH);
- }
- else if(pwm<0)
- {
- digitalWrite(IN1, LOW);
- digitalWrite(IN2, HIGH);
- digitalWrite(IN3, HIGH);
- digitalWrite(IN4, LOW);
- }
- int PWMr = abs(pwm);
- int PWMl = abs(pwm);
-
- analogWrite(ENB, max(PWMl,60)); //PWM調速a==0-255 51
- analogWrite(ENA, max(PWMr,60)); //PWM調速a==0-255 54
-
- }
- void LwheelSpeed()
- {
- if(digitalRead(encoder0pinB))
- Lduration++;
- else Lduration--;
- }
- void RwheelSpeed()
- {
- if(digitalRead(encoder1pinB))
- Rduration--;
- else Rduration++;
- }
- void control()
- {
- if(Serial.available()){
- int val;
- val=Serial.read();
- switch(val){
- case 'w':
- if(move_flag<5)move_flag += 0.5;
- else move_flag = 0;
- break;
- case 's':
- if(move_flag<5)move_flag -= 0.5;
- else move_flag = 0;
- Serial.println("back");
- break;
- case 'a':
- turn_flag = 1;
- left_need = 0.6;
- Serial.println("zuo");
- break;
- case 'd':
- turn_flag = 2;
- right_need = 0.6;
- Serial.println("you");
- break;
- case 't':
- move_flag=0;
- turn_flag=0;
- right_need = left_need = 0;
- Serial.println("stop");
- break;
- default:
- break;
- }
- }
- }
- void loop()
- {
- //control();
- while (i2cRead(0x3B, i2cData, 14));
- accX = ((i2cData[0] << 8) | i2cData[1]);
- //accY = ((i2cData[2] << 8) | i2cData[3]);
- accZ = ((i2cData[4] << 8) | i2cData[5]);
- //gyroX = (i2cData[8] << 8) | i2cData[9];
- gyroY = (i2cData[10] << 8) | i2cData[11];
- //gyroZ = (i2cData[12] << 8) | i2cData[13];
- double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
- timer = micros();
-
- double roll = atan2(accX, accZ) * RAD_TO_DEG - move_flag;
- //double gyroXrate = gyroX / 131.0; // Convert to deg/s
- double gyroYrate = -gyroY / 131.0; // Convert to deg/s
- //gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
- //gyroYangle += gyroYrate * dt;
- Kalman_Filter(roll,gyroYrate);
- if(abs(angle)<35){
- //Serial.println(angle);
- pwm_calculate();
- PWMD();
- }
- else{
- analogWrite(ENB, 0); //PWM調速a==0-255
- analogWrite(ENA, 0); //PWM調速a==0-255
- }
- delay(2);
- }
器件：Arduino UNO或者Mega2560、LM298模塊，MPU6050傳感器、兩個編碼電機，
公母杜邦線，藍牙HT-06、鋰電池18650（4）——（成本大概200左右）
如果用手機控制可以下載應用寶里面的一個“小蜜蜂機器人”APP，個人覺得這個APP功能超強大，大神寫的APP就是屌！專門連接藍牙的串口助手，還有調試舵機的功能，簡直可以上天了。
調試過程中會學習到很多東西，希望大家可以看看，雖然我也做的很好，不過在這期間學習到了很多東西……
幾天就可以看到你想看到的現象喲，還等什么，開始動手吧，慢慢看資料慢慢理解（不對之處，敬請諒解）
——初學者心聲
http://v.youku.com/v_show/id_XMTUyODUwMDkxMg==.html
|
[復制鏈接]
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
