hidaka sato
MD_rorikon_9/30
Dependencies: PID QEI i2cslave mbed
Diff: main.cpp
- Revision:
- 0:d527838453f6
diff -r 000000000000 -r d527838453f6 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Sep 30 12:31:49 2016 +0000
@@ -0,0 +1,261 @@
+#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();
+ }
+}
\ No newline at end of file