开发工具:
文件大小: 7kb
下载次数: 0
上传时间: 2014-06-04
详细说明： 【转】两轮自平衡小车开源!公布arduino可用的代码! 两轮自平衡小车开源!公布arduino可用的代码! autopopo 最后编辑于 2010-10-21 20:07:58 http://bbs.roboticfan.com/showtopic-8281.aspx //IDG330 Mannual, 2mv= 1度/s 的角速率,ad读数1024,3.3v,那么每读数对应 3.223mV,所以每读数对应3.223/2/180*PI= 0.028123弧度/秒 static const double SEMICIRCLE_ARC = 57.29578; /*半圆对应的弧度值*/ static const double GYRO_OPERATOR = 0.028123; /*AD读取的陀螺仪数值对应的弧度算子*/ /*kalman*/ static const double C_0 = 1; static const double Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.005;//注意：dt的取值为kalman滤波器采样时间 double P[2][2] = {{ 1, 0 }, { 0, 1 }}; double Pdot[4] ={ 0,0,0,0}; double q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1; double angle, angle_dot; /*sensor*/ double sensorPort[6] = { 0, 1, 2, 3, 4, 5};/*传感器地址,电路决定.accZ,gyroX, AD1, AD2, AD3, AD4*/ double sensorValue[6];/*传感器的返回值*/ /*传感器零点 0:Z轴(平行轴) 1:陀螺仪中点*/ double sensorZero[2] = {499,505}; double sensorAdjusted[2];/*传感器的返回值重整*/ //double provAngle; /*moto*/ int E1 = 6; int E2 = 9; int M1 = 7; int M2 = 8; double deadAreaCompensation1 = 45,deadAreaCompensation2 = 35; /*balance*/ double RATE[4] = { 0,0,0,0};/*公式中的4个变量*/ double K[4] = { 60.45, 1.27, 125, 0.75};/*公式中的4个常量*/ double K_AD[4] = { 1, 1, 1, 1};/*公式中的4个常量*/ double wheel_ls[7];/*左轮:0:编码器 1:位移(position) 2:position_dot 3:position_dot_filter 4:speedNeed 5:turnNeed 6:speedOutPut */ double wheel_rs[7];/*右轮:0:编码器 1:位移(position) 2:position_dot 3:position_dot_filter 4:speedNeed 5:turnNeed 6:speedOutPut */ double Torque;/*扭矩*/ unsigned int count,count2; boolean OK=false;//这个是误差达到一定程度后的系统关闭开关. int bf,X,Y;//从无线端发来的命令 /*kalman*/ /*angle_m:经过atan2(ax,ay)方法计算的偏角,弧度值 gyro_m:经过初步减去零点的陀螺仪角速度值,弧度值 */ void Kalman_Filter(double angle_m,double gyro_m) { angle+=(gyro_m-q_bias) * dt; angle_err = angle_m - angle; 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] * dt; P[0][1] += Pdot[1] * dt; P[1][0] += Pdot[2] * dt; P[1][1] += Pdot[3] * dt; 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 do_balance() { sensorValue[0] = analogRead(sensorPort[0]);//Z轴加速度 sensorValue[1] = analogRead(sensorPort[1]);//X轴旋转(陀螺) //sensorValue[2] = analogRead(sensorPort[2]);//旋钮1 //sensorValue[3] = analogRead(sensorPort[3]);//旋钮2 //sensorValue[4] = analogRead(sensorPort[4]);//旋钮3 //sensorValue[5] = analogRead(sensorPort[5]);//旋钮4 //算出整理过的零点值 sensorAdjusted[0] = sensorValue[0] - sensorZero[0]; if(abs(sensorAdjusted[0]) > 70)//如果倾斜到一定程度,就停机 { OK = false; sensorAdjusted[0] =sensorAdjusted[0]>0 70:-70; } sensorAdjusted[1] = sensorZero[1] - sensorValue[1];/*减去初始零点*/ /*根据可变电阻改变K的倍数*/ //deadAreaCompensation = (sensorValue[4]+1)/10.24; //K_AD[0] = (sensorValue[2]+1)/256;//102.4;/*值在0.001~10之间变化.在AD=100的时候接近1 */ //K_AD[1] = (sensorValue[3]+1)/256;//102.4; //K_AD[2] = (sensorValue[4]+1)/256;//102.4; //K_AD[3] = (sensorValue[5]+1)/256;//102.4; /*根据上一周期Torqu的值统计轮子转动积分*/ wheel_ls[2] = Torque > 0 wheel_ls[0]:-wheel_ls[0]; wheel_rs[2] = Torque > 0 wheel_rs[0]:-wheel_rs[0]; wheel_ls[0] = wheel_rs[0] = 0; /*************** balance *********************************************/ /*小车初始状态*/ { if(!OK) { if(abs(sensorAdjusted[0]) <= 3)//小车被推倒平衡位置后才启动电机 { count=0; OK = true; } for(int i = 0; i <7;i++) { wheel_ls[i] = 0; wheel_rs[i] = 0; } } } /*balance*/ { /*GYRO_OPERATOR = 0.028123 AD读取的陀螺仪数值对应的弧度算子*/ Kalman_Filter(atan2(sensorAdjusted[0], sqrt(10000-sensorAdjusted[0]*sensorAdjusted[0])), sensorAdjusted[1] * GYRO_OPERATOR);//10000:因为只有一轴加速度计,所以虚拟一个斜边 RATE[0] = angle * SEMICIRCLE_ARC;//SEMICIRCLE_ARC=57.29578; /*半圆对应的弧度值*/ RATE[1] = angle_dot * SEMICIRCLE_ARC; } /*计算速度 double wheel_ls[8]; 0:编码器累加 1:位移(position) 2:position_dot 3:速度(position_dot_filter) 4:speedNeed 5:turnNeed 6:speedOutPut */ { wheel_ls[3] *= 0.95; /*车轮速度滤波,wheel_ls[3] : position_dot_filter*/ wheel_ls[3] += wheel_ls[2]*0.05; /*wheel_ls[2] : position_dot*/ wheel_ls[1] += wheel_ls[3]; /*wheel_ls[1] : position*/ wheel_ls[1] += wheel_ls[4]; /*wheel_ls[4] : speedNeed*/ wheel_ls[1] = max(-50, wheel_ls[1]); wheel_ls[1] = min(50, wheel_ls[1]); RATE[2] = wheel_ls[3];//速度--滤波过了 RATE[3] = wheel_ls[1];//位置 } /*Torque 综合所有参数算出扭矩*/ { Torque = RATE[0] * K[0] * K_AD[0] + RATE[1] * K[1] * K_AD[1] + RATE[2] * K[2] * K_AD[2] + RATE[3] * K[3]* K_AD[3]; //根据扭矩算轮子的命令值 wheel_ls[6] = abs(Torque)+ deadAreaCompensation1;//wheel_ls[6]:扭矩输出 deadAreaCompensation1:左轮的死区补偿 wheel_ls[6] = min(255, wheel_ls[6]);//限制最大扭矩255 wheel_ls[6] = OK wheel_ls[6]:0; wheel_rs[6] = abs(Torque)+ deadAreaCompensation2; wheel_rs[6] = min(255, wheel_rs[6]); wheel_rs[6] = OK wheel_rs[6]:0; } if(Torque > 0) { digitalWrite(M2, HIGH); digitalWrite(M1, LOW); } else { digitalWrite(M2, LOW); digitalWrite(M1, HIGH); } analogWrite(E1, wheel_ls[6]); //PWM调速a==0-255 analogWrite(E2, wheel_rs[6]); } void do_msg(){ if(count0==0) { Serial.print("$CLEAR\r\n"); Serial.print("$GO 1 1\r\n"); Serial.print("$PRINT "); Serial.print(wheel_ls[1]); Serial.print(" "); Serial.print(wheel_ls[3]); //Serial.print(" "); //Serial.print(RATE[2]); //Serial.print(K[0]*K_AD[0]); //Serial.print(" "); //Serial.print(K[1]*K_AD[1]); //Serial.print(" "); Serial.print("\r\n"); Serial.print("$GO 2 1\r\n"); Serial.print("$PRINT "); //Serial.print(K[0]*K_AD[0]); //Serial.print(" "); //Serial.print(K[1]*K_AD[1]); //Serial.print(" "); //Serial.print(K[2]*K_AD[2]); //Serial.print(" "); //Serial.print(K[3]*K_AD[3]); //Serial.print(count); Serial.print(wheel_ls[5]); Serial.print(" "); Serial.print(wheel_rs[5]); //Serial.print(count); //Serial.print(Torque); Serial.print("\r\n"); } } /***************** setup-loop *************************************************/ void setup() { count=0; X=Y=8; bf=-1; //init motos for (int i = 6; i <= 9; i++) { pinMode(i, OUTPUT); } Serial.begin(9600);//115200); analogReference(EXTERNAL); //设置模拟输入为外部参考3.3V attachInterrupt(0, blinkone, CHANGE); //设置为0号中断，中断函数blink，触发方式为RISING attachInterrupt(1, blinktwo, CHANGE); //设置为0号中断，中断函数blink，触发方式为RISING } void blinkone()//中断函数 { wheel_ls[0] ++; } void blinktwo()//中断函数 { wheel_rs[0] ++; } void loop() { do_balance();//计算平衡 do_msg(); delay(3); count++; if(count>=60000) count=0; } ...展开收缩
(系统自动生成,下载前可以参看下载内容)
下载文件列表
相关说明
- 本站资源为会员上传分享交流与学习,如有侵犯您的权益,请联系我们删除.
- 本站是交换下载平台，提供交流渠道，下载内容来自于网络，除下载问题外，其它问题请自行百度。
- 本站已设置防盗链，请勿用迅雷、QQ旋风等多线程下载软件下载资源，下载后用WinRAR最新版进行解压.
- 如果您发现内容无法下载，请稍后再次尝试；或者到消费记录里找到下载记录反馈给我们.
- 下载后发现下载的内容跟说明不相乎，请到消费记录里找到下载记录反馈给我们，经确认后退回积分.
- 如下载前有疑问，可以通过点击"提供者"的名字，查看对方的联系方式，联系对方咨询.
