📄 pid.lst
字号:
684 5 }
685 4 }
686 3
687 3 c_cm1=0;
688 3 if(K21==OFF)
689 3 {
690 4 PWM0=temp_rot_vp;
691 4 if(temp_rot_vp==rot_vp)
692 4 {
693 5 if(rot_ok==0)
694 5 {
695 6 rot_ok=1;
696 6 rot_pid_ok=1;
697 6 }
698 5 else
699 5 r_slope_ok=1;
700 5 }
701 4 else
702 4 IEN1=IEN1|0x20; //CM1
703 4 }
704 3 else
705 3 {
706 4 if(temp_rot_vp==rot_vp&&temp_rot_vb==rot_vb)
707 4 {
708 5 PWM0=temp_rot_vb;
709 5 if(rot_ok==0)
710 5 {
711 6 rot_ok=1;
712 6 rot_pid_ok=1;
713 6 r_sample_num=0;
714 6 }
715 5 else
716 5 r_slope_ok=1;
717 5 }
718 4 else
719 4 IEN1=IEN1|0x20; //CM1
720 4 }
721 3 }
722 2 CML1=TML2+0XC8;
723 2 if(CML1<TML2) //CM1,400us,旋转速度变化时间
724 2 CMH1=TMH2+1+0X00;
725 2 else
726 2 CMH1=TMH2+0X00;
727 2 }
728 1 if(rot_pid_ok==1)
729 1 {
730 2 r_sample_num++;
731 2 new_rot_num=rot_num;
732 2 rot_num=0;
733 2 switch(r_sample_num)
734 2 {
735 3 case 1: {
736 4 temp_rot_num[r_sample_num-1]=new_rot_num;
737 4 average_r_num=new_rot_num+average_r_num;
C51 COMPILER V7.06 PID 09/08/2005 15:02:37 PAGE 13
738 4 new_rot_num=0;
739 4 }break;
740 3 case 2: {
741 4 if(new_rot_num<=temp_rot_num[r_sample_num-2])
742 4 {
743 5 temp_rot_num[r_sample_num-1]=temp_rot_num[r_sample_num-2];
744 5 temp_rot_num[r_sample_num-2]=new_rot_num;
745 5 }
746 4 else
747 4 temp_rot_num[r_sample_num-1]=new_rot_num;
748 4 average_r_num=new_rot_num+average_r_num;
749 4 new_rot_num=0;
750 4 }break;
751 3 case 3: {
752 4 if(new_rot_num<=temp_rot_num[r_sample_num-3])
753 4 {
754 5 temp_rot_num[r_sample_num-1]=temp_rot_num[r_sample_num-2];
755 5 temp_rot_num[r_sample_num-2]=temp_rot_num[r_sample_num-3];
756 5 temp_rot_num[r_sample_num-3]=new_rot_num;
757 5 }
758 4 else if(new_rot_num<=temp_rot_num[r_sample_num-2])
759 4 {
760 5 temp_rot_num[r_sample_num-1]=temp_rot_num[r_sample_num-2];
761 5 temp_rot_num[r_sample_num-2]=new_rot_num;
762 5 }
763 4 else
764 4 temp_rot_num[r_sample_num-1]=new_rot_num;
765 4 average_r_num=new_rot_num+average_r_num;
766 4 new_rot_num=0;
767 4 }break;
768 3 case 4: {
769 4 if(new_rot_num<=temp_rot_num[r_sample_num-4])
770 4 {
771 5 temp_rot_num[r_sample_num-1]=temp_rot_num[r_sample_num-2];
772 5 temp_rot_num[r_sample_num-2]=temp_rot_num[r_sample_num-3];
773 5 temp_rot_num[r_sample_num-3]=temp_rot_num[r_sample_num-4];
774 5 temp_rot_num[r_sample_num-4]=new_rot_num;
775 5 }
776 4 else if(new_rot_num<=temp_rot_num[r_sample_num-3])
777 4 {
778 5 temp_rot_num[r_sample_num-1]=temp_rot_num[r_sample_num-2];
779 5 temp_rot_num[r_sample_num-2]=temp_rot_num[r_sample_num-3];
780 5 temp_rot_num[r_sample_num-3]=new_rot_num;
781 5 }
782 4 else if(new_rot_num<=temp_rot_num[r_sample_num-2])
783 4 {
784 5 temp_rot_num[r_sample_num-1]=temp_rot_num[r_sample_num-2];
785 5 temp_rot_num[r_sample_num-2]=new_rot_num;
786 5 }
787 4 else
788 4 temp_rot_num[r_sample_num-1]=new_rot_num;
789 4 average_r_num=new_rot_num+average_r_num;
790 4 new_rot_num=0;
791 4 }break;
792 3 case 5: {
793 4 if(new_rot_num<=temp_rot_num[r_sample_num-5])
794 4 {
795 5 temp_rot_num[r_sample_num-1]=temp_rot_num[r_sample_num-2];
796 5 temp_rot_num[r_sample_num-2]=temp_rot_num[r_sample_num-3];
797 5 temp_rot_num[r_sample_num-3]=temp_rot_num[r_sample_num-4];
798 5 temp_rot_num[r_sample_num-4]=temp_rot_num[r_sample_num-5];
799 5 temp_rot_num[r_sample_num-5]=new_rot_num;
C51 COMPILER V7.06 PID 09/08/2005 15:02:37 PAGE 14
800 5 }
801 4 else if(new_rot_num<=temp_rot_num[r_sample_num-4])
802 4 {
803 5 temp_rot_num[r_sample_num-1]=temp_rot_num[r_sample_num-2];
804 5 temp_rot_num[r_sample_num-2]=temp_rot_num[r_sample_num-3];
805 5 temp_rot_num[r_sample_num-3]=temp_rot_num[r_sample_num-4];
806 5 temp_rot_num[r_sample_num-4]=new_rot_num;
807 5 }
808 4 else if(new_rot_num<=temp_rot_num[r_sample_num-3])
809 4 {
810 5 temp_rot_num[r_sample_num-1]=temp_rot_num[r_sample_num-2];
811 5 temp_rot_num[r_sample_num-2]=temp_rot_num[r_sample_num-3];
812 5 temp_rot_num[r_sample_num-3]=new_rot_num;
813 5 }
814 4 else if(new_rot_num<=temp_rot_num[r_sample_num-2])
815 4 {
816 5 temp_rot_num[r_sample_num-1]=temp_rot_num[r_sample_num-2];
817 5 temp_rot_num[r_sample_num-2]=new_rot_num;
818 5 }
819 4 else
820 4 temp_rot_num[r_sample_num-1]=new_rot_num;
821 4 average_r_num=new_rot_num+average_r_num;
822 4 new_rot_num=0;
823 4 }break;
824 3 case 6: {
825 4 if(new_rot_num<=temp_rot_num[r_sample_num-6])
826 4 {
827 5 temp_rot_num[r_sample_num-1]=temp_rot_num[r_sample_num-2];
828 5 temp_rot_num[r_sample_num-2]=temp_rot_num[r_sample_num-3];
829 5 temp_rot_num[r_sample_num-3]=temp_rot_num[r_sample_num-4];
830 5 temp_rot_num[r_sample_num-4]=temp_rot_num[r_sample_num-5];
831 5 temp_rot_num[r_sample_num-5]=temp_rot_num[r_sample_num-6];
832 5 temp_rot_num[r_sample_num-6]=new_rot_num;
833 5 }
834 4 else if(new_rot_num<=temp_rot_num[r_sample_num-5])
835 4 {
836 5 temp_rot_num[r_sample_num-1]=temp_rot_num[r_sample_num-2];
837 5 temp_rot_num[r_sample_num-2]=temp_rot_num[r_sample_num-3];
838 5 temp_rot_num[r_sample_num-3]=temp_rot_num[r_sample_num-4];
839 5 temp_rot_num[r_sample_num-4]=temp_rot_num[r_sample_num-5];
840 5 temp_rot_num[r_sample_num-5]=new_rot_num;
841 5 }
842 4 else if(new_rot_num<=temp_rot_num[r_sample_num-4])
843 4 {
844 5 temp_rot_num[r_sample_num-1]=temp_rot_num[r_sample_num-2];
845 5 temp_rot_num[r_sample_num-2]=temp_rot_num[r_sample_num-3];
846 5 temp_rot_num[r_sample_num-3]=temp_rot_num[r_sample_num-4];
847 5 temp_rot_num[r_sample_num-4]=new_rot_num;
848 5 }
849 4 else if(new_rot_num<=temp_rot_num[r_sample_num-3])
850 4 {
851 5 temp_rot_num[r_sample_num-1]=temp_rot_num[r_sample_num-2];
852 5 temp_rot_num[r_sample_num-2]=temp_rot_num[r_sample_num-3];
853 5 temp_rot_num[r_sample_num-3]=new_rot_num;
854 5 }
855 4 else if(new_rot_num<=temp_rot_num[r_sample_num-2])
856 4 {
857 5 temp_rot_num[r_sample_num-1]=temp_rot_num[r_sample_num-2];
858 5 temp_rot_num[r_sample_num-2]=new_rot_num;
859 5 }
860 4 else
861 4 temp_rot_num[r_sample_num-1]=new_rot_num;
C51 COMPILER V7.06 PID 09/08/2005 15:02:37 PAGE 15
862 4 average_r_num=new_rot_num+average_r_num;
863 4 new_rot_num=0;
864 4 }break;
865 3 case 7: {
866 4 if(new_rot_num<=temp_rot_num[r_sample_num-7])
867 4 average_r_num=average_r_num-temp_rot_num[5];
868 4 else if(new_rot_num>=temp_rot_num[r_sample_num-2])
869 4 average_r_num=average_r_num-temp_rot_num[0];
870 4 else
871 4 average_r_num=average_r_num-temp_rot_num[0]-temp_rot_num[5]+new_rot_num;
872 4 average_r_num=average_r_num*10/5;
873 4
874 4 rot_last_last_difference=rot_last_difference;
875 4 rot_last_difference=rot_difference;
876 4 rot_difference=academic_num-average_r_num;
877 4 rot_P=r_Kp*rot_difference;
878 4 rot_I=r_Ki*(rot_difference+rot_last_difference+rot_last_last_difference);
879 4 rot_D=r_Kd*(rot_difference-rot_last_difference);
880 4 rot_actual_difference=rot_P+rot_I+rot_D;
881 4 if(p_b==1)
882 4 temp_rot_vp=255*(new_rot_num+rot_actual_difference)/22;
883 4 else
884 4 temp_rot_vb=255*(new_rot_num+rot_actual_difference)/22;
885 4 new_rot_num=0;
886 4 }break;
887 3 }
888 2 }
889 1 }
890
891 main()
892 {
893 1 TMOD=0x22; //定时器1，方式2;
894 1 TL1=0XFD; //波特率9600
895 1 TH1=0XFD;
896 1 TR1=1; //启动定时器
897 1 PCON=0X00; //SMOD=0
898 1 TMOD=0x23; //定时器0，方式3,两个8位定时器;
899 1 TR1=0;
900 1 TF1=0;
901 1 S0CON=0XF8; //串口方式3，SM2=0，REN=1，TB8=1，RB8=0，TI=0，RI=0
902 1 IP0=0X98; //S0=1,T1=1
903 1 ES0=1; //开串口中断
904 1 ET0=1;
905 1 ET1=1;
906 1 ES1=1; //I2C
907 1 EAD=1; //AD中断允许
908 1 EA=1; //中断总允许
909 1 start_t2();
910 1
911 1 urgent_stop=0;
912 1 rw_err=0;
913 1 err_ok=0;
914 1 err=0;
915 1
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -