- 博客(0)
- 资源 (1)
空空如也
自平衡小车代码
是小车自平衡的代码,是arduino的,// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#include "Wire.h"`
/******************************************************/
//UNO pin map
int ENA=9;
int ENB=11;
int IN1=7;
int IN2=8;
int IN3=10;
int IN4=12;
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
int Lduration,Rduration;
boolean LcoderDir,RcoderDir;
const byte encoder0pinA = 2;
const byte encoder0pinB = 5;
byte encoder0PinALast;
const byte encoder1pinA = 3;
const byte encoder1pinB = 4;
byte encoder1PinALast;
int RotationCoder[2];
/*--------------------------------------*/
float k1,k2,k3,k4; //Adjust the PID Parameters
int turn_flag=0;
float move_flag=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 setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
Wire.begin();
// initialize serial communication
Serial.begin(9600);
//Serial.println("AT+NAMEitead");
//motor contraler
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
pinMode(ENA, OUTPUT);
pinMode(ENB, OUTPUT);
pinMode(6, OUTPUT);
2015-07-23
空空如也
TA创建的收藏夹 TA关注的收藏夹
TA关注的人