自平衡算法

自平衡的控制用到了PID控制算法

void GetGyroOffset(float &offset);
void balance(float offset);task main()
{float offset;Wait(50);GetGyroOffset(offset);balance(offset);
}void GetGyroOffset(float &offset)
{SetSensorHTGyro(IN_2);float gsum;int i,gmin,gmax,g;ClearScreen();TextOut(10,40,"keep the robot");TextOut(20,20,"static");Off(OUT_AC);do{gsum=0.0;gmin=1000;gmax=-1000;for(i=0;i<100;i++){g=SensorHTGyro(IN_2);if(g>gmax)gmax=g;if(g<gmin)gmin=g;gsum=gsum+g;Wait(5);}ClearScreen();NumOut(20,0,(gmax-gmin));}while((gmax-gmin)>2);offset=gsum/100+1.0;ClearScreen();TextOut(20,40,"Successful");PlayTone(440,500);Wait(500);
}void balance(float offset)
{SetSensorColorFull(IN_1);SetSensorHTGyro(IN_2);ColorSensorReadType csr;csr.Port = IN_1;int color;int go=0;float gyrospeed,gyroangle;float motorpos,motorspeed;int power,powerleft,powerright,steer;long tmotorposok;long loop=0;ClearScreen();TextOut(0,60,"Hello!");TextOut(0,40,"balancing...");Wait(200);float time;long tstart;float gyroraw;float angle=0.0;long left,right,delta;long motorsum,sum,motordiff;long delta1,delta2,delta3;offset+=0.15;tmotorposok=CurrentTick();ResetRotationCount(OUT_A);ResetRotationCount(OUT_C);do{if(loop==0){time=0.0055;tstart=CurrentTick();}else{time=(CurrentTick()-tstart)/(loop*1000.0);}gyroraw=SensorHTGyro(IN_2);offset=0.0005*gyroraw+0.9995*offset;gyrospeed=gyroraw-offset;angle+=gyrospeed*time;gyroangle=angle;left=MotorRotationCount(OUT_A);right=MotorRotationCount(OUT_C);sum=motorsum;motorsum=left+right;motordiff=left-right;delta=motorsum-sum;motorpos+=delta;motorpos-=go*time;motorspeed=(delta+delta1+delta2+delta3)/(4*time);delta3=delta2;delta2=delta1;delta1=delta;SysColorSensorRead(csr);color=csr.ColorValue;ClearScreen();NumOut(20,60,gyroangle);NumOut(20,40,gyrospeed);NumOut(20,20,motorpos);NumOut(20,0,motorspeed);power=(5*gyroangle+1*gyrospeed)/0.45+motorpos*0.05+motorspeed*0.1-0.02*go;go=loop-2000;if(go<0)go=0;steer=0;powerleft=power+steer;powerright=power+steer;OnFwd(OUT_A,powerleft);OnFwd(OUT_C,powerright);loop+=1;}while(true);
}

利用颜色传感器走黑线

void GetGyroOffset(float &offset);          //declare the subfunction to get offset
void balance(float offset);                 //declare the subfunction to take balancetask main()
{float offset;                             //define a variable represent original gyro speedWait(50);                                 //wait for 0.05 secondGetGyroOffset(offset);                    //run the function to get the offsetbalance(offset);                          //run the function to keep balance base on the offset}void GetGyroOffset(float &offset)           //define the subfunction
{SetSensorHTGyro(IN_1);                   //declare the gyro sensorfloat gsum;                              //define the sum of gyro value with float typeint i,gmin,gmax,g;                       //define variables relevent the gyro valueClearScreen();                           //clear the screenTextOut(10,40,"keep the robot"); TextOut(20,20,"static");                 //display on the screenOff(OUT_AC);do{gsum=0.0;gmin=1000;gmax=-1000;         //define the original valuefor(i=0;i<100;i++){g=SensorHTGyro(IN_1);                //get the value from gyro sensor, assign it to gif(g>gmax)gmax=g;if(g<gmin)gmin=g;                            //update the maximum and minimum value of gyrogsum=gsum+g;                         //update the sum of gyroWait(5);}}while((gmax-gmin)>2);                 //when the segway is stable enough, break the loopoffset=gsum/100+1.0;                   //calculate the offset valueClearScreen();                         //clear the screenTextOut(20,40,"Successful");           //inform user that the segway is ready to self balancePlayTone(440,500);Wait(500);
}void balance(float offset)
{SetSensorHTGyro(IN_1);                    //declare the gyro sensorSetSensorColorFull(IN_2);                 //declare the colour sensorColorSensorReadType csr;                  //define a struct discribe colourcsr.Port = IN_2;                          //get the value from colour sensorint colour;                               //define the variable discribe colourint ok1=0,ok2=0,ok3=0;int go=0;float gyrospeed,gyroangle;float motorpos,motorspeed;int power,powerleft,powerright,steer;long tmotorposok;long loop=0;                              //define the variables in calculationClearScreen();TextOut(0,60,"Hello!");TextOut(0,40,"balancing...");             //inform user the segway is balancingWait(200);float time;long tstart,t1,t2;float gyroraw;float angle=0.0;long left,right,delta;long motorsum,sum,motordiff;long delta1,delta2,delta3;                //define the variables in calculationoffset+=0.15;                             //adjust the offset because the sensor is not accurate enoughtmotorposok=CurrentTick();ResetRotationCount(OUT_A);ResetRotationCount(OUT_C);                //reset the parameter of motorsdo{                                       //the balance loopif(loop==0){time=0.0055;tstart=CurrentTick();}else{time=(CurrentTick()-tstart)/(loop*1000.0);}                                       //get the time of a single loopgyroraw=SensorHTGyro(IN_1);offset=0.0005*gyroraw+0.9995*offset;    //self-correctinggyrospeed=gyroraw-offset;angle+=gyrospeed*time;gyroangle=angle;                        //calculate the gyro angle and speedleft=MotorRotationCount(OUT_A);right=MotorRotationCount(OUT_C);        //get raw motor sensor valuesum=motorsum;motorsum=left+right;motordiff=left-right;delta=motorsum-sum;motorpos+=delta;motorpos-=go*time;motorspeed=(delta+delta1+delta2+delta3)/(4*time);delta3=delta2;delta2=delta1;delta1=delta;                           //calculate the motor angle and speedSysColorSensorRead(csr);color=csr.ColorValue;                   //get raw colour sensor valuepower=(7.5*gyroangle+1.15*gyrospeed)/0.45+motorpos*0.07+motorspeed*0.1-0.02*go;//power operation, balance equationif(loop<3000){go=0;OnFwd(OUT_AC,power);ClearScreen();TextOut(20,20,"preparing");           //first, segway stands until stabe}else{if(ok1==0){go=loop-3000;if(go>200)go=200;OnFwd(OUT_AC,power);ClearScreen();TextOut(20,40,"searching");TextOut(20,20,"the line");           //second, segway searchs the black lineif(color==1){PlayTone(440,500);ok1=1;}}else{if(ok2==0){go-=1;if(go<100)go=100;OnFwd(OUT_AC,power);ClearScreen();TextOut(20,40,"found it!");        //third, segway reach the black line and slow downif(color==6){PlayTone(440,500);ok2=1;}}else{if(ok3==0){go-=1;if(go<0)go=0;steer=-20;powerleft=power+steer;powerright=power-steer;OnFwd(OUT_C,powerleft);OnFwd(OUT_A,powerright);          //fourth, seyway rotates to follow the line's directionif((color==1)&&(go==0)){PlayTone(440,500);ok3=1;}}else{if(go>300)go=300;powerleft=power;powerright=power;if(color<4){t1=CurrentTick();powerleft+=30;}else{t2=CurrentTick();powerright+=30;}if((t2-t1)<100)go+=1;OnFwd(OUT_C,powerleft);            //fifth, seyway follow the line and speed upOnFwd(OUT_A,powerright);ClearScreen();TextOut(20,40,"line-hunting...");}}}}if(loop==3000)PlayTone(440,500);loop+=1;}while((power<100)&&(power>-100));          //if the segway fall down, break the loopwhile(true){Off(OUT_AC);ClearScreen();TextOut(20,40,"fell..");                     //turn off the motors}
}

利用乐高NXT自带的遥控器遥控小车

void GetGyroOffset(float &offset);          //declare the subfunction to get offset
void balance(float offset);                 //declare the subfunction to take balancetask main()
{float offset;                             //define a variable represent original gyro speedWait(50);                                 //wait for 0.05 secondGetGyroOffset(offset);                    //run the function to get the offsetbalance(offset);                          //run the function to keep balance base on the offset
}void GetGyroOffset(float &offset)           //define the subfunction
{SetSensorHTGyro(IN_1);                   //declare the gyro sensorfloat gsum;                              //define the sum of gyro value with float   typeint i,gmin,gmax,g;                       //define variables relevent the gyro valueClearScreen();                           //clear the screenTextOut(10,40,"keep the robot");TextOut(20,20,"static");                 //display on the screenOff(OUT_AC);do{gsum=0.0;gmin=1000;gmax=-1000;         //define the original valuefor(i=0;i<100;i++){g=SensorHTGyro(IN_1);                //get the value from gyro sensor, assign it to gif(g>gmax)gmax=g;if(g<gmin)gmin=g;                            //update the maximum and minimum value of gyrogsum=gsum+g;                         //update the sum of gyroWait(5);}}while((gmax-gmin)>2);                 //when the segway is stable enough, break the loopoffset=gsum/100+1.0;                   //calculate the offset valueClearScreen();                         //clear the screenTextOut(20,40,"Successful");           //inform user that the segway is ready to self balancePlayTone(440,500);Wait(500);
}void balance(float offset)
{SetSensorHTGyro(IN_1);                    //declare the gyro sensorSetSensorLowspeed(IN_2);                  //declare the IR receiverchar control[8];                          //define the variable of IR receiverint go=0;float gyrospeed,gyroangle;float motorpos,motorspeed;int power,powerleft,powerright,steer;long tmotorposok;long loop=0;                              //define the variables in calculationfloat time;long tstart;float gyroraw;float angle=0.0;long left,right,delta;long motorsum,sum,motordiff;long delta1,delta2,delta3;                //define the variables in calculationoffset+=0.15;                             //adjust the offset because the sensor is not accurate enoughtmotorposok=CurrentTick();ResetRotationCount(OUT_A);ResetRotationCount(OUT_C);                //reset the parameter of motorsdo{                                       //the balance loopif(loop==0){time=0.0055;tstart=CurrentTick();}else{time=(CurrentTick()-tstart)/(loop*1000.0);}                                       //get the time of a single loopReadSensorHTIRReceiver(IN_2,control);if(control[HT_CH1_A]==100){go+=1;if(go<300)go=300;if(go>600)go=600;}                                        //if set left switch, "go" changes from 300 to 600if(control[HT_CH1_A]==-100){go-=1;if(go>-300)go=-300;if(go<-600)go=-600;}                                        //if reset left switch, "go" changes from -300 to -600if(control[HT_CH1_A]==0){if(go>0)go-=1;if(go<0)go+=1;}                                        //if not output from left switch, "go" changes to 0 slowlyif(control[HT_CH1_B]==100){steer+=1;if(steer<20)steer=20;if(steer>40)steer=40;}                                        //if set right switch, "steer" changes from 300 to 600if(control[HT_CH1_B]==-100){steer-=1;if(steer>-20)steer=-20;if(steer<-40)steer=-40;}                                        //if reset right switch, "steer" changes from -300 to -600if(control[HT_CH1_B]==0){if(steer>0)steer-=1;if(steer<0)steer+=1;}                                        //if not output from right switch, "steer" changes to 0 slowlygyroraw=SensorHTGyro(IN_1);offset=0.0005*gyroraw+0.9995*offset;    //self-correctinggyrospeed=gyroraw-offset;angle+=gyrospeed*time;gyroangle=angle;                        //calculate the gyro angle and speedleft=MotorRotationCount(OUT_A);right=MotorRotationCount(OUT_C);        //get raw motor sensor valuesum=motorsum;motorsum=left+right;motordiff=left-right;delta=motorsum-sum;motorpos+=delta;motorpos-=go*time;motorspeed=(delta+delta1+delta2+delta3)/(4*time);delta3=delta2;delta2=delta1;delta1=delta;                           //calculate the motor angle and speedpower=(7.5*gyroangle+1.15*gyrospeed)/0.45+motorpos*0.07+motorspeed*0.1-0.02*go;//power operation, balance equationpowerleft=power+steer;powerright=power-steer;                //get the left and right wheels powerOnFwd(OUT_A,powerleft);OnFwd(OUT_C,powerright);               //apply the power into wheelsClearScreen();NumOut(20,20,go);NumOut(20,40,steer);NumOut(20,60,power);                   //print relevant parameters on the screenloop+=1;}while((power<100)&&(power>-100));         //if the segway fall down, break the loopwhile(true){Off(OUT_AC);ClearScreen();TextOut(20,40,"fell..");               //turn off the motors}
}

乐高NXT实现自平衡小车Segway相关推荐

matlab两轮自平衡小车,两轮自平衡小车（全部设计资料+设计分析）
自己做的自平衡小车,基本达到预期效果.制作资料在压缩包里面,供参考. 该两轮自平衡小车硬件设计概述:控制器:ATmega16:8MHz: 加速度传感器:MMA2260:陀螺仪:EWTS82: 传感器的 ...
直立车各环的调试_平衡小车调试指南(直立环速度环)
平衡小车之家出品 1 平衡小车调试指南接下来将和大家一起以工程的思想去完成一个平衡小车的调试, 包括平衡小车的直立环.速度环.转向环,一般我们是先调试直立环,再调试速度环,最好调试转向环.另外需 ...
基于模糊控制的自平衡小车的研究
绪论课题研究背景及意义: 随着人类文明的发展,传感器技术.计算机应用技术.机械学.微电子技术.通讯技术以及人工智能技术也得到了飞速的发展.进入21世纪后,在机器人学和机器人技术领域,自平衡小车已成为 ...
手把手教学，平衡小车详细教程，平衡车
0.前言原谅我起这么猛的标题,但看完,你不会来打我的. 如果此项目有朋友已经做过,可以越过不看. 回归正题,平衡小车,这个开源项目已经出来很多年了,虽然是一个比较基础的项目,但也牵扯到方方面面的知 ...
平衡小车卡尔曼滤波算法
最近研究STM32的自平衡小车,发现有两座必过的大山,一为卡尔曼滤波,二为PID算法. 网上看了很多关于卡尔曼滤波的代码,感觉写得真不咋地.一怒之下,自己重写,不废话,贴代码 [pre lang=&q ...
MATLAB/Simulink模型开发乐高EV3 双足平衡机器人
MATLAB/Simulink 2018a 模型开发乐高EV3 双足平衡机器人(含视频) 之前只是用Matlab做一些算法的仿真,数据的计算分析处理,知道MATLAB中包含有Simulink,只知道S ...
Arduino平衡小车
Arduino平衡小车 1.概述此Arduino平衡小车在主控方面由Arduino UNO R3和Arduino sensor shield v5.0传感器扩展板组成.采用TB6612FNG作为电源 ...
平衡小车PID，就该这么调！！！
上一篇文章:看完这篇文章,还不会做平衡小车,你来打我. 描述了平衡小车的制作过程,也开源了一部分设计资料. 在上篇文章留言中,有朋友说: 安排,必须安排! 1.PID 关于PID的概念,网上相关的帖子 ...
基于STM32-F401的平衡小车
目录一.控制系统设计 1.1机械结构设计 1.2传感系统设计 1.3执行器设计 1.4控制算法设计二.控制系统的制作与调试 2.1机械结构的制作与调试 2.2电路系统的制作与调试 2.3控制程序的 ...

乐高NXT实现自平衡小车Segway

文章目录

自平衡算法

利用颜色传感器走黑线

利用乐高NXT自带的遥控器遥控小车

乐高NXT实现自平衡小车Segway相关推荐

最新文章

热门文章