robocon_2016 / Mbed 2 deprecated 2016_slave_MD_rorikon

MD_rorikon_9/30

Dependencies:   PID QEI i2cslave mbed

Fork of 2016_slave_MD_rorikon by hidaka sato

main.cpp@0:d527838453f6, 2016-09-30 (annotated)

Committer:
sgrsn
Date:
Fri Sep 30 12:31:49 2016 +0000
Revision:
0:d527838453f6
MD_rorikon_9/30

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sgrsn 0:d527838453f6 1 #include "mbed.h"
sgrsn 0:d527838453f6 2 #include "QEI.h"
sgrsn 0:d527838453f6 3 #include "i2cslave.h"
sgrsn 0:d527838453f6 4 #include "PID.h"
sgrsn 0:d527838453f6 5 #include "define.h"
sgrsn 0:d527838453f6 6
sgrsn 0:d527838453f6 7 #define Enter 13
sgrsn 0:d527838453f6 8 #define BackSpace 8
sgrsn 0:d527838453f6 9 #define Space 32
sgrsn 0:d527838453f6 10
sgrsn 0:d527838453f6 11 union UNION
sgrsn 0:d527838453f6 12 {
sgrsn 0:d527838453f6 13 int integer;
sgrsn 0:d527838453f6 14 char c[6];
sgrsn 0:d527838453f6 15 };
sgrsn 0:d527838453f6 16
sgrsn 0:d527838453f6 17 UNION PM_target;
sgrsn 0:d527838453f6 18 UNION RE_target;
sgrsn 0:d527838453f6 19 UNION gain;
sgrsn 0:d527838453f6 20 UNION PM_ofset;
sgrsn 0:d527838453f6 21
sgrsn 0:d527838453f6 22 char Registar[128] = {};
sgrsn 0:d527838453f6 23 QEI rorikon(dp25, dp17, NC, 50, QEI::X4_ENCODING);
sgrsn 0:d527838453f6 24 //AnalogIn potentiometer(dp10);
sgrsn 0:d527838453f6 25 AnalogIn potentiometer(dp11);
sgrsn 0:d527838453f6 26 i2cslave myi2c(dp5, dp27, Registar);
sgrsn 0:d527838453f6 27 int getmode_PM_angle(const int kaisu);
sgrsn 0:d527838453f6 28 int checker = 0;
sgrsn 0:d527838453f6 29 void check();
sgrsn 0:d527838453f6 30
sgrsn 0:d527838453f6 31 int main()
sgrsn 0:d527838453f6 32 {
sgrsn 0:d527838453f6 33 NVIC_SetPriority(EINT3_IRQn, 30);
sgrsn 0:d527838453f6 34 NVIC_SetPriority(EINT2_IRQn, 30);
sgrsn 0:d527838453f6 35 NVIC_SetPriority(EINT1_IRQn, 30);
sgrsn 0:d527838453f6 36 NVIC_SetPriority(EINT0_IRQn, 30);
sgrsn 0:d527838453f6 37 NVIC_SetPriority(TIMER_16_0_IRQn, 20);
sgrsn 0:d527838453f6 38 NVIC_SetPriority(TIMER_16_1_IRQn, 20);
sgrsn 0:d527838453f6 39 NVIC_SetPriority(TIMER_32_0_IRQn, 20);
sgrsn 0:d527838453f6 40 NVIC_SetPriority(TIMER_32_1_IRQn, 20);
sgrsn 0:d527838453f6 41 NVIC_SetPriority(I2C_IRQn, 10);
sgrsn 0:d527838453f6 42 NVIC_SetPriority(UART_IRQn, 5);
sgrsn 0:d527838453f6 43
sgrsn 0:d527838453f6 44 /*BusOut PM_motor(dp4, dp9, dp13, dp14);
sgrsn 0:d527838453f6 45 BusOut RE_motor(dp25, dp24, dp18, dp17);*/
sgrsn 0:d527838453f6 46 BusOut PM_motor(dp14, dp13, dp10, dp9);
sgrsn 0:d527838453f6 47 BusOut RE_motor(dp28, dp26, dp24, dp18);
sgrsn 0:d527838453f6 48 PwmOut PM_pwm(dp2);
sgrsn 0:d527838453f6 49 PwmOut RE_pwm(dp1);
sgrsn 0:d527838453f6 50
sgrsn 0:d527838453f6 51 Ticker tic;
sgrsn 0:d527838453f6 52 tic.attach(check, 0.1);
sgrsn 0:d527838453f6 53 Timer t;
sgrsn 0:d527838453f6 54 PID PM_PID(&t);
sgrsn 0:d527838453f6 55 PID RE_PID(&t);
sgrsn 0:d527838453f6 56 PM_PID.set_parameter(0.0200, 0.43);//0.0200, 0.43//0.0200, 0.4 //0.0400, 0.317460317
sgrsn 0:d527838453f6 57 //RE_PID.set_parameter(0.001, 0.07);
sgrsn 0:d527838453f6 58 //RE_PID.set_parameter(0.00070, 0.30);//0.00074, 0.35 //0.00074, 0.277777778 //parameter of 1500
sgrsn 0:d527838453f6 59 //RE_PID.set_parameter(0.00025, 0.37); //0.00040, 0.344827586 //parameter of 300
sgrsn 0:d527838453f6 60 float pm_pid = 0;
sgrsn 0:d527838453f6 61 float re_pid = 0;
sgrsn 0:d527838453f6 62 float RE_angle = 0;
sgrsn 0:d527838453f6 63 float prev_RE_angle = 0;
sgrsn 0:d527838453f6 64 int PM_angle = 0;
sgrsn 0:d527838453f6 65 float nowtime = 0;
sgrsn 0:d527838453f6 66 float prev_time = 0;
sgrsn 0:d527838453f6 67 float RE_speed = 0;
sgrsn 0:d527838453f6 68 int target = 1800;
sgrsn 0:d527838453f6 69 int prev_target = 1800;
sgrsn 0:d527838453f6 70 float Threshold = 5.5;
sgrsn 0:d527838453f6 71 float RE_Threshold = 5;
sgrsn 0:d527838453f6 72 /*****************************************/
sgrsn 0:d527838453f6 73 myi2c.address(MD1_addr);
sgrsn 0:d527838453f6 74 /*****************************************/
sgrsn 0:d527838453f6 75 myi2c.frequency(1000000);
sgrsn 0:d527838453f6 76
sgrsn 0:d527838453f6 77 PM_ofset.integer = getmode_PM_angle(128);
sgrsn 0:d527838453f6 78 for(int i = 0; i < 4; i++)
sgrsn 0:d527838453f6 79 {
sgrsn 0:d527838453f6 80 Registar[PM_ofset_reg + i] = PM_ofset.c[i];
sgrsn 0:d527838453f6 81 }
sgrsn 0:d527838453f6 82 printf("start");
sgrsn 0:d527838453f6 83 t.start();
sgrsn 0:d527838453f6 84 while(1)
sgrsn 0:d527838453f6 85 {
sgrsn 0:d527838453f6 86 PM_angle = getmode_PM_angle(50);
sgrsn 0:d527838453f6 87 for(int i = 0; i < 5; i++)
sgrsn 0:d527838453f6 88 {
sgrsn 0:d527838453f6 89 PM_target.c[i] = Registar[PM_target_reg + i];
sgrsn 0:d527838453f6 90 RE_target.c[i] = Registar[RE_target_reg + i];
sgrsn 0:d527838453f6 91 gain.c[i] = Registar[Kp + i];
sgrsn 0:d527838453f6 92 }
sgrsn 0:d527838453f6 93 prev_target = target;
sgrsn 0:d527838453f6 94 target = PM_target.integer;
sgrsn 0:d527838453f6 95 /*if(!Registar[Joystick_ctrl] &&(abs((target - PM_ofset.integer) % 45) != 0))
sgrsn 0:d527838453f6 96 {
sgrsn 0:d527838453f6 97 target = prev_target;
sgrsn 0:d527838453f6 98 }*/
sgrsn 0:d527838453f6 99 //printf("%d.%d%d%d", PM_angle, target, (int)(PM_PID.Ki*10000), (int)nowtime);
sgrsn 0:d527838453f6 100 prev_RE_angle = RE_angle;
sgrsn 0:d527838453f6 101 prev_time = nowtime;
sgrsn 0:d527838453f6 102 RE_angle = rorikon.getSumangle();
sgrsn 0:d527838453f6 103 nowtime = t.read();
sgrsn 0:d527838453f6 104 RE_speed = (RE_angle - prev_RE_angle)/(nowtime - prev_time);
sgrsn 0:d527838453f6 105 printf("%f%d%d", RE_speed, gain.integer, (int)nowtime);
sgrsn 0:d527838453f6 106 switch(RE_target.integer)
sgrsn 0:d527838453f6 107 {
sgrsn 0:d527838453f6 108 case 1500:
sgrsn 0:d527838453f6 109 RE_PID.set_parameter(0.00070, 0.30);
sgrsn 0:d527838453f6 110 break;
sgrsn 0:d527838453f6 111 case 300:
sgrsn 0:d527838453f6 112 RE_PID.set_parameter(0.00025, 0.37);
sgrsn 0:d527838453f6 113 break;
sgrsn 0:d527838453f6 114 }
sgrsn 0:d527838453f6 115 printf("%d", Registar[wheel_start]);
sgrsn 0:d527838453f6 116 if(Registar[wheel_start] == 2)
sgrsn 0:d527838453f6 117 {
sgrsn 0:d527838453f6 118 re_pid = RE_PID.PI_lateD((-1 * (float)RE_target.integer), RE_speed);//, ((float)gain.integer)/10000);
sgrsn 0:d527838453f6 119 }
sgrsn 0:d527838453f6 120 else if(Registar[wheel_start] == 1)
sgrsn 0:d527838453f6 121 {
sgrsn 0:d527838453f6 122 re_pid = RE_PID.PI_lateD((float)RE_target.integer, RE_speed);//, ((float)gain.integer)/10000);
sgrsn 0:d527838453f6 123 }
sgrsn 0:d527838453f6 124 /*else if(Registar[pid_start] && ( (RE_speed < (RE_target.integer - RE_Threshold)) || (RE_speed > (RE_target.integer + RE_Threshold)) ) )
sgrsn 0:d527838453f6 125 {
sgrsn 0:d527838453f6 126 re_pid = RE_PID.control_P((float)RE_target.integer, RE_speed, (float)gain.integer/100000);
sgrsn 0:d527838453f6 127 }*/
sgrsn 0:d527838453f6 128 else
sgrsn 0:d527838453f6 129 {
sgrsn 0:d527838453f6 130 re_pid = 0;
sgrsn 0:d527838453f6 131 RE_PID.reset();
sgrsn 0:d527838453f6 132 //pid = mypid.control_P(0, PM_angle, 0.005);
sgrsn 0:d527838453f6 133 }
sgrsn 0:d527838453f6 134
sgrsn 0:d527838453f6 135 if(re_pid > 0.00005)
sgrsn 0:d527838453f6 136 {
sgrsn 0:d527838453f6 137 RE_motor = 0;
sgrsn 0:d527838453f6 138 wait_ms(5);
sgrsn 0:d527838453f6 139 RE_pwm = re_pid;
sgrsn 0:d527838453f6 140 RE_motor = 10;
sgrsn 0:d527838453f6 141 }
sgrsn 0:d527838453f6 142 else if(re_pid < -0.00005)
sgrsn 0:d527838453f6 143 {
sgrsn 0:d527838453f6 144 RE_motor = 0;
sgrsn 0:d527838453f6 145 wait_ms(5);
sgrsn 0:d527838453f6 146 RE_pwm = re_pid * (-1);
sgrsn 0:d527838453f6 147 RE_motor = 5;
sgrsn 0:d527838453f6 148 }
sgrsn 0:d527838453f6 149 else
sgrsn 0:d527838453f6 150 {
sgrsn 0:d527838453f6 151 RE_motor = 0;
sgrsn 0:d527838453f6 152 wait_ms(5);
sgrsn 0:d527838453f6 153 RE_pwm = 1;
sgrsn 0:d527838453f6 154 RE_motor = 3;
sgrsn 0:d527838453f6 155 }
sgrsn 0:d527838453f6 156
sgrsn 0:d527838453f6 157
sgrsn 0:d527838453f6 158 if( ( PM_angle > (target - Threshold) ) && ( PM_angle < (target + Threshold) ) )
sgrsn 0:d527838453f6 159 {
sgrsn 0:d527838453f6 160 pm_pid = 0;
sgrsn 0:d527838453f6 161 PM_PID.reset();
sgrsn 0:d527838453f6 162 PM_motor = 0;
sgrsn 0:d527838453f6 163 wait_ms(1);
sgrsn 0:d527838453f6 164 PM_motor = 3;
sgrsn 0:d527838453f6 165 PM_pwm = 1;
sgrsn 0:d527838453f6 166 }
sgrsn 0:d527838453f6 167
sgrsn 0:d527838453f6 168 if(Registar[PM_start])
sgrsn 0:d527838453f6 169 {
sgrsn 0:d527838453f6 170 pm_pid = PM_PID.control_P(target, PM_angle, 0.007);
sgrsn 0:d527838453f6 171 }
sgrsn 0:d527838453f6 172 if(Registar[PM_start] && Registar[pid_start] && ( (PM_angle < (target - Threshold)) || (PM_angle > (target + Threshold)) ) )
sgrsn 0:d527838453f6 173 {
sgrsn 0:d527838453f6 174 pm_pid = PM_PID.PI_lateD(target, PM_angle);//, ((float)gain.integer)/10000);
sgrsn 0:d527838453f6 175 }
sgrsn 0:d527838453f6 176 else if(Registar[pid_start])
sgrsn 0:d527838453f6 177 {
sgrsn 0:d527838453f6 178 pm_pid = 0;
sgrsn 0:d527838453f6 179 PM_PID.reset();
sgrsn 0:d527838453f6 180 //pid = mypid.control_P(0, PM_angle, 0.005);
sgrsn 0:d527838453f6 181 }
sgrsn 0:d527838453f6 182
sgrsn 0:d527838453f6 183 if(pm_pid > 0.04)
sgrsn 0:d527838453f6 184 {
sgrsn 0:d527838453f6 185 PM_motor = 0;
sgrsn 0:d527838453f6 186 wait_ms(1);
sgrsn 0:d527838453f6 187 PM_pwm = pm_pid;
sgrsn 0:d527838453f6 188 PM_motor = 5;
sgrsn 0:d527838453f6 189 }
sgrsn 0:d527838453f6 190 else if(pm_pid < -0.04)
sgrsn 0:d527838453f6 191 {
sgrsn 0:d527838453f6 192 PM_motor = 0;
sgrsn 0:d527838453f6 193 wait_ms(1);
sgrsn 0:d527838453f6 194 PM_pwm = pm_pid * (-1);
sgrsn 0:d527838453f6 195 PM_motor = 10;
sgrsn 0:d527838453f6 196 }
sgrsn 0:d527838453f6 197 else
sgrsn 0:d527838453f6 198 {
sgrsn 0:d527838453f6 199 PM_motor = 0;
sgrsn 0:d527838453f6 200 wait_ms(1);
sgrsn 0:d527838453f6 201 PM_pwm = 1;
sgrsn 0:d527838453f6 202 PM_motor = 3;
sgrsn 0:d527838453f6 203 PM_PID.reset();
sgrsn 0:d527838453f6 204 }
sgrsn 0:d527838453f6 205 printf("\r\n");
sgrsn 0:d527838453f6 206 wait_ms(60);
sgrsn 0:d527838453f6 207 }
sgrsn 0:d527838453f6 208 }
sgrsn 0:d527838453f6 209
sgrsn 0:d527838453f6 210 int getmode_PM_angle(const int kaisu)
sgrsn 0:d527838453f6 211 {
sgrsn 0:d527838453f6 212 int PMangle[128] = {};
sgrsn 0:d527838453f6 213 int samecount[128] = {};
sgrsn 0:d527838453f6 214 int tmpcount[128] = {};
sgrsn 0:d527838453f6 215 int num = 0;
sgrsn 0:d527838453f6 216 for(int i = 0; i < kaisu; i++)
sgrsn 0:d527838453f6 217 {
sgrsn 0:d527838453f6 218 PMangle[i] = (int)(potentiometer * 3600 + 0.5);
sgrsn 0:d527838453f6 219 }
sgrsn 0:d527838453f6 220 for(int i = 0; i < kaisu; i++)
sgrsn 0:d527838453f6 221 {
sgrsn 0:d527838453f6 222 for(int j = 0; j < kaisu; j++)
sgrsn 0:d527838453f6 223 {
sgrsn 0:d527838453f6 224 if(PMangle[i] == PMangle[j])
sgrsn 0:d527838453f6 225 {
sgrsn 0:d527838453f6 226 samecount[i]++;
sgrsn 0:d527838453f6 227 }
sgrsn 0:d527838453f6 228 }
sgrsn 0:d527838453f6 229 tmpcount[i] = samecount[i];
sgrsn 0:d527838453f6 230 }
sgrsn 0:d527838453f6 231 for(int i = 0; i < kaisu; i++)
sgrsn 0:d527838453f6 232 {
sgrsn 0:d527838453f6 233 for(int j = 0; j < i; j++)
sgrsn 0:d527838453f6 234 {
sgrsn 0:d527838453f6 235 if(tmpcount[i] > tmpcount[j])
sgrsn 0:d527838453f6 236 {
sgrsn 0:d527838453f6 237 int tmp = tmpcount[j];
sgrsn 0:d527838453f6 238 tmpcount[j] = tmpcount[i];
sgrsn 0:d527838453f6 239 tmpcount[i] = tmp;
sgrsn 0:d527838453f6 240 }
sgrsn 0:d527838453f6 241 }
sgrsn 0:d527838453f6 242 }
sgrsn 0:d527838453f6 243 int mostcount = tmpcount[0];
sgrsn 0:d527838453f6 244 for(int i = 0; i < kaisu; i++)
sgrsn 0:d527838453f6 245 {
sgrsn 0:d527838453f6 246 if(samecount[i] == mostcount)
sgrsn 0:d527838453f6 247 {
sgrsn 0:d527838453f6 248 num = i;
sgrsn 0:d527838453f6 249 }
sgrsn 0:d527838453f6 250 }
sgrsn 0:d527838453f6 251 return PMangle[num];
sgrsn 0:d527838453f6 252 }
sgrsn 0:d527838453f6 253
sgrsn 0:d527838453f6 254 void check()
sgrsn 0:d527838453f6 255 {
sgrsn 0:d527838453f6 256 Registar[check_reg]++;
sgrsn 0:d527838453f6 257 if(Registar[check_reg] > 2)
sgrsn 0:d527838453f6 258 {
sgrsn 0:d527838453f6 259 NVIC_SystemReset();
sgrsn 0:d527838453f6 260 }
sgrsn 0:d527838453f6 261 }