![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
MD_rorikon_9/30
Dependencies: PID QEI i2cslave mbed
Fork of 2016_slave_MD_rorikon by
main.cpp
- Committer:
- sgrsn
- Date:
- 2016-09-30
- Revision:
- 0:d527838453f6
File content as of revision 0:d527838453f6:
#include "mbed.h" #include "QEI.h" #include "i2cslave.h" #include "PID.h" #include "define.h" #define Enter 13 #define BackSpace 8 #define Space 32 union UNION { int integer; char c[6]; }; UNION PM_target; UNION RE_target; UNION gain; UNION PM_ofset; char Registar[128] = {}; QEI rorikon(dp25, dp17, NC, 50, QEI::X4_ENCODING); //AnalogIn potentiometer(dp10); AnalogIn potentiometer(dp11); i2cslave myi2c(dp5, dp27, Registar); int getmode_PM_angle(const int kaisu); int checker = 0; void check(); int main() { NVIC_SetPriority(EINT3_IRQn, 30); NVIC_SetPriority(EINT2_IRQn, 30); NVIC_SetPriority(EINT1_IRQn, 30); NVIC_SetPriority(EINT0_IRQn, 30); NVIC_SetPriority(TIMER_16_0_IRQn, 20); NVIC_SetPriority(TIMER_16_1_IRQn, 20); NVIC_SetPriority(TIMER_32_0_IRQn, 20); NVIC_SetPriority(TIMER_32_1_IRQn, 20); NVIC_SetPriority(I2C_IRQn, 10); NVIC_SetPriority(UART_IRQn, 5); /*BusOut PM_motor(dp4, dp9, dp13, dp14); BusOut RE_motor(dp25, dp24, dp18, dp17);*/ BusOut PM_motor(dp14, dp13, dp10, dp9); BusOut RE_motor(dp28, dp26, dp24, dp18); PwmOut PM_pwm(dp2); PwmOut RE_pwm(dp1); Ticker tic; tic.attach(check, 0.1); Timer t; PID PM_PID(&t); PID RE_PID(&t); PM_PID.set_parameter(0.0200, 0.43);//0.0200, 0.43//0.0200, 0.4 //0.0400, 0.317460317 //RE_PID.set_parameter(0.001, 0.07); //RE_PID.set_parameter(0.00070, 0.30);//0.00074, 0.35 //0.00074, 0.277777778 //parameter of 1500 //RE_PID.set_parameter(0.00025, 0.37); //0.00040, 0.344827586 //parameter of 300 float pm_pid = 0; float re_pid = 0; float RE_angle = 0; float prev_RE_angle = 0; int PM_angle = 0; float nowtime = 0; float prev_time = 0; float RE_speed = 0; int target = 1800; int prev_target = 1800; float Threshold = 5.5; float RE_Threshold = 5; /*****************************************/ myi2c.address(MD1_addr); /*****************************************/ myi2c.frequency(1000000); PM_ofset.integer = getmode_PM_angle(128); for(int i = 0; i < 4; i++) { Registar[PM_ofset_reg + i] = PM_ofset.c[i]; } printf("start"); t.start(); while(1) { PM_angle = getmode_PM_angle(50); for(int i = 0; i < 5; i++) { PM_target.c[i] = Registar[PM_target_reg + i]; RE_target.c[i] = Registar[RE_target_reg + i]; gain.c[i] = Registar[Kp + i]; } prev_target = target; target = PM_target.integer; /*if(!Registar[Joystick_ctrl] &&(abs((target - PM_ofset.integer) % 45) != 0)) { target = prev_target; }*/ //printf("%d.%d%d%d", PM_angle, target, (int)(PM_PID.Ki*10000), (int)nowtime); prev_RE_angle = RE_angle; prev_time = nowtime; RE_angle = rorikon.getSumangle(); nowtime = t.read(); RE_speed = (RE_angle - prev_RE_angle)/(nowtime - prev_time); printf("%f%d%d", RE_speed, gain.integer, (int)nowtime); switch(RE_target.integer) { case 1500: RE_PID.set_parameter(0.00070, 0.30); break; case 300: RE_PID.set_parameter(0.00025, 0.37); break; } printf("%d", Registar[wheel_start]); if(Registar[wheel_start] == 2) { re_pid = RE_PID.PI_lateD((-1 * (float)RE_target.integer), RE_speed);//, ((float)gain.integer)/10000); } else if(Registar[wheel_start] == 1) { re_pid = RE_PID.PI_lateD((float)RE_target.integer, RE_speed);//, ((float)gain.integer)/10000); } /*else if(Registar[pid_start] && ( (RE_speed < (RE_target.integer - RE_Threshold)) || (RE_speed > (RE_target.integer + RE_Threshold)) ) ) { re_pid = RE_PID.control_P((float)RE_target.integer, RE_speed, (float)gain.integer/100000); }*/ else { re_pid = 0; RE_PID.reset(); //pid = mypid.control_P(0, PM_angle, 0.005); } if(re_pid > 0.00005) { RE_motor = 0; wait_ms(5); RE_pwm = re_pid; RE_motor = 10; } else if(re_pid < -0.00005) { RE_motor = 0; wait_ms(5); RE_pwm = re_pid * (-1); RE_motor = 5; } else { RE_motor = 0; wait_ms(5); RE_pwm = 1; RE_motor = 3; } if( ( PM_angle > (target - Threshold) ) && ( PM_angle < (target + Threshold) ) ) { pm_pid = 0; PM_PID.reset(); PM_motor = 0; wait_ms(1); PM_motor = 3; PM_pwm = 1; } if(Registar[PM_start]) { pm_pid = PM_PID.control_P(target, PM_angle, 0.007); } if(Registar[PM_start] && Registar[pid_start] && ( (PM_angle < (target - Threshold)) || (PM_angle > (target + Threshold)) ) ) { pm_pid = PM_PID.PI_lateD(target, PM_angle);//, ((float)gain.integer)/10000); } else if(Registar[pid_start]) { pm_pid = 0; PM_PID.reset(); //pid = mypid.control_P(0, PM_angle, 0.005); } if(pm_pid > 0.04) { PM_motor = 0; wait_ms(1); PM_pwm = pm_pid; PM_motor = 5; } else if(pm_pid < -0.04) { PM_motor = 0; wait_ms(1); PM_pwm = pm_pid * (-1); PM_motor = 10; } else { PM_motor = 0; wait_ms(1); PM_pwm = 1; PM_motor = 3; PM_PID.reset(); } printf("\r\n"); wait_ms(60); } } int getmode_PM_angle(const int kaisu) { int PMangle[128] = {}; int samecount[128] = {}; int tmpcount[128] = {}; int num = 0; for(int i = 0; i < kaisu; i++) { PMangle[i] = (int)(potentiometer * 3600 + 0.5); } for(int i = 0; i < kaisu; i++) { for(int j = 0; j < kaisu; j++) { if(PMangle[i] == PMangle[j]) { samecount[i]++; } } tmpcount[i] = samecount[i]; } for(int i = 0; i < kaisu; i++) { for(int j = 0; j < i; j++) { if(tmpcount[i] > tmpcount[j]) { int tmp = tmpcount[j]; tmpcount[j] = tmpcount[i]; tmpcount[i] = tmp; } } } int mostcount = tmpcount[0]; for(int i = 0; i < kaisu; i++) { if(samecount[i] == mostcount) { num = i; } } return PMangle[num]; } void check() { Registar[check_reg]++; if(Registar[check_reg] > 2) { NVIC_SystemReset(); } }