MD_rorikon_9/30
Dependencies: PID QEI i2cslave mbed
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?
User | Revision | Line number | New 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 | } |