下载 >  网络技术 >  其它 > 自平衡小车代码

自平衡小车代码 评分:

是小车自平衡的代码,是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); digitalWrite(6, LOW);//disable buzzer /******************************************************/ //Serial.println("Initializing I2C devices..."); 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; //The balance PID k1=43;//24.80; k2=1.4;//9.66; k3=-0.88 ;//4.14; k4=-0.55;//0.99; /*************************************************************************************/ EnCoderInit(); timer = micros(); //CoderInit();//Initialize the module //Serial.println("BOOT.............."); } /*****************************main loop ****************************************************/ 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; /* Serial.print("roll:"); Serial.print(roll); Serial.print("\t"); Serial.print("gyroY:"); Serial.print(gyroY); Serial.print("\t"); Serial.print("gyroYangle:"); Serial.print(gyroYangle); Serial.print("\t"); Serial.print("compAngleX:"); Serial.print(compAngleX); Serial.print("\t"); Serial.print("kalAngleX:"); Serial.print(kalAngleX); Serial.print("\t"); */ /****************************************************/ Kalman_Filter(roll,gyroYrate); if(abs(angle)<35){ // Serial.print("kfAngle=");Serial.print(roll);//Serial.print("\t"); //Serial.print(",angleAx="); //Serial.print(angle); Serial.print("\t"); pwm_calculate(); PWMD(); Serial.print("pwm:"); Serial.print(pwm); Serial.print("\n"); //set_motor(); } else{ analogWrite(ENB, 0); //PWM调速a==0-255 analogWrite(ENA,0 ); //PWM调速a==0-255 } delay(2); Serial.print(";\t\n"); } /*************************************************************************/ 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 } // ************************** // Init the Incoder // ************************** void EnCoderInit() { //LcoderDir = true; //RcoderDir = true; pinMode(8,INPUT); pinMode(9,INPUT); attachInterrupt(LF, LwheelSpeed, RISING); attachInterrupt(RT, RwheelSpeed, RISING); } // ************************** // Calculate the pwm // ************************** void pwm_calculate() { unsigned long now = millis(); // 当前时间(ms) int Time = now - lastTime; int range_error; //Serial.print(" R:");Serial.print(Rduration); //Serial.print(" L:");Serial.print(Lduration); range+=(Lduration+Rduration)*0.5; range*=0.9; range_error=Lduration-Rduration; range_error_all+=range_error; wheel_speed=range-last_wheel; last_wheel=range; pwm=angle*k1+angle_dot*k2+range*k3+wheel_speed*k4; //use PID tho calculate the pwm if(pwm>255) //Maximum Minimum Limitations pwm=255; if(pwm<-255) pwm=-255; //Serial.print(pwm);Serial.print("\t"); Serial.print(Lduration);Serial.print("\t"); Serial.print(Rduration);Serial.print("\t\n"); /* if(turn_flag==0) { pwm_R=pwm+range_error_all; pwm_L=pwm-range_error_all; } else { pwm_R=pwm-turn_flag*68; //Direction PID control pwm_L=pwm+turn_flag*68; range_error_all=0; //clean } */ Lduration = 0;//clean Rduration = 0; lastTime = now; //Serial.print(Time);Serial.print("\n"); } /************************************************************/ void PWMD() { if(pwm>0) { digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW); digitalWrite(IN3, LOW); digitalWrite(IN4, HIGH); Serial.print("..."); } else if(pwm<0) { digitalWrite(IN1, LOW); digitalWrite(IN2, HIGH); digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW); Serial.print("***"); } int PWMr = abs(pwm); int PWMl = abs(pwm); analogWrite(ENB, PWMl); //PWM调速a==0-255 analogWrite(ENA, PWMr ); //PWM调速a==0-255 } // ******************************* // Read the Speed from the Encoder // ******************************* void LwheelSpeed() { if(digitalRead(encoder0pinB)) Lduration++; else Lduration--; /* int Lstate = digitalRead(encoder0pinA); if((encoder0PinALast == LOW)&&Lstate==HIGH) { int val = digitalRead(encoder0pinB); if(val == LOW && LcoderDir) LcoderDir = false; //Lreverse else if(val == HIGH && !LcoderDir) LcoderDir = true; //Lforward } encoder0PinALast = Lstate; if(!LcoderDir) Lduration++; else Lduration--; */ } void RwheelSpeed() { if(digitalRead(encoder1pinB)) Rduration--; else Rduration++; /* int Rstate = digitalRead(encoder1pinA); if((encoder1PinALast == LOW)&&Rstate==HIGH) { int val = digitalRead(encoder1pinB); if(val == LOW && RcoderDir) RcoderDir = false; //Rreverse else if(val == HIGH && !RcoderDir) RcoderDir = true; //Rforward } encoder1PinALast = Rstate; if(!RcoderDir) Rduration--; else Rduration++; */ } void control() { if(Serial.available()){ int val; val=Serial.read(); switch(val){ case 'F': if(move_flag<5) move_flag+=0.1; Serial.println("forword"); break; case 'B': if(move_flag>-5)move_flag-=0.1; Serial.println("back"); break; case 'S': move_flag=0; turn_flag=0; Serial.println("stop"); break; default: break; } } }
...展开详情收缩
2015-07-23 上传大小:5KB
分享
收藏 举报
自己写的,平衡小车代码,亲测有效

前几天一直在整平衡小车,想用这个理解一下pid,并调一调带编码盘的电机。 首先谈谈我对pid算法的理解,刚开始以为平衡小车是采取的串级pid来是小车平衡,就是通过角度算出此时的pwm,在通过这个pwm去当作速度环的入口参数,通过编码盘来获取小车的速度控制小车由角度环指定速度下行驶。但通过实践并不是这样,速度环和角度环应该是两个独立的系统,角度环用来让小车快速的恢复到平衡状态,如果初始化陀螺仪时陀螺仪的角度正好重力重合,那么你的小车就会平衡,否则就会向一个方向加速,只至倒下,加上速度环就会解决这一问题,速度环是为了消除小车的位移,让小车能够在一定的位移范围内调整角度,如果下车像一个方向一直加速

立即下载
两轮自平衡小车代码

基于51单片机的两轮自平衡小车的所有源代码。

立即下载
【单片机项目】平衡小车(一) 控制流程
平衡小车之PID算法
平衡小车程序

可以通过程序进行平衡小车的制作以及自己设计平衡小车

立即下载
平衡小车的32代码程序

对于平衡车的代码

立即下载
平衡车源代码

提供小车代码,以供大家学习。。。

立即下载
写一点我自己做平衡小车的经验吧
二轮平衡车全套源码(超全)

这是本科期间做的平衡车资料,软件(调试通过、无错误)、硬件资料全都有,可以用它DIY自己的平衡小车。内容包含所有平衡小车相关资料(硬件(stm32)、源码、电路图、连线方法等);源码包含括互补滤波版、卡尔曼滤波板、DMP版,都是编译通过的。想自己DIY平衡车的赶快行动起来。

立即下载
平衡小车制作制作经验分享
51单片机两轮自平衡小车全套设计方案

基于51单片机的两轮自平衡小车全套设计方案,其中包括源代码 app 电路等资源。

立即下载
基于51单片机的两轮平衡车程序

里面包含PWM,PID,卡尔曼滤波等程序资料

立即下载
51做平衡

其实程序过程很简单,先是采集传感器MPU6050的数据,并进行加工,获得当前小车倾斜角,然后根据此角度做PID调节,得到小车两个电机的PWM脉宽,调整轮子速度,使之回到倾角为0的状态,即保持平衡。然后就不停地重复采集->处理->调节->处理这一过程。 在此基础上,附加两电机的PWM值,即可实现前进,倒退,左转,右转动作

立即下载
平衡小车源码(精简入门版)

软件设计中主要有MPU-6050传感器数据的滤波处理、电机PID控制、编码器测速、超声波测距、蓝牙通信、OLED显示以及主电源的电压测量等。同时作品也可以实现遥控功能,只需将手机APP与作品上的蓝牙模块连接即可实现控制。代码注释特别详细,适合PID入门

立即下载
两轮自平衡小车的设计与实现

摘 要: 两轮自平衡小车被各种竞赛所喜爱,在很多大型比赛中都有涉及此方向的题目。阐述了基于飞思卡尔系列单片机为核心控制器,配合陀螺仪、角度传感器实现两轮小车的自平衡。着重介绍了两轮自平 衡小车的结构、平衡原理、控制算法,通过反复调试最终实现了小车的自平衡。

立即下载
两轮平衡小车(K5环境+STM32+MPU6050+卡尔曼滤波源码+汉语注释)

两轮平衡小车(K5环境+STM32+MPU6050+卡尔曼滤波源码)

立即下载
STM32平衡小车程序

基于stm32单片机的两轮平衡小车,可以实现前进、后退、左转、右转。

立即下载
平衡小车代码(亲测)有问题可随时问

基于MPU6050的自平衡小车,采用TB6612驱动模块,拥有超声波避障,蓝牙遥控功能。 能实现上述所有基本功能,结合相关手机软件(有需求可向我索要),能实现软件显示超声波数据,蓝牙(按键 重力 遥杆)遥控,还可以实时监控到加速度和角速度波形

立即下载
完整的STM32自平衡小车

该工程采用MDK4编写,MCU是STM32F103,软件实现了速度,方向,和平衡等功能。主要采用MPU6050和编码器。内有详细注释。

立即下载
平衡小车arduino+mpu6050+卡尔曼滤波+PID

自平衡小车arduino主控+mpu6050+卡尔曼滤波+PID

立即下载

热点文章

下载码下载
做任务获取下载码
取消 提交下载码
img

spring mvc+mybatis+mysql+maven+bootstrap 整合实现增删查改简单实例.zip

资源所需积分/C币 当前拥有积分 当前拥有C币
5 0 0
点击完成任务获取下载码
输入下载码
为了良好体验,不建议使用迅雷下载
img

自平衡小车代码

会员到期时间: 剩余下载个数: 剩余C币: 剩余积分:0
为了良好体验,不建议使用迅雷下载
VIP下载
您今日下载次数已达上限(为了良好下载体验及使用,每位用户24小时之内最多可下载20个资源)

积分不足!

资源所需积分/C币 当前拥有积分
您可以选择
开通VIP
4000万
程序员的必选
600万
绿色安全资源
现在开通
立省522元
或者
购买C币兑换积分 C币抽奖
img

资源所需积分/C币 当前拥有积分 当前拥有C币
5 4 45
为了良好体验,不建议使用迅雷下载
确认下载
img

资源所需积分/C币 当前拥有积分 当前拥有C币
3 0 0
为了良好体验,不建议使用迅雷下载
VIP和C币套餐优惠
img

资源所需积分/C币 当前拥有积分 当前拥有C币
5 4 45
您的积分不足,将扣除 10 C币
为了良好体验,不建议使用迅雷下载
确认下载
下载
您还未下载过该资源
无法举报自己的资源

兑换成功

你当前的下载分为234开始下载资源
你还不是VIP会员
开通VIP会员权限,免积分下载
立即开通

你下载资源过于频繁,请输入验证码

您因违反CSDN下载频道规则而被锁定帐户,如有疑问,请联络:webmaster@csdn.net!

举报

  • 举报人:
  • 被举报人:
  • *类型:
    • *投诉人姓名:
    • *投诉人联系方式:
    • *版权证明:
  • *详细原因: