|
0.png (11.26 KB, 下載次數(shù): 37)
下載附件
2019-6-30 18:45 上傳
風(fēng)力搖擺控制系統(tǒng)源代碼
單片機源程序如下:
- /***********************************************************************************************************************
- * File Name : r_main.c
- * Version : CodeGenerator for RL78/G13 V2.00.00.07 [22 Feb 2013]
- * Device(s) : R5F100LG
- * Tool-Chain : CA78K0R
- * Description : This file implements main function.
- * Creation Date: 2015/8/15
- ***********************************************************************************************************************/
- /***********************************************************************************************************************
- Pragma directive
- ***********************************************************************************************************************/
- /* Start user code for pragma. Do not edit comment generated here */
- /* End user code. Do not edit comment generated here */
- /***********************************************************************************************************************
- Includes
- ***********************************************************************************************************************/
- #include "r_cg_macrodriver.h"
- #include "r_cg_cgc.h"
- #include "r_cg_port.h"
- #include "r_cg_serial.h"
- #include "r_cg_timer.h"
- #include "r_cg_pclbuz.h"
- /* Start user code for include. Do not edit comment generated here */
- #include "r_cg_KeyScan.h"
- #include "r_cg_lcd.h"
- #include "Delay.h"
- #include "pid.h"
- /* End user code. Do not edit comment generated here */
- #include "r_cg_userdefine.h"
- /***********************************************************************************************************************
- Global variables and functions
- ***********************************************************************************************************************/
- /* Start user code for global. Do not edit comment generated here */
- #define ZHENG_1 P5.0 //電機各信號線定義
- #define FAN_1 P5.1
- #define ZHENG_2 P5.2
- #define FAN_2 P5.3
- #define ZHENG_3 P5.4
- #define FAN_3 P5.5
- #define ZHENG_4 P6.0
- #define FAN_4 P6.1
- #define PWM_1 TDR06
- #define PWM_2 TDR07
- #define PWM_3 TDR04
- #define PWM_4 TDR05
- int nRight_x;
- int nRight_y;
- float x_angle,y_angle,x_anglespeed,y_anglespeed,y_anglejiasu;
- int expect_x_angle=0;
- int expect_y_angle=0;
- float k_angle=1.8; //4__ 3.0 0.5
- float k_angle_dot=0.13;
- float k_angle_jiasu=1;
- float k;
- float k_angle_dot_x,x_anglejiasu=1;
- unsigned int set_angle;
- extern double Acceleration[3]; //加速度
- extern double AngularAcceleration[3]; //速度
- extern double Angle[3];//角度
- extern volatile unsigned char num_keyboard;
- unsigned char key_1,key_2,key_3,key_4,key_5,key_6,key_7,key_8,key_9;
- void MotorSpeedOut_x(int i);
- void MotorSpeedOut_y(int i);
- void CarAngleAdjust_x(void);
- void CarAngleAdjust_y(void);
- extern void BUZZ(void);
- void CarAngleAdjust_y_2(void);
- void CarAngleAdjust_x_2(void);
- /* End user code. Do not edit comment generated here */
- void R_MAIN_UserInit(void);
- /***********************************************************************************************************************
- * Function Name: main
- * Description : This function implements main function.
- * Arguments : None
- * Return Value : None
- ***********************************************************************************************************************/
- void main(void)
- {
- R_MAIN_UserInit();
- /* Start user code. Do not edit comment generated here */
- lcd_init();
- R_UART0_Start();
- while (1U)
- { lcd_display(0," "
- " Welcome ");
- BUZZ();
- BUZZ();
- BUZZ();
- while(Keyboard_scan()!=11);
- BUZZ();
- while(Keyboard_scan()==11);
- lcd_display(0,"please set mode "
- " _____ ");
- while(Keyboard_scan()>16||Keyboard_scan()<1);
- BUZZ();
- key_1=num_keyboard;
- while(Keyboard_scan()!=0);
-
- lcd_write_number(24,key_1);
- while(Keyboard_scan()!=11);
- BUZZ();
- while(Keyboard_scan()==11);
- if(key_1==1)
- { lcd_display(0," starting "
- " __1__ ");
- R_TAU0_Channel0_Start();
- k_angle=0.65; //4__ 3.0 0.5
- k_angle_dot=0.06;
- k_angle_jiasu=8.0;
-
- PWM_3=23100;
- while(Angle[1]>-8.5);
- PWM_3=0;
- while(Keyboard_scan()!=12)
- {
- expect_y_angle=-8;
- y_angle=Angle[1];
- y_anglespeed=AngularAcceleration[1];
- y_anglejiasu=Acceleration[1];
- CarAngleAdjust_y_2();
- }
- R_TAU0_Channel0_Stop();
-
- }
- else if(key_1==2)
- { lcd_display(0," set cent "
- " __CM ");
- while(Keyboard_scan()>6||Keyboard_scan()<3);
- BUZZ();
- key_2=num_keyboard;
- while(Keyboard_scan()!=0);
- lcd_write_number(24,key_2);
- while(Keyboard_scan()>10||Keyboard_scan()<1);
- BUZZ();
- key_3=num_keyboard;
- if(key_3==10){key_3=0;}
- while(Keyboard_scan()!=0);
- lcd_write_number(25,key_3);
- while(Keyboard_scan()!=11);
- BUZZ();
- while(Keyboard_scan()==11);
- lcd_display(0," set K "
- " 40_._ ");
- while(Keyboard_scan()>10||Keyboard_scan()<1);
- BUZZ();
- key_5=num_keyboard;
- if(key_5==10){key_5=0;}
- while(Keyboard_scan()!=0);
- lcd_write_number(25,key_5);
- while(Keyboard_scan()>10||Keyboard_scan()<1);
- BUZZ();
- key_6=num_keyboard;
- if(key_6==10){key_6=0;}
- while(Keyboard_scan()!=0);
- lcd_write_number(27,key_6);
- k=400+key_5+key_6*0.1;
- while(Keyboard_scan()!=11);
- BUZZ();
- while(Keyboard_scan()==11);
- lcd_display(0," starting "
- " __2__ ");
- /*********************2*********************/
- key_4=key_2*10+key_3;
- R_TAU0_Channel0_Start();
- k_angle=0.65; //4__ 3.0 0.5
- k_angle_dot=0.0001*key_4*key_4-0.013*key_4+0.48;
- k_angle_jiasu=8.0;
-
- // PWM_3=12000; //30
- // PWM_3=14400;// 35
- // PWM_3=16000;// 40
- //PWM_3=17200;// 45
- // PWM_3=20400;//50
- //PWM_3 = 22000;// 55
- // PWM_3=23500; //60
-
- PWM_3=k*key_4-121;
- while(Angle[1]>-8.5);
- PWM_3=0;
- while(Keyboard_scan()!=12)
- {
- expect_y_angle=-8;
- y_angle=Angle[1];
- y_anglespeed=AngularAcceleration[1];
- y_anglejiasu=Acceleration[1];
- CarAngleAdjust_y_2();
- }
- R_TAU0_Channel0_Stop();
-
- }
- else if(key_1==3)
- {
- lcd_display(0," set angle "
- " __0 ");
- while(Keyboard_scan()!=1 &&Keyboard_scan()!=10);
- BUZZ();
- key_7=num_keyboard;
- if(key_7==10){key_7=0;}
- while(Keyboard_scan()!=0);
- lcd_write_number(24,key_7);
- while(Keyboard_scan()>8||Keyboard_scan()<1 && Keyboard_scan()!=10);
- BUZZ();
- key_8=num_keyboard;
- while(Keyboard_scan()!=0);
- if(key_8==10){key_8=0;}
- lcd_write_number(25,key_8);
- while(Keyboard_scan()!=11);
- BUZZ();
- while(Keyboard_scan()==11);
- set_angle=key_7*100+key_8*10;
- lcd_display(0," starting "
- " __3__ ");
- /*******************3******************************/
- R_TAU0_Channel0_Start();
- if(set_angle<45)
- {
- k_angle=0.65;
- k_angle_dot=0.24; //1
- k_angle_jiasu=8.0;
- k_angle_dot_x=-0.0042*(float)set_angle+0.41; //2
- PWM_3=12000;
- PWM_1=185.9*set_angle+1328; //3
- while(Angle[1]>-5.5);
- PWM_3=0;
- PWM_1=0;
- while(Keyboard_scan()!=12)
- {
- expect_y_angle=-8;
- expect_x_angle=-8;
- y_angle=Angle[1];
- y_anglespeed=AngularAcceleration[1];
- y_anglejiasu=Acceleration[1];
- x_angle=Angle[0];
- x_anglespeed=AngularAcceleration[0];
- x_anglejiasu=Acceleration[0];
- CarAngleAdjust_y_2();
- CarAngleAdjust_x_2();
- }
- }
- else if (set_angle>45 && set_angle<91)
- {
- set_angle=90-set_angle;
- k_angle=0.65;
- k_angle_dot=0.185; //1
- k_angle_jiasu=8.0;
- k_angle_dot_x=-0.0042*(float)set_angle+0.41; //2
- PWM_3=194*set_angle+4388;
- PWM_1=12000; //3
- while(Angle[0]>-5.5);
- PWM_3=0;
- PWM_1=0;
- while(Keyboard_scan()!=12)
- {
- expect_y_angle=-8;
- expect_x_angle=-8;
- y_angle=Angle[1];
- y_anglespeed=AngularAcceleration[1];
- y_anglejiasu=Acceleration[1];
- x_angle=Angle[0];
- x_anglespeed=AngularAcceleration[0];
- x_anglejiasu=Acceleration[0];
- CarAngleAdjust_y_2();
- CarAngleAdjust_x_2();
- }
- }
-
- else if (set_angle>90&&set_angle<180)
- {
- set_angle=180-set_angle;
- if(set_angle<45)
- {
- k_angle=0.65;
- k_angle_dot=0.185; //1
- k_angle_jiasu=8.0;
- k_angle_dot_x=-0.0042*(float)set_angle+0.41; //2
- PWM_1=192*set_angle+4388;
- PWM_4=12000; //3
- while(Angle[1]<4.5);
- PWM_1=0;
- PWM_4=0;
- while(Keyboard_scan()!=12)
- {
- expect_y_angle=8;
- expect_x_angle=8;
- y_angle=Angle[1];
- y_anglespeed=AngularAcceleration[1];
- y_anglejiasu=Acceleration[1];
- x_angle=Angle[0];
- x_anglespeed=AngularAcceleration[0];
- x_anglejiasu=Acceleration[0];
- CarAngleAdjust_y_2();
- CarAngleAdjust_x_2();
- }
- }
-
- else if (set_angle>45 && set_angle<91)
- {
- set_angle=90-set_angle;
- k_angle=0.65;
- k_angle_dot=0.185; //1
- k_angle_jiasu=8.0;
- k_angle_dot_x=-0.0042*(float)set_angle+0.41; //2
- PWM_4=194*set_angle+4388;
- PWM_1=12000; //3
- while(Angle[1]<5.5);
- PWM_4=0;
- PWM_1=0;
- while(Keyboard_scan()!=12)
- {
- expect_y_angle=8;
- expect_x_angle=8;
- y_angle=Angle[1];
- y_anglespeed=AngularAcceleration[1];
- y_anglejiasu=Acceleration[1];
- x_angle=Angle[0];
- x_anglespeed=AngularAcceleration[0];
- x_anglejiasu=Acceleration[0];
- CarAngleAdjust_y_2();
- CarAngleAdjust_x_2();
- }
-
- }
-
-
- }
- R_TAU0_Channel0_Stop();
-
- }
- else if(key_1==4)
- {
- lcd_display(0," starting "
- " __4__ ");
- R_TAU0_Channel0_Start();
- expect_x_angle=0;
- expect_y_angle=0;
- k_angle=1.8; //4__ 3.0 0.5
- k_angle_dot=0.125;
- while(Keyboard_scan()!=12)
- {
- y_angle=AngularAcceleration[1];
- x_angle=AngularAcceleration[0];
- y_anglespeed=Acceleration[1];
- x_anglespeed=Acceleration[0];
- CarAngleAdjust_x();
- CarAngleAdjust_y();
- }
-
- }
-
- else if(key_1==13)
- {
- unsigned char write_data[10];
- lcd_display(0,"x: "
- "y: ");
- while(Keyboard_scan()!=12)
- {
- sprintf(write_data,"%6.3f",Angle[0]);
- lcd_display(3,write_data);
- Delay_ms(100);
- sprintf(write_data,"%6.3f",Angle[1]);
- lcd_display(19,write_data);
- Delay_ms(100);
- }
-
- }
- }
- }
-
-
- /* End user code. Do not edit comment generated here */
- /***********************************************************************************************************************
- * Function Name: R_MAIN_UserInit
- * Description : This function adds user code before implementing main function.
- * Arguments : None
- * Return Value : None
- ***********************************************************************************************************************/
- void R_MAIN_UserInit(void)
- {
- /* Start user code. Do not edit comment generated here */
- EI();
- /* End user code. Do not edit comment generated here */
- }
- /* Start user code for adding. Do not edit comment generated here */
- void CarAngleAdjust_x(void)
- {
- int PWM_Right; //風(fēng)扇輸出PWM的值
- int nSpeed_balance; //PWM控制變量
- int nP, nD; //PD控制量
- nP = (int)(expect_x_angle-x_angle)*k_angle; //傾角,濾波后,相當(dāng)于比例環(huán)節(jié) ,轉(zhuǎn)化成PWM脈寬調(diào)制的直接控制量
- nD = (int)x_anglespeed*k_angle_dot; //角速度 ,角速度歸一化,相當(dāng)于微分環(huán)節(jié),轉(zhuǎn)化成PWM脈寬調(diào)制的直接控制量
- nSpeed_balance = nD + nP; //實時角度+實時角速度,作為控制輸入量
- // if(nSpeed > MOTOR_SPEED_SET_MAX) nSpeed = MOTOR_SPEED_SET_MAX; //這個控制量輸出飽和保護,防止過大
- // else if(nSpeed < MOTOR_SPEED_SET_MIN) nSpeed = MOTOR_SPEED_SET_MIN;//防止過小
- PWM_Right = nSpeed_balance ;//+ g_nRightMotorOutSpeed; //+ g_nMotorLeftRightDiff;
- MotorSpeedOut_x(PWM_Right);
- }
- void CarAngleAdjust_y(void)
- {
- int PWM_Right; //風(fēng)扇輸出PWM的值
- int nSpeed_balance; //PWM控制變量
- int nP, nD; //PD控制量
- // if((expect_y_angle-y_angle)<0.5||(expect_y_angle-y_angle)>-0.5)
- // {expect_y_angle=y_angle;}
- nP = (int)(expect_y_angle-y_angle)*k_angle; //傾角,濾波后,相當(dāng)于比例環(huán)節(jié) ,轉(zhuǎn)化成PWM脈寬調(diào)制的直接控制量
- nD = (int)y_anglespeed*k_angle_dot; //角速度 ,角速度歸一化,相當(dāng)于微分環(huán)節(jié),轉(zhuǎn)化成PWM脈寬調(diào)制的直接控制量
- nSpeed_balance = nD + nP; //實時角度+實時角速度,作為控制輸入量
- // if(nSpeed > MOTOR_SPEED_SET_MAX) nSpeed = MOTOR_SPEED_SET_MAX; //這個控制量輸出飽和保護,防止過大
- // else if(nSpeed < MOTOR_SPEED_SET_MIN) nSpeed = MOTOR_SPEED_SET_MIN;//防止過小
- PWM_Right = nSpeed_balance ;//+ g_nRightMotorOutSpeed; //+ g_nMotorLeftRightDiff;
- MotorSpeedOut_y(PWM_Right);
- }
- void MotorSpeedOut_x(int i)
- { int nRight_x;
- if(nRight_x<0)
- { PWM_2=0;
- nRight_x=-nRight_x;
- //if(nRight>90)nRight=90;
- PWM_1=nRight_x*320;
- Delay_ms(2);
-
- }
- else
- { PWM_1=0;
- nRight_x=nRight_x;
- //if(nRight>90)nRight=90;
- PWM_2=nRight_x*320;
- Delay_ms(2);
- }
- }
- void MotorSpeedOut_y(int i)
- { int nRight;
- nRight_y=i;
-
- if(nRight<0)
- { PWM_4=0;
- nRight_y=-nRight_y;
- //if(nRight>90)nRight=90;
- PWM_3=nRight_y*320;
- Delay_ms(2);
-
- }
- else
- { PWM_3=0;
- nRight_y=nRight_y;
- //if(nRight>90)nRight=90;
- PWM_4=nRight_y*320;
- Delay_ms(2);
- }
- }
- void lcd_display_speed(unsigned char i,unsigned int adc) //數(shù)值顯示
- {
- lcd_write_number(i,((adc/10000)%10));
- lcd_write_number(i+1,((adc/1000)%10));
- lcd_write_number(i+2,((adc/100)%10));
- lcd_write_number(i+3,((adc/10)%10));
- lcd_write_number(i+4,((adc/1)%10));
- }
- void CarAngleAdjust_y_2(void)
- {
- int PWM_Right; //風(fēng)扇輸出PWM的值
- int nSpeed_balance; //PWM控制變量
- int nP,nI,nD; //PD控制量
- // if((expect_y_angle-y_angle)<0.5||(expect_y_angle-y_angle)>-0.5)
- // {expect_y_angle=y_angle;}
- nP = (int)(expect_y_angle-y_angle)*k_angle; //傾角,濾波后,相當(dāng)于比例環(huán)節(jié) ,轉(zhuǎn)化成PWM脈寬調(diào)制的直接控制量
- nD = (int)y_anglespeed*k_angle_dot; //角速度 ,角速度歸一化,相當(dāng)于微分環(huán)節(jié),轉(zhuǎn)化成PWM脈寬調(diào)制的直接控制量
- nI = (int)y_anglejiasu*k_angle_jiasu;
- nSpeed_balance = nD + nP; //實時角度+實時角速度,作為控制輸入量
- // if(nSpeed > MOTOR_SPEED_SET_MAX) nSpeed = MOTOR_SPEED_SET_MAX; //這個控制量輸出飽和保護,防止過大
- // else if(nSpeed < MOTOR_SPEED_SET_MIN) nSpeed = MOTOR_SPEED_SET_MIN;//防止過小
- PWM_Right = nSpeed_balance ;//+ g_nRightMotorOutSpeed; //+ g_nMotorLeftRightDiff;
- MotorSpeedOut_y(PWM_Right);
- }
- void CarAngleAdjust_x_2(void)
- {
- int PWM_Right;
- int nSpeed_balance;
- int nP,nI,nD;
- nP = (int)(expect_x_angle-x_angle)*k_angle;
- nD = (int)x_anglespeed*k_angle_dot_x;
- nI = (int)x_anglejiasu*k_angle_jiasu;
- nSpeed_balance = nD + nP;
- PWM_Right = nSpeed_balance;
- MotorSpeedOut_x(PWM_Right);
- }
- /* End user code. Do not edit comment generated here */
復(fù)制代碼
所有資料51hei提供下載:
源程序.zip
(187.6 KB, 下載次數(shù): 18)
2019-6-30 17:51 上傳
點擊文件名下載附件
下載積分: 黑幣 -5
|
|