標(biāo)題: 瑞薩RL78/G13單片機(jī)風(fēng)力搖擺控制系統(tǒng)程序設(shè)計(jì) [打印本頁]
作者: uitwd    時(shí)間: 2019-6-30 17:41
標(biāo)題: 瑞薩RL78/G13單片機(jī)風(fēng)力搖擺控制系統(tǒng)程序設(shè)計(jì)

風(fēng)力搖擺控制系統(tǒng)源代碼

單片機(jī)源程序如下:
  1. /***********************************************************************************************************************
  2. * File Name    : r_main.c
  3. * Version      : CodeGenerator for RL78/G13 V2.00.00.07 [22 Feb 2013]
  4. * Device(s)    : R5F100LG
  5. * Tool-Chain   : CA78K0R
  6. * Description  : This file implements main function.
  7. * Creation Date: 2015/8/15
  8. ***********************************************************************************************************************/

  10. /***********************************************************************************************************************
  11. Pragma directive
  12. ***********************************************************************************************************************/
  13. /* Start user code for pragma. Do not edit comment generated here */
  14. /* End user code. Do not edit comment generated here */

  16. /***********************************************************************************************************************
  17. Includes
  18. ***********************************************************************************************************************/
  19. #include "r_cg_macrodriver.h"
  20. #include "r_cg_cgc.h"
  21. #include "r_cg_port.h"
  22. #include "r_cg_serial.h"
  23. #include "r_cg_timer.h"
  24. #include "r_cg_pclbuz.h"
  25. /* Start user code for include. Do not edit comment generated here */
  26. #include "r_cg_KeyScan.h"
  27. #include "r_cg_lcd.h"
  28. #include "Delay.h"
  29. #include "pid.h"
  30. /* End user code. Do not edit comment generated here */
  31. #include "r_cg_userdefine.h"

  33. /***********************************************************************************************************************
  34. Global variables and functions
  35. ***********************************************************************************************************************/
  36. /* Start user code for global. Do not edit comment generated here */
  37. #define  ZHENG_1   P5.0 //電機(jī)各信號線定義
  38. #define  FAN_1     P5.1
  39. #define  ZHENG_2   P5.2
  40. #define  FAN_2     P5.3
  41. #define  ZHENG_3   P5.4
  42. #define  FAN_3     P5.5
  43. #define  ZHENG_4   P6.0
  44. #define  FAN_4     P6.1
  45. #define  PWM_1           TDR06
  46. #define  PWM_2           TDR07
  47. #define  PWM_3           TDR04
  48. #define  PWM_4           TDR05

  50. int nRight_x;
  51. int nRight_y;
  52. float  x_angle,y_angle,x_anglespeed,y_anglespeed,y_anglejiasu;

  54. int expect_x_angle=0;
  55. int expect_y_angle=0;

  57. float  k_angle=1.8;     //4__ 3.0 0.5
  58. float  k_angle_dot=0.13;
  59. float  k_angle_jiasu=1;
  60. float  k;
  61. float  k_angle_dot_x,x_anglejiasu=1;
  62. unsigned int  set_angle;
  63. extern double Acceleration[3]; //加速度
  64. extern double AngularAcceleration[3]; //速度
  65. extern double Angle[3];//角度
  66. extern volatile unsigned char  num_keyboard;
  67. unsigned char key_1,key_2,key_3,key_4,key_5,key_6,key_7,key_8,key_9;
  68. void MotorSpeedOut_x(int i);
  69. void MotorSpeedOut_y(int i);
  70. void CarAngleAdjust_x(void);
  71. void CarAngleAdjust_y(void);
  72. extern void BUZZ(void)；
  73. void CarAngleAdjust_y_2(void);
  74. void CarAngleAdjust_x_2(void);
  75. /* End user code. Do not edit comment generated here */
  76. void R_MAIN_UserInit(void);

  78. /***********************************************************************************************************************
  79. * Function Name: main
  80. * Description  : This function implements main function.
  81. * Arguments    : None
  82. * Return Value : None
  83. ***********************************************************************************************************************/
  84. void main(void)
  85. {
  86.     R_MAIN_UserInit();
  87.     /* Start user code. Do not edit comment generated here */
  88.     lcd_init();
  89.     R_UART0_Start();
  90.     while (1U)
  91. {         lcd_display(0,"                "
  92.                            "    Welcome     ");       
  93.                        BUZZ();
  94.                        BUZZ();
  95.                        BUZZ();
  96.             while(Keyboard_scan()!=11);
  97.                         BUZZ();
  98.         while(Keyboard_scan()==11);
  99.          lcd_display(0,"please set mode "
  100.                            "      _____     ");
  101.         while(Keyboard_scan()>16||Keyboard_scan()<1);
  102.                         BUZZ();
  103.                      key_1=num_keyboard;
  104.         while(Keyboard_scan()!=0);
  105.        
  106.         lcd_write_number(24,key_1);
  107.         while(Keyboard_scan()!=11);
  108.                         BUZZ();
  109.         while(Keyboard_scan()==11);
  110.         if(key_1==1)
  111.         {         lcd_display(0,"     starting   "
  112.                                    "      __1__     ");
  113.              R_TAU0_Channel0_Start();
  114.              k_angle=0.65;     //4__ 3.0 0.5
  115.              k_angle_dot=0.06;
  116.              k_angle_jiasu=8.0;
  117.                       
  118.                   PWM_3=23100;
  119.                   while(Angle[1]>-8.5);
  120.                   PWM_3=0;
  121.                   while(Keyboard_scan()!=12)
  122.                  {   
  123.                   expect_y_angle=-8;
  124.                   y_angle=Angle[1];
  125.                   y_anglespeed=AngularAcceleration[1];
  126.                   y_anglejiasu=Acceleration[1];
  127.                   CarAngleAdjust_y_2();
  128.                  }
  129.                  R_TAU0_Channel0_Stop();
  130.        
  131.         }
  132.         else if(key_1==2)
  133.         {         lcd_display(0,"     set cent   "
  134.                                    "        __CM    ");
  135.                 while(Keyboard_scan()>6||Keyboard_scan()<3);
  136.                 BUZZ();
  137.                 key_2=num_keyboard;
  138.                 while(Keyboard_scan()!=0);
  139.                 lcd_write_number(24,key_2);
  140.                 while(Keyboard_scan()>10||Keyboard_scan()<1);
  141.                 BUZZ();
  142.                 key_3=num_keyboard;
  143.                 if(key_3==10){key_3=0;}
  144.                 while(Keyboard_scan()!=0);
  145.                 lcd_write_number(25,key_3);
  146.                 while(Keyboard_scan()!=11);
  147.                 BUZZ();
  148.                 while(Keyboard_scan()==11);
  149.                 lcd_display(0,"      set  K    "
  150.                                   "       40_._     ");
  151.                 while(Keyboard_scan()>10||Keyboard_scan()<1);
  152.                 BUZZ();
  153.                 key_5=num_keyboard;
  154.                 if(key_5==10){key_5=0;}
  155.                 while(Keyboard_scan()!=0);
  156.                 lcd_write_number(25,key_5);
  157.                 while(Keyboard_scan()>10||Keyboard_scan()<1);
  158.                 BUZZ();
  159.                 key_6=num_keyboard;
  160.                 if(key_6==10){key_6=0;}
  161.                 while(Keyboard_scan()!=0);
  162.                 lcd_write_number(27,key_6);
  163.                 k=400+key_5+key_6*0.1;
  164.                 while(Keyboard_scan()!=11);
  165.                 BUZZ();
  166.                 while(Keyboard_scan()==11);
  167.                 lcd_display(0,"     starting   "
  168.                                   "      __2__     ");
  169.             /*********************2*********************/
  170.              key_4=key_2*10+key_3;
  171.              R_TAU0_Channel0_Start();
  172.              k_angle=0.65;     //4__ 3.0 0.5
  173.              k_angle_dot=0.0001*key_4*key_4-0.013*key_4+0.48;
  174.              k_angle_jiasu=8.0;
  175.              
  176.                 //  PWM_3=12000; //30
  177.                   //  PWM_3=14400;// 35
  178.                 //  PWM_3=16000;// 40
  179.                 //PWM_3=17200;// 45
  180.                 // PWM_3=20400;//50
  181.                  //PWM_3 = 22000;// 55
  182.                 // PWM_3=23500;  //60
  183.                  
  184.                   PWM_3=k*key_4-121;
  185.                   while(Angle[1]>-8.5);
  186.                   PWM_3=0;
  187.                   while(Keyboard_scan()!=12)
  188.                  {   
  189.                   expect_y_angle=-8;
  190.                   y_angle=Angle[1];
  191.                   y_anglespeed=AngularAcceleration[1];
  192.                   y_anglejiasu=Acceleration[1];
  193.                   CarAngleAdjust_y_2();
  194.                  }
  195.                  R_TAU0_Channel0_Stop();
  196.        
  197.         }
  198.         else if(key_1==3)
  199. {                
  200.                 lcd_display(0,"    set  angle   "
  201.                                   "       __0       ");
  202.                 while(Keyboard_scan()!=1 &&Keyboard_scan()!=10);
  203.                 BUZZ();
  204.                 key_7=num_keyboard;
  205.                 if(key_7==10){key_7=0;}
  206.                 while(Keyboard_scan()!=0);
  207.                 lcd_write_number(24,key_7);
  208.                 while(Keyboard_scan()>8||Keyboard_scan()<1 && Keyboard_scan()!=10);
  209.                 BUZZ();
  210.                 key_8=num_keyboard;
  211.                 while(Keyboard_scan()!=0);
  212.                 if(key_8==10){key_8=0;}               
  213.                 lcd_write_number(25,key_8);
  214.                 while(Keyboard_scan()!=11);
  215.                 BUZZ();
  216.                 while(Keyboard_scan()==11);
  217.                 set_angle=key_7*100+key_8*10;
  218.                  lcd_display(0,"     starting   "
  219.                                    "      __3__     ");
  220.      /*******************3******************************/
  221.              R_TAU0_Channel0_Start();
  222.              if(set_angle<45)
  223.         {
  224.              k_angle=0.65;     
  225.              k_angle_dot=0.24; //1
  226.              k_angle_jiasu=8.0;       
  227.              k_angle_dot_x=-0.0042*(float)set_angle+0.41; //2
  228.              PWM_3=12000;
  229.              PWM_1=185.9*set_angle+1328; //3
  230.              while(Angle[1]>-5.5);
  231.              PWM_3=0;
  232.              PWM_1=0;
  233.              while(Keyboard_scan()!=12)
  234.              {   
  235.                   expect_y_angle=-8;
  236.                   expect_x_angle=-8;
  237.                   y_angle=Angle[1];
  238.                   y_anglespeed=AngularAcceleration[1];
  239.                   y_anglejiasu=Acceleration[1];
  240.                   x_angle=Angle[0];
  241.                   x_anglespeed=AngularAcceleration[0];
  242.                   x_anglejiasu=Acceleration[0];
  243.                   CarAngleAdjust_y_2();
  244.                   CarAngleAdjust_x_2();
  245.                  }
  246.              }
  247.              else if (set_angle>45 && set_angle<91)
  248.              {
  249.              set_angle=90-set_angle;
  250.              k_angle=0.65;     
  251.              k_angle_dot=0.185; //1
  252.              k_angle_jiasu=8.0;       
  253.              k_angle_dot_x=-0.0042*(float)set_angle+0.41; //2
  254.              PWM_3=194*set_angle+4388;
  255.              PWM_1=12000; //3
  256.              while(Angle[0]>-5.5);
  257.              PWM_3=0;
  258.              PWM_1=0;
  259.              while(Keyboard_scan()!=12)
  260.              {   
  261.                   expect_y_angle=-8;
  262.                   expect_x_angle=-8;
  263.                   y_angle=Angle[1];
  264.                   y_anglespeed=AngularAcceleration[1];
  265.                   y_anglejiasu=Acceleration[1];
  266.                   x_angle=Angle[0];
  267.                   x_anglespeed=AngularAcceleration[0];
  268.                   x_anglejiasu=Acceleration[0];
  269.                   CarAngleAdjust_y_2();
  270.                   CarAngleAdjust_x_2();
  271.                  }
  272.              }
  273.              
  274.              else if (set_angle>90&&set_angle<180)
  275. {
  276.              set_angle=180-set_angle;
  277.              if(set_angle<45)
  278.              {
  279.              k_angle=0.65;     
  280.              k_angle_dot=0.185; //1
  281.              k_angle_jiasu=8.0;       
  282.              k_angle_dot_x=-0.0042*(float)set_angle+0.41; //2
  283.              PWM_1=192*set_angle+4388;
  284.              PWM_4=12000; //3
  285.              while(Angle[1]<4.5);
  286.              PWM_1=0;
  287.              PWM_4=0;
  288.              while(Keyboard_scan()!=12)
  289.              {   
  290.                   expect_y_angle=8;
  291.                   expect_x_angle=8;
  292.                   y_angle=Angle[1];
  293.                   y_anglespeed=AngularAcceleration[1];
  294.                   y_anglejiasu=Acceleration[1];
  295.                   x_angle=Angle[0];
  296.                   x_anglespeed=AngularAcceleration[0];
  297.                   x_anglejiasu=Acceleration[0];
  298.                   CarAngleAdjust_y_2();
  299.                   CarAngleAdjust_x_2();
  300.               }
  301.               }
  302.              
  303.              else if (set_angle>45 && set_angle<91)
  304.              {
  305.              set_angle=90-set_angle;
  306.              k_angle=0.65;     
  307.              k_angle_dot=0.185; //1
  308.              k_angle_jiasu=8.0;       
  309.              k_angle_dot_x=-0.0042*(float)set_angle+0.41; //2
  310.              PWM_4=194*set_angle+4388;
  311.              PWM_1=12000; //3
  312.              while(Angle[1]<5.5);
  313.              PWM_4=0;
  314.              PWM_1=0;
  315.              while(Keyboard_scan()!=12)
  316.              {   
  317.                   expect_y_angle=8;
  318.                   expect_x_angle=8;
  319.                   y_angle=Angle[1];
  320.                   y_anglespeed=AngularAcceleration[1];
  321.                   y_anglejiasu=Acceleration[1];
  322.                   x_angle=Angle[0];
  323.                   x_anglespeed=AngularAcceleration[0];
  324.                   x_anglejiasu=Acceleration[0];
  325.                   CarAngleAdjust_y_2();
  326.                   CarAngleAdjust_x_2();
  327.                  }
  328.              
  329.              }
  330.              
  331.              
  332. }
  333.                  R_TAU0_Channel0_Stop();

  335.        
  336. }
  337.         else if(key_1==4)
  338.         {
  339.         lcd_display(0,"     starting   "
  340.                           "      __4__     ");
  341.         R_TAU0_Channel0_Start();
  342.         expect_x_angle=0;
  343.         expect_y_angle=0;
  344.         k_angle=1.8;     //4__ 3.0 0.5
  345.         k_angle_dot=0.125;
  346.                       while(Keyboard_scan()!=12)
  347.                       {
  348.                         y_angle=AngularAcceleration[1];
  349.                         x_angle=AngularAcceleration[0];
  350.                         y_anglespeed=Acceleration[1];
  351.                         x_anglespeed=Acceleration[0];
  352.                         CarAngleAdjust_x();
  353.                         CarAngleAdjust_y();
  354.                       }
  355.        
  356.         }
  357.        
  358.         else if(key_1==13)
  359.         {
  360.                 unsigned char write_data[10];
  361.         lcd_display(0,"x:              "
  362.                           "y:              ");

  364.                       while(Keyboard_scan()!=12)
  365.                       {
  366.                         sprintf(write_data,"%6.3f",Angle[0]);
  367.                         lcd_display(3,write_data);
  368.                         Delay_ms(100);
  369.                         sprintf(write_data,"%6.3f",Angle[1]);
  370.                         lcd_display(19,write_data);
  371.                         Delay_ms(100);
  372.                       }
  373.        
  374.         }
  375. }
  376. }

  378.    
  379.     /* End user code. Do not edit comment generated here */


  382. /***********************************************************************************************************************
  383. * Function Name: R_MAIN_UserInit
  384. * Description  : This function adds user code before implementing main function.
  385. * Arguments    : None
  386. * Return Value : None
  387. ***********************************************************************************************************************/
  388. void R_MAIN_UserInit(void)
  389. {
  390.     /* Start user code. Do not edit comment generated here */
  391.     EI();
  392.     /* End user code. Do not edit comment generated here */
  393. }

  395. /* Start user code for adding. Do not edit comment generated here */
  396. void CarAngleAdjust_x(void)
  397. {
  398.   int PWM_Right;                 //風(fēng)扇輸出PWM的值
  399.   int nSpeed_balance;                         //PWM控制變量
  400.   int nP, nD;                                         //PD控制量
  401.   nP = (int)(expect_x_angle-x_angle)*k_angle;         //傾角，濾波后，相當(dāng)于比例環(huán)節(jié)        ,轉(zhuǎn)化成PWM脈寬調(diào)制的直接控制量
  402.   nD = (int)x_anglespeed*k_angle_dot;         //角速度 ，角速度歸一化，相當(dāng)于微分環(huán)節(jié),轉(zhuǎn)化成PWM脈寬調(diào)制的直接控制量
  403.   nSpeed_balance = nD + nP;                 //實(shí)時(shí)角度+實(shí)時(shí)角速度，作為控制輸入量

  405. // if(nSpeed > MOTOR_SPEED_SET_MAX) nSpeed = MOTOR_SPEED_SET_MAX; //這個(gè)控制量輸出飽和保護(hù)，防止過大
  406. // else if(nSpeed < MOTOR_SPEED_SET_MIN) nSpeed = MOTOR_SPEED_SET_MIN;//防止過小
  407.   PWM_Right = nSpeed_balance ;//+ g_nRightMotorOutSpeed; //+ g_nMotorLeftRightDiff;
  408.   MotorSpeedOut_x(PWM_Right);
  409. }
  410. void CarAngleAdjust_y(void)
  411. {
  412.   int PWM_Right;                 //風(fēng)扇輸出PWM的值
  413.   int nSpeed_balance;                         //PWM控制變量
  414.   int nP, nD;                                         //PD控制量
  415. // if((expect_y_angle-y_angle)<0.5||(expect_y_angle-y_angle)>-0.5)
  416. //        {expect_y_angle=y_angle;}
  417.   nP = (int)(expect_y_angle-y_angle)*k_angle;         //傾角，濾波后，相當(dāng)于比例環(huán)節(jié)        ,轉(zhuǎn)化成PWM脈寬調(diào)制的直接控制量
  418.   nD = (int)y_anglespeed*k_angle_dot;         //角速度 ，角速度歸一化，相當(dāng)于微分環(huán)節(jié),轉(zhuǎn)化成PWM脈寬調(diào)制的直接控制量
  419.   nSpeed_balance = nD + nP;                 //實(shí)時(shí)角度+實(shí)時(shí)角速度，作為控制輸入量

  421. // if(nSpeed > MOTOR_SPEED_SET_MAX) nSpeed = MOTOR_SPEED_SET_MAX; //這個(gè)控制量輸出飽和保護(hù)，防止過大
  422. // else if(nSpeed < MOTOR_SPEED_SET_MIN) nSpeed = MOTOR_SPEED_SET_MIN;//防止過小
  423.   PWM_Right = nSpeed_balance ;//+ g_nRightMotorOutSpeed; //+ g_nMotorLeftRightDiff;
  424.   MotorSpeedOut_y(PWM_Right);
  425. }


  428. void MotorSpeedOut_x(int i)
  429. { int nRight_x;
  430.   if(nRight_x<0)
  431.   {        PWM_2=0;
  432.         nRight_x=-nRight_x;
  433.         //if(nRight>90)nRight=90;
  434.                 PWM_1=nRight_x*320;
  435.                 Delay_ms(2);
  436.                
  437.   }
  438.   else
  439.    {        PWM_1=0;
  440.         nRight_x=nRight_x;
  441.         //if(nRight>90)nRight=90;
  442.         PWM_2=nRight_x*320;
  443.         Delay_ms(2);
  444.    }
  445. }       


  448. void MotorSpeedOut_y(int i)
  449. { int nRight;
  450.    nRight_y=i;
  451.    
  452.   if(nRight<0)
  453.   {        PWM_4=0;
  454.         nRight_y=-nRight_y;
  455.         //if(nRight>90)nRight=90;
  456.                 PWM_3=nRight_y*320;
  457.                 Delay_ms(2);
  458.                
  459.   }
  460.   else
  461.    {        PWM_3=0;
  462.         nRight_y=nRight_y;
  463.         //if(nRight>90)nRight=90;
  464.         PWM_4=nRight_y*320;
  465.         Delay_ms(2);
  466.    }
  467. }       

  469. void lcd_display_speed(unsigned char i,unsigned int adc) //數(shù)值顯示
  470. {
  471. lcd_write_number(i,((adc/10000)%10));      
  472. lcd_write_number(i+1,((adc/1000)%10));
  473. lcd_write_number(i+2,((adc/100)%10));
  474. lcd_write_number(i+3,((adc/10)%10));
  475. lcd_write_number(i+4,((adc/1)%10));

  477. }

  479. void CarAngleAdjust_y_2(void)
  480. {
  481.   int PWM_Right;                 //風(fēng)扇輸出PWM的值
  482.   int nSpeed_balance;                         //PWM控制變量
  483.   int nP,nI,nD;                                         //PD控制量
  484. // if((expect_y_angle-y_angle)<0.5||(expect_y_angle-y_angle)>-0.5)
  485. //        {expect_y_angle=y_angle;}
  486.   nP = (int)(expect_y_angle-y_angle)*k_angle;         //傾角，濾波后，相當(dāng)于比例環(huán)節(jié)        ,轉(zhuǎn)化成PWM脈寬調(diào)制的直接控制量
  487.   nD = (int)y_anglespeed*k_angle_dot;         //角速度 ，角速度歸一化，相當(dāng)于微分環(huán)節(jié),轉(zhuǎn)化成PWM脈寬調(diào)制的直接控制量
  488.   nI = (int)y_anglejiasu*k_angle_jiasu;
  489.   nSpeed_balance = nD + nP;                 //實(shí)時(shí)角度+實(shí)時(shí)角速度，作為控制輸入量

  491. // if(nSpeed > MOTOR_SPEED_SET_MAX) nSpeed = MOTOR_SPEED_SET_MAX; //這個(gè)控制量輸出飽和保護(hù)，防止過大
  492. // else if(nSpeed < MOTOR_SPEED_SET_MIN) nSpeed = MOTOR_SPEED_SET_MIN;//防止過小
  493.   PWM_Right = nSpeed_balance ;//+ g_nRightMotorOutSpeed; //+ g_nMotorLeftRightDiff;
  494.   MotorSpeedOut_y(PWM_Right);
  495. }
  496. void CarAngleAdjust_x_2(void)
  497. {
  498.   int PWM_Right;               
  499.   int nSpeed_balance;                       
  500.   int nP,nI,nD;                                         
  501.   nP = (int)(expect_x_angle-x_angle)*k_angle;         
  502.   nD = (int)x_anglespeed*k_angle_dot_x;         
  503.   nI = (int)x_anglejiasu*k_angle_jiasu;
  504.   nSpeed_balance = nD + nP;
  505.   PWM_Right = nSpeed_balance;
  506.   MotorSpeedOut_x(PWM_Right);
  507. }
  508. /* End user code. Do not edit comment generated here */
復(fù)制代碼

所有資料51hei提供下載:
源程序.zip (187.6 KB, 下載次數(shù): 18)


作者: admin    時(shí)間: 2019-6-30 18:46
本帖需要重新編輯補(bǔ)全電路原理圖,源碼,詳細(xì)說明與圖片即可獲得100+黑幣(帖子下方有編輯按鈕)



歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1