跨届大侠
 
- 积分
- 6089
- 威望
- 2660
- 贡献
- 1737
- 兑换币
- 1625
- 注册时间
- 2011-7-22
- 在线时间
- 846 小时
|
wmslecz 发表于 2014-5-1 19:12 ![]()
学姐....再请教您一下哈...你修改过了官方的速度方案...官方的速度方案貌似确实不是很稳定....我看学姐您 ...
我的车子速度这块也确实没做好，后来因为时间问题，就没有接着去研究了，比赛的时候就是用直立控制去加减速的，因为我的车直立控制做的比较好吧，如果给定一个角度，他会立刻达到这个角度，所以加速的时候，将平衡点前倾n°，就实现了加速，遇到拐角，再改回平衡点，你可以参考一下我的视频：
http://v.youku.com/v_show/id_XNjQ1OTA2NzUy.html
下面这个视频里，就没有给加速，所以遇到了直线就会变慢：
http://v.youku.com/v_show/id_XNjQ1OTAxMTAw.html
我再解释一下我的代码吧：
P=SPEED_CONTROL_P; //速度P为何要这种赋值操作? 为何不直接使用这个值?
SPEED_CONTROL_P是个全局变量，定义在头文件中，这样的好处是，可以用BDM直接在线调试这个变量，非常方便，如果这个变量是在函数里，就不能在线调试了
tangent[k]也在头文件中
在头文件中，P=SPEED_CONTROL_I=0;P=SPEED_CONTROL_D=0;也就是说我只用了P控制
另外我觉得我的程序速度控制没做好的原因可能是速度积分没有限幅，虽然我给速度积分输出项限幅了，但是没有给fI限幅，所以只要加了I参数就很难调，你可以试一下
INCLUDES.h:
- #ifndef INCLUDES_H
- #define INCLUDES_H
- #define PI 3.14159265358979323846
- #define true 1
- #define false 0
- #define gyroPin_Z1 5
- #define gyroPin_X1 6
- #define accPin_Z 4
- #define accPin_Y 3
- #define accPin_X 2
- float gyroOffset=121;
- float gyroZ,accX,accY,accZ;
- float angleG=0;
- //端口定义
- #define FLED PORTK_PK0
- #define BMQR PORTA_PA4 //编码器复位
- #define TSL_SI PORTA_PA1 //定义线性传感器的端口 SI
- #define TSL_CLK PORTA_PA0 //定义线性传感器的端口 CLK
- #define TSL_SI1 PORTA_PA3 //定义线性传感器的端口 SI
- #define TSL_CLK1 PORTA_PA2 //定义线性传感器的端口 CLK
- //控制参数
- #define PRINT_AD (1) //设置串口打印数据类型，0:打印二值化数据，1：打印AD值
- #define THRESHOLD (100) //设置二值化阈值
- #define WINDOW_WIDTH (128) //设置串口打印的像素个数，最大128，最小0
- //直立控制
- float TLYLDZ1=1950;
- float TLYLDZ2=1950;
- float TLYLDX1=1950;
- float TLYLDX2=1950;
- //#define TLYLD 1950 //陀螺仪零点，前倾则增加
- float TLYBL=0.536; //陀螺仪比例
- float TLYBLZ2=0.536; //陀螺仪比例
- float TLYBLX1=0.536; //陀螺仪比例
- float TLYBLX2=0.536; //陀螺仪比例
- #define JSDLD 1365 //加速度计零点,往前跑则增加
- float KP=700.0;
- float KD=20.5;
- float Tg =2.0;
- float kjifen=150.0;
- //速度控制
- float SPEED_CONTROL_P=30;
- float SPEED_CONTROL_I=0;
- float SPEED_CONTROL_D=0.0;
- int k=1;
- float speedK=1;
- #define M_SQ 0 //电机死区
- int M_SQL=1500; //电机死区
- int M_SQR=1200; //电机死区
- #define CAR_SPEED_SET_MAX -500
- int CAR_SPEED_SET =0;
- #define SPEED_CONTROL_OUT_MAX 8000
- #define SPEED_CONTROL_OUT_MIN -8000
- #define gyroPin_Z 5
- #define accPin_Z 4
- #define accPin_Y 3
- #define accPin_X 2
- int accOffset=1365;
- float angleAZ=0,angleAX=0,angleAY=0,angleA=0,angleFilter=0;
- //定义全局变量
- unsigned char c,cc;
- int CCDTime=0,ccdMultiple=1,CCDBL=3;
- byte ADV[2][128]={0,0}; //声明数组，用于存放采集的线性数值
- byte CCDLine[128]={0};
- int CCDDebugSwitch=1,CCDDebugSwitch2=0;
- float FZBL= 0.5;//0.35
- float FZBL1=0.6;
- int CCDt;
- long CCDa;
- #define DCCD 5
- int FZ,CCDAvr,Rblack,LastRblack,Lblack,LastLblack,LineCenter=64;
- int CCDEdge=0,Threshold,ThresholdMax,ThresholdMin;
- int CCDAvr0=0,obstacleSign=0,obstacleCounter=0;
- char scratchLine=false,StopCarOn;
- int scratchLineCount=0;
- unsigned long scratchLineTimer;
- int edge[20]={0};
- int FZ1,CCDAvr1,Rblack1,LastRblack1,Lblack1,LastLblack1,LineCenter1=64;
- int LastC1=64,LastC2=64,LastC3=64;;
- int trackWidth=73;
- float sWeight=0.6;
- float dWeight=0.4;
- char CCDFirstTime=true;
- float gyro,Aangle;
- int Lspeed,Rspeed,LspeedJF,RspeedJF;
- float gyro,angle,jifen;
- int speed,AngleControlOut;
- float KDIR=70,DDIR=-5; //转向比例系数
- int dcPeriod=1;
- float DError,DLastError,DDError;;
- float g_fDirectionControlOutNew=0,g_fDirectionControlOutOld=0,g_fDirectionControlOut=0,ZX;
- int g_nDirectionControlPeriod;
- int g_nDirectionControlCount;
- char MotorEnable=true;
- int LOUT,ROUT;
- float g_fCarSpeed,g_fSpeedControlIntegral=0,g_fSpeedControlOutOld=0,g_fSpeedControlOutNew=0,g_fSpeedControlOut=0;
- int g_nSpeedControlCount = 0;
- int g_nSpeedControlPeriod = 0;
- int scPeriod=8;
- int ControlFlag=0;
- int Lspeed,Rspeed,LspeedJF,RspeedJF;
- float gyro,Aangle;
- #define CMD_TURN_LEFT '!'
- #define CMD_TURN_RIGHT '@'
- #define CMD_FORWARD '#'
- #define CMD_BACKWARD '
- #define CMD_RUN '%'
- #define CMD_STOP '^'
- #define CMD_SET_SPEED '&'
- #define CMD_SET_GYROOFFSET '&'
- char debug=1;
- unsigned long gyroTimer=0;
- float weightFlag=0;
- float GyroSense=0.01;
- float GyroSense2=0.67;
- float AccSense=800;//mV
- float gravity=0,gravityG=1,gravityError;
- float gravityGate=0.06,gravityVibrationGate=0.2;
- float weight=0.99;
- float weight1=0.995,weight2=0.98;
- float Setpoint=-56,OriginPoint=-56,Input=0,Output=0; //前倾角度增大，后仰减小
- int ValueK;
- float kp=60,kd=1;//kp=1250,kd=18; kp=60,kd=1;
- float ka=0.1;
- float Error,dErr,LastErr=0;
- float val_kp,val_kd;
- float MMperPulse=0.14165,PulseperMM=7.05965;
- float SetSpeed=0,SetSpeedMM=0,fspeed,sp=1,si=0;
- float SError=0,SErrI=0;
- float SOutput=0;
- float angleG2=0,angleFilter2=0;
- float angleGY=0,angleFilterY=0;
- float angleA2=0;
- unsigned char CCDBuf[128]=0;
- int CCDBuf2[128]=0,CCDBuf3[128]=0;
- float tangent[46]={
- 1.000000,1.035530,1.072369,1.110612,1.150368,1.191754,1.234897,1.279942,1.327045,
- 1.376382,1.428148,1.482561,1.539865,1.600334,1.664279,1.732051,1.804048,1.880726,
- 1.962610,2.050304,2.144507,2.246037,2.355852,2.475087,2.605089,2.747477,2.904211,
- 3.077683,3.270852,3.487414,3.732050,4.010781,4.331475,4.704630,5.144553,5.671281,
- 6.313751,7.115368,8.144345,9.514362,11.430049,14.300661,19.081127,28.636232,57.289875,57.289875,
- };
- #endif
|
|
[复制链接]
发表于 2014-4-26 19:34:57
楼主