自平衡机器人算法实现 DF创客社区

大家好，我是这里的新人，是一名机械工程的学生，最近想用自平衡机器人实现自动化算法，但是在实际操作过程中遇到了一些问题，希望有专业知识的朋友可以指点一下

小车先读取MPU6050模块上的角度及角速度，还有马达上编码器上读速度读数，以这些度数乘以一些系数，获得马达所需电压，然后用adafruit motor sheild控制输出电压。但是遇到以下问题

给马达零电压，也就是不控制马达是，能够读出角度及角速度，但是一旦控制马达有转速时。会产生I2C read failuer, 或者I2C timeout的错误提示，然后程序就不跑了。

我附上自己写的代码，请各位帮助看一下，由于自己是机械学生，代码能力有限，请各位见谅。

#include <Wire.h>
#include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter


#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_PWMServoDriver.h"
//**********define motors****************//
Adafruit_MotorShield AFMS = Adafruit_MotorShield();

Adafruit_DCMotor *M_L = AFMS.getMotor(1);
Adafruit_DCMotor *M_R = AFMS.getMotor(2);

#define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf

Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;

//**************  1 ------ Left     2----------Right   ******************

//*********Motor Left defines*****************
#define encodPinA1      12                       // encoder A pin
#define encodPinB1      11                       // encoder B pin

//*********Motor Right defines*****************
#define encodPinA2      3                       // encoder A pin
#define encodPinB2      4                       // encoder B pin




#define CURRENT_LIMIT   1000                     // high current warning
#define LOW_BAT         10000                   // low bat warning
#define LOOPTIME        10                     // PID loop time
#define NUMREADINGS     10                      // samples for Amp average




 int encoder0PinALast_1 = LOW;
 int n_1 = LOW;
 int encoder0PinALast_2 = LOW;
 int n_2 = LOW;

 
int readings[NUMREADINGS];
unsigned long lastMilli = 0;                    // loop timing 
unsigned long lastMilliPrint = 0;               // loop timing
float speed_act_1 = 0;                              // speed (actual value)
float speed_act_2 = 0;                              // speed (actual value)
int voltage = 0;                                // in mV
int current = 0;                                // in mA
volatile long count_1 = 0;                        // rev counter of Left Motor
volatile long count_2 = 0;                        // rev counter of Right Motor

//********************gain design ************************************
float K1_1 = -0.178184631752522;
float K1_2 = -0.071916510883366;
float K1_3 = -0.098188582242225;
float K1_4 = -0.005587314894462;
float K2_1 = -0.003404946688540;
float K2_2 = -7.908193215839999e-04;
// ********motor control*******
//***************motor gain*******
float Kp_m = 2;
float Ki_m= 100;
//*********motor input***********;
float vol_in_L;
float vol_in_R;
float w_dot_req_L;
float w_dot_req_R;
float w_dot_act_L;
float w_dot_act_R;

//****************CONTROL SIGNAL***********************************
float X_in = 0;
float X_dot_in = 0;
float yaw_in = 0;
float yaw_dot_in = 0;

//*****************Parameters*************
float wheel_r = 0.045085; //wheel radius meter
float pi = 3.1415926;
float D = 0.24384; //m lateral distance between the contact patches of the wheel
float deg2rad = 2*pi/(360);
float rad2deg = 360/(2*pi);
float J = 0.0079; // moment of inertia about rotation axis

//************States**********
float X_act;
float X_act_dot;
float yaw_act;
float yaw_act_dot;

//***************system control input********
float torque_sum;
float torque_min;
float torque_L;
float torque_R;

//****************debug*************print
 float error_w_dot_L;
 float error_w_dot_R;
float int_error_w_dot_L = 0;
float error_w_dot_L_old = 0;
float int_error_w_dot_R = 0;
float error_w_dot_R_old = 0;


/* 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


uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
// TODO: Make calibration routine

void setup() {
  AFMS.begin();
  Serial.begin(9600);
  Wire.begin();
#if Arduino >= 157
  Wire.setClock(400000UL); // Set I2C frequency to 400kHz
#else
  TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
#endif

  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(100); // Wait for sensor to stabilize

  /* Set kalman and gyro starting angle */
  while (i2cRead(0x3B, i2cData, 6));
  accX = (i2cData[0] << 8) | i2cData[1];
  accY = (i2cData[2] << 8) | i2cData[3];
  accZ = (i2cData[4] << 8) | i2cData[5];


  // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
  // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
  // It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
  double roll  = atan2(accY, accZ) * RAD_TO_DEG;
  double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
  double roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
  double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif

  kalmanX.setAngle(roll); // Set starting angle
  kalmanY.setAngle(pitch);
//  gyroXangle = roll;
//  gyroYangle = pitch;
//  compAngleX = roll;
//  compAngleY = pitch;

  timer = micros();

  analogReference(EXTERNAL);                            // Current external ref is 3.3V
  pinMode(encodPinA1, INPUT); 
  pinMode(encodPinB1, INPUT); 
  digitalWrite(encodPinA1, HIGH);                      // turn on pullup resistor
  digitalWrite(encodPinB1, HIGH);

  pinMode(encodPinA2, INPUT); 
  pinMode(encodPinB2, INPUT); 
  digitalWrite(encodPinA2, HIGH);                      // turn on pullup resistor
  digitalWrite(encodPinB2, HIGH);
  
  attachInterrupt(1, rencoder, FALLING);
  for(int i=0; i<NUMREADINGS; i++)   readings[i] = 0;  // initialize readings to 0

  
}

void loop() {
  
 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();

#ifdef RESTRICT_PITCH // Eq. 25 and 26
  double roll  = atan2(accY, accZ) * RAD_TO_DEG;
  double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
  double roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
  double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif

  double gyroXrate = gyroX / 131.0; // Convert to deg/s
  double gyroYrate = gyroY / 131.0; // Convert to deg/s

#ifdef RESTRICT_PITCH
  // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
    kalmanX.setAngle(roll);
    compAngleX = roll;
    kalAngleX = roll;
    gyroXangle = roll;
  } else
    kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter

  if (abs(kalAngleX) > 90)
    gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
  kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
  // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
    kalmanY.setAngle(pitch);
    compAngleY = pitch;
    kalAngleY = pitch;
    gyroYangle = pitch;
  } else
    kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter

  if (abs(kalAngleY) > 90)
    gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
  kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif

  gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
  gyroYangle += gyroYrate * dt;

  // Reset the gyro angle when it has drifted too much
  if (gyroXangle < -180 || gyroXangle > 180)
    gyroXangle = kalAngleX;
  if (gyroYangle < -180 || gyroYangle > 180)
    gyroYangle = kalAngleY;
    if((millis()-lastMilli) >= LOOPTIME){
     lastMilli=millis();
    getMotorData();
    X_act = count_1*2*pi/1600*wheel_r;
    X_act_dot = speed_act_1*2*pi*wheel_r/60;
    yaw_act = (count_1-count_2)*2*pi*wheel_r/(16*30*D);
    yaw_act_dot = (speed_act_1-speed_act_2)*2*pi*wheel_r/(16*30*D);
    torque_sum = -(K1_1*(X_act-X_in)-K1_2*(X_act_dot-X_dot_in)-K1_3*(kalAngleY*deg2rad)-K1_4*gyroYrate*deg2rad);
    torque_min = 0; // -(K2_1*(yaw_act-yaw_in)-K2_2*(yaw_act_dot-yaw_dot_in));
    torque_L = (torque_sum+torque_min)/2;
    torque_R = torque_sum-torque_L;
    w_dot_req_L = torque_L/J;
    w_dot_req_R = torque_R/J;
    error_w_dot_L = w_dot_req_L-w_dot_act_L;
    error_w_dot_R = w_dot_req_R-w_dot_act_R;
    int_error_w_dot_L = int_error_w_dot_L+error_w_dot_L*(LOOPTIME/1000);
    int_error_w_dot_R = int_error_w_dot_R+error_w_dot_R*(LOOPTIME/1000);
    vol_in_L = error_w_dot_L*Kp_m+int_error_w_dot_L*Ki_m;
    vol_in_R = error_w_dot_R*Kp_m+int_error_w_dot_R*Ki_m;
      if(vol_in_L>0) {
      M_L->run(FORWARD);
      }
      if(vol_in_L<0) {
      M_L->run(BACKWARD);
      }
      if(vol_in_R>0) {
      M_R->run(FORWARD);
      }
      if(vol_in_R<0) {
      M_R->run(BACKWARD);
      } 
  
  PWM_val_1 = volt2pwm(abs(vol_in_L));
  PWM_val_2 = volt2pwm(abs(vol_in_R));
  M_L->setSpeed(PWM_val_1);
  M_R->setSpeed(PWM_val_2);
    printdata();
    }
   
}

void printdata(){
  //Serial.print("KalAngleY = ");
  Serial.print(PWM_val_1); Serial.print(" ");
   Serial.println(PWM_val_2);
//  Serial.print("\t");
}

void rencoder()  {                                   
n_1 = digitalRead(encodPinA1);
if ((encoder0PinALast_1 == LOW) && (n_1 == HIGH)){
  if (digitalRead(encodPinB1) == LOW) {
  count_1--;
  } else{
    count_1++;
    }
}
encoder0PinALast_1 = n_1;

n_2 = digitalRead(encodPinA2);
if ((encoder0PinALast_2 == LOW) && (n_2 == HIGH)){
  if (digitalRead(encodPinB2) == LOW) {
  count_2--;
  } else{
    count_2++;
    }
}
encoder0PinALast_2 = n_2;
}

void getMotorData()  {                                                    
static long countAnt_1 = 0;
static long countAnt_2 = 0;
static float speed_act_1_old = 0;
static float speed_act_2_old = 0;
static float speed_act_1_oold = 0;
static float speed_act_2_oold = 0;
  speed_act_1 = (count_1 - countAnt_1)*2*pi*1000/(16*30*LOOPTIME);//   radia/s
  countAnt_1 = count_1;
  speed_act_2 = (count_2 - countAnt_2)*2*pi*1000/(16*30*LOOPTIME);
  countAnt_2 = count_2;
  w_dot_act_L = (3*speed_act_1-4*speed_act_1_old+speed_act_1_oold)*1000/(LOOPTIME);
  w_dot_act_R = (3*speed_act_2-4*speed_act_2_old+speed_act_2_oold)*1000/(LOOPTIME);
  speed_act_1_old = speed_act_1;
  speed_act_1_oold = speed_act_1_old;
  speed_act_2_old = speed_act_2;
  speed_act_2_oold = speed_act_2_old;
}

//void MotorVoltageUpdate ()  {
//
//
//  
//  error_w_dot_L = w_dot_req_L-w_dot_act_L;
//  error_w_dot_R = w_dot_req_R-w_dot_act_R;
//  int_error_w_dot_L = int_error_w_dot_L+error_w_dot_L*(LOOPTIME/1000);
//  int_error_w_dot_R = int_error_w_dot_R+error_w_dot_R*(LOOPTIME/1000);
//  vol_in_L = error_w_dot_L*Kp_m+int_error_w_dot_L*Ki_m;
//  vol_in_R = error_w_dot_R*Kp_m+int_error_w_dot_R*Ki_m;
//
//    if(vol_in_L>0) {
//      digitalWrite(InA1, LOW);
//      digitalWrite(InB1, HIGH);
//      }
//      if(vol_in_L<0) {
//        digitalWrite(InA1,HIGH);
//        digitalWrite(InB1, LOW);
//      }
//      if(vol_in_R>0) {
//      digitalWrite(InA2, LOW);
//      digitalWrite(InB2, HIGH);
//      }
//      if(vol_in_R<0) {
//        digitalWrite(InA2,HIGH);
//        digitalWrite(InB2, LOW);
//      } 
//  
//  PWM_val_1 = volt2pwm(abs(vol_in_L));
//  PWM_val_2 = volt2pwm(abs(vol_in_R));
//  
//}

int volt2pwm(float vol){
  int pwm;
  pwm = 0.6272*vol*vol*vol-6.475*vol*vol+27.72*vol+11.53;
  return constrain(pwm,0,255);
}
复制代码