project-R / Mbed 2 deprecated TOUTEKI_all_mbed

TOUTEKI

Dependencies:   mbed QEI2 UnderBody Filter

main.cpp@5:869dc702b923, 2019-01-11 (annotated)

Committer:
e5118069
Date:
Fri Jan 11 09:33:04 2019 +0000
Revision:
5:869dc702b923
Parent:
4:017c55052d44
Child:
6:7afdc6a81566
2019111_complete

Who changed what in which revision?

UserRevisionLine numberNew contents of line
e5118069 0:d46cb1df87f1 1 #include "mbed.h"
e5118069 0:d46cb1df87f1 2 #include "QEI.h"
e5118069 1:94e15665b69f 3 #include "Filter.h"
e5118069 1:94e15665b69f 4 #define SB_ADRS 132
e5118069 1:94e15665b69f 5 #define SABER_ADDR 128
e5118069 0:d46cb1df87f1 6 #define INT_TIME 0.02
e5118069 0:d46cb1df87f1 7 #define RESOLUTION 48
e5118069 0:d46cb1df87f1 8 #define MULTIPLU 4.0
e5118069 0:d46cb1df87f1 9
e5118069 0:d46cb1df87f1 10 Ticker timer;
e5118069 0:d46cb1df87f1 11 Timer T;
e5118069 1:94e15665b69f 12 QEI Enc2(p7,p8,NC,RESOLUTION,&T,QEI::X4_ENCODING);
e5118069 1:94e15665b69f 13 QEI Enc3(p5,p6,NC,RESOLUTION,&T,QEI::X4_ENCODING);
e5118069 5:869dc702b923 14 QEI Enc(p12,p11,NC,RESOLUTION,&T,QEI::X4_ENCODING);
e5118069 3:de0b5dc55627 15 Serial Saber(p13,p14);
e5118069 3:de0b5dc55627 16 Serial pc(USBTX,USBRX);
e5118069 1:94e15665b69f 17 Filter velfilter(INT_TIME);
e5118069 0:d46cb1df87f1 18
e5118069 5:869dc702b923 19 DigitalIn sw2(p25);
e5118069 3:de0b5dc55627 20 DigitalIn sw1(p26);//モード切替
e5118069 3:de0b5dc55627 21 DigitalOut fet1(p22);
e5118069 2:965cba546262 22 DigitalOut fet2(p21);
e5118069 0:d46cb1df87f1 23
e5118069 2:965cba546262 24 DigitalIn limit1(p15);
e5118069 2:965cba546262 25 DigitalIn limit2(p16);
e5118069 1:94e15665b69f 26
e5118069 2:965cba546262 27 DigitalIn SENS1(p18);
e5118069 1:94e15665b69f 28 DigitalIn SENS2(p17);
e5118069 1:94e15665b69f 29
e5118069 5:869dc702b923 30 DigitalIn G_limit1(p9);
e5118069 5:869dc702b923 31 DigitalIn G_limit2(p10);
e5118069 5:869dc702b923 32
e5118069 2:965cba546262 33 int cmd,A;
e5118069 2:965cba546262 34 int SA1,B_SA1,LIM1,LIM2;
e5118069 2:965cba546262 35 int S1,S2;
e5118069 1:94e15665b69f 36
e5118069 1:94e15665b69f 37 float angle,pre_angle,SOKUDO,e_D,pre_e_D,ed_D,ei_D,e_V,ed_V,pre_e_V,bcmd;
e5118069 1:94e15665b69f 38 float goal_D=0,Kp=5,Ki=0.01,Kd=0.1;
e5118069 4:017c55052d44 39 float Ksp2 = 6.5, Ksd2 = 0.4;
e5118069 4:017c55052d44 40 float Ksp3 = 6.5, Ksd3 = 0.4;
e5118069 1:94e15665b69f 41
e5118069 2:965cba546262 42 float encount,b_encount;
e5118069 1:94e15665b69f 43
e5118069 5:869dc702b923 44 int mode = 7;
e5118069 0:d46cb1df87f1 45 int cmd2 = 0;
e5118069 0:d46cb1df87f1 46 int cmd3 = 0;
e5118069 0:d46cb1df87f1 47
e5118069 0:d46cb1df87f1 48 float spd2=0;
e5118069 0:d46cb1df87f1 49 float spd3=0;
e5118069 0:d46cb1df87f1 50
e5118069 0:d46cb1df87f1 51 float spd_err2=0;
e5118069 0:d46cb1df87f1 52 float spd_err3=0;
e5118069 0:d46cb1df87f1 53
e5118069 1:94e15665b69f 54 int tmp1;
e5118069 1:94e15665b69f 55 int tmp2;
e5118069 0:d46cb1df87f1 56
e5118069 1:94e15665b69f 57 double filtered_ref_spd;
e5118069 0:d46cb1df87f1 58
e5118069 5:869dc702b923 59 int G_LIM1=0,G_LIM2=0;
e5118069 5:869dc702b923 60
e5118069 5:869dc702b923 61 int G_cmd;
e5118069 5:869dc702b923 62
e5118069 5:869dc702b923 63 int modee=2;
e5118069 5:869dc702b923 64 int button_in_2=0;
e5118069 5:869dc702b923 65
e5118069 2:965cba546262 66 int Button() {
e5118069 1:94e15665b69f 67
e5118069 0:d46cb1df87f1 68 int button_in = sw1.read();
e5118069 1:94e15665b69f 69
e5118069 1:94e15665b69f 70 static int pre_button = 0;
e5118069 1:94e15665b69f 71 static int sw_state = 3;
e5118069 0:d46cb1df87f1 72
e5118069 1:94e15665b69f 73 if(button_in && pre_button)sw_state = 0;
e5118069 1:94e15665b69f 74 if(!button_in && !pre_button)sw_state = 3;
e5118069 1:94e15665b69f 75 if(button_in && !pre_button)sw_state = 1;
e5118069 1:94e15665b69f 76 if(!button_in && pre_button)sw_state = 2;
e5118069 0:d46cb1df87f1 77
e5118069 0:d46cb1df87f1 78 pre_button = button_in;
e5118069 0:d46cb1df87f1 79
e5118069 0:d46cb1df87f1 80 return sw_state;
e5118069 0:d46cb1df87f1 81 }
e5118069 5:869dc702b923 82 int Button2() {//スイッチのノイズをとる関数
e5118069 5:869dc702b923 83
e5118069 5:869dc702b923 84 button_in_2 = sw2.read();
e5118069 5:869dc702b923 85
e5118069 5:869dc702b923 86 static int pre_button_2 = 0;
e5118069 5:869dc702b923 87 static int sw_state_2 = 3;
e5118069 5:869dc702b923 88
e5118069 5:869dc702b923 89 if(button_in_2 && pre_button_2)sw_state_2 = 0;
e5118069 5:869dc702b923 90 if(!button_in_2 && !pre_button_2)sw_state_2 = 3;
e5118069 5:869dc702b923 91 if(button_in_2 && !pre_button_2)sw_state_2 = 1;
e5118069 5:869dc702b923 92 if(!button_in_2 && pre_button_2)sw_state_2 = 2;
e5118069 5:869dc702b923 93
e5118069 5:869dc702b923 94 pre_button_2 = button_in_2;
e5118069 5:869dc702b923 95
e5118069 5:869dc702b923 96 return sw_state_2;
e5118069 5:869dc702b923 97 }
e5118069 5:869dc702b923 98
e5118069 0:d46cb1df87f1 99
e5118069 0:d46cb1df87f1 100 void timer_warikomi()
e5118069 1:94e15665b69f 101 {
e5118069 1:94e15665b69f 102
e5118069 2:965cba546262 103 LIM1=!limit1.read();
e5118069 2:965cba546262 104 LIM2=!limit2.read();
e5118069 2:965cba546262 105 S1=SENS1.read();
e5118069 0:d46cb1df87f1 106 S2=SENS2.read();
e5118069 2:965cba546262 107 encount=Enc.getPulses()-b_encount;
e5118069 2:965cba546262 108
e5118069 2:965cba546262 109 float ppr = 1.0;
e5118069 0:d46cb1df87f1 110
e5118069 0:d46cb1df87f1 111 static float pre_spd2 = 0.0;
e5118069 0:d46cb1df87f1 112 static float pre_spd3 = 0.0;
e5118069 0:d46cb1df87f1 113
e5118069 0:d46cb1df87f1 114 static float pre_err2 = 0.0;
e5118069 0:d46cb1df87f1 115 static float pre_err3 = 0.0;
e5118069 0:d46cb1df87f1 116
e5118069 2:965cba546262 117 static float ref_spd = 0.0;
e5118069 2:965cba546262 118
e5118069 4:017c55052d44 119 static int lim_cmd2 = 87;
e5118069 2:965cba546262 120 static int lim_cmd3 = 92;
e5118069 2:965cba546262 121
e5118069 2:965cba546262 122 int sw_point = Button();
e5118069 2:965cba546262 123
e5118069 2:965cba546262 124 angle=(float)(encount)*(360.0/48.0)/4.0;
e5118069 2:965cba546262 125 SOKUDO=(angle-pre_angle)/INT_TIME;
e5118069 2:965cba546262 126
e5118069 2:965cba546262 127 e_D=(goal_D-angle);
e5118069 2:965cba546262 128 ed_D=(e_D-pre_e_D)/INT_TIME;
e5118069 2:965cba546262 129 ei_D+=(e_D+pre_e_D)*INT_TIME/2.0;
e5118069 2:965cba546262 130
e5118069 2:965cba546262 131 cmd=(int)((e_D*Kp)+(ed_D*Kd)+(ei_D*Ki));
e5118069 2:965cba546262 132
e5118069 2:965cba546262 133 float encount2 = Enc2.getPulses();
e5118069 2:965cba546262 134 float encount3 = Enc3.getPulses();
e5118069 2:965cba546262 135
e5118069 2:965cba546262 136 float rot_sp2 = encount2/MULTIPLU/ppr;
e5118069 2:965cba546262 137 spd2 = (rot_sp2 - pre_spd2)/INT_TIME/(48*4);
e5118069 2:965cba546262 138 float rot_sp3 = encount3/MULTIPLU/ppr;
e5118069 2:965cba546262 139 spd3 = (rot_sp3 - pre_spd3)/INT_TIME/(48*4);
e5118069 0:d46cb1df87f1 140
e5118069 2:965cba546262 141 spd_err2 = filtered_ref_spd - spd2;
e5118069 2:965cba546262 142 float spd_d2 = (spd_err2 - pre_err2)/INT_TIME;
e5118069 2:965cba546262 143 tmp1 = (int)((spd_err2 * Ksp2) + (spd_d2 * Ksd2));
e5118069 2:965cba546262 144 if(tmp1>=127)tmp1=127;
e5118069 2:965cba546262 145 if(tmp1<=-127)tmp1=-127;
e5118069 2:965cba546262 146 cmd2 += tmp1;
e5118069 0:d46cb1df87f1 147
e5118069 2:965cba546262 148 spd_err3 = filtered_ref_spd - spd3;
e5118069 2:965cba546262 149 float spd_d3 = (spd_err3 - pre_err3)/INT_TIME;
e5118069 2:965cba546262 150 tmp2 = (int)((spd_err3 * Ksp3) + (spd_d3 * Ksd3));
e5118069 2:965cba546262 151 if(tmp2>=127)tmp2=127;
e5118069 2:965cba546262 152 if(tmp2<=-127)tmp2=-127;
e5118069 2:965cba546262 153 cmd3 += tmp2;
e5118069 2:965cba546262 154
e5118069 2:965cba546262 155 if (cmd2 > lim_cmd2) cmd2 = lim_cmd2;
e5118069 2:965cba546262 156 if (cmd2 < -lim_cmd2) cmd2 = -lim_cmd2;
e5118069 2:965cba546262 157
e5118069 2:965cba546262 158 if (cmd3 > lim_cmd3) cmd3 = lim_cmd3;
e5118069 2:965cba546262 159 if (cmd3 < -lim_cmd3) cmd3 = -lim_cmd3;
e5118069 3:de0b5dc55627 160
e5118069 3:de0b5dc55627 161 if(sw_point != 0) switch(mode){
e5118069 1:94e15665b69f 162 case 0:
e5118069 0:d46cb1df87f1 163 goal_D=0;
e5118069 1:94e15665b69f 164 if(sw_point==2)mode=1;
e5118069 0:d46cb1df87f1 165 break;
e5118069 0:d46cb1df87f1 166 case 1:
e5118069 2:965cba546262 167 cmd=-15;
e5118069 1:94e15665b69f 168 if(sw_point==2)mode=2;
e5118069 0:d46cb1df87f1 169 if(LIM2==1){
e5118069 0:d46cb1df87f1 170 cmd=0;
e5118069 0:d46cb1df87f1 171 b_encount=Enc.getPulses();
e5118069 0:d46cb1df87f1 172 }
e5118069 0:d46cb1df87f1 173 break;
e5118069 3:de0b5dc55627 174 case 2:
e5118069 3:de0b5dc55627 175 goal_D=125;
e5118069 1:94e15665b69f 176 if(sw_point==2)mode=3;
e5118069 2:965cba546262 177 if(angle>=120)cmd=0;
e5118069 5:869dc702b923 178 if(S1==0&&S2==0){
e5118069 5:869dc702b923 179 fet1=1;
e5118069 5:869dc702b923 180 A=1;
e5118069 5:869dc702b923 181 }
e5118069 0:d46cb1df87f1 182 break;
e5118069 0:d46cb1df87f1 183
e5118069 5:869dc702b923 184 /*case 3:
e5118069 1:94e15665b69f 185 if(sw_point==2)mode=4;
e5118069 0:d46cb1df87f1 186 if(S1==0&&S2==0){
e5118069 3:de0b5dc55627 187 fet1=1;
e5118069 0:d46cb1df87f1 188 A=1;
e5118069 0:d46cb1df87f1 189 }
e5118069 5:869dc702b923 190 break;*/
e5118069 0:d46cb1df87f1 191
e5118069 5:869dc702b923 192 case 3:
e5118069 0:d46cb1df87f1 193 goal_D=0;
e5118069 1:94e15665b69f 194 if(sw_point==2){
e5118069 0:d46cb1df87f1 195 cmd=0;
e5118069 0:d46cb1df87f1 196 b_encount=Enc.getPulses();
e5118069 5:869dc702b923 197 mode=4;
e5118069 0:d46cb1df87f1 198 }
e5118069 0:d46cb1df87f1 199 break;
e5118069 5:869dc702b923 200 case 4:
e5118069 3:de0b5dc55627 201 fet1=0;
e5118069 5:869dc702b923 202 if(sw_point==2)mode=5;
e5118069 1:94e15665b69f 203 break;
e5118069 3:de0b5dc55627 204
e5118069 5:869dc702b923 205 case 5:
e5118069 3:de0b5dc55627 206 ref_spd = 26.0;
e5118069 5:869dc702b923 207 if (sw_point == 2) mode = 6;
e5118069 5:869dc702b923 208 break;
e5118069 5:869dc702b923 209
e5118069 5:869dc702b923 210 case 6:
e5118069 5:869dc702b923 211 fet2 = 0;
e5118069 3:de0b5dc55627 212 if (sw_point == 2) mode = 7;
e5118069 3:de0b5dc55627 213 break;
e5118069 0:d46cb1df87f1 214
e5118069 5:869dc702b923 215 case 7:
e5118069 3:de0b5dc55627 216 ref_spd = 0.0;
e5118069 3:de0b5dc55627 217 fet2 = 1;
e5118069 3:de0b5dc55627 218 if (sw_point == 2) mode = 0;
e5118069 5:869dc702b923 219 if (spd3<=5.0) {
e5118069 5:869dc702b923 220 cmd2 = 0;
e5118069 5:869dc702b923 221 cmd3 = 0;
e5118069 5:869dc702b923 222 }
e5118069 3:de0b5dc55627 223 break;
e5118069 3:de0b5dc55627 224 }
e5118069 4:017c55052d44 225
e5118069 5:869dc702b923 226 G_LIM1=!G_limit1.read();//pullupなので逆
e5118069 5:869dc702b923 227 G_LIM2=!G_limit2.read();//pullupなので逆
e5118069 5:869dc702b923 228
e5118069 5:869dc702b923 229 int sw_point2 = Button2();//スイッチの関数からリターン
e5118069 5:869dc702b923 230
e5118069 5:869dc702b923 231 if(sw_point2 != 0) switch(modee){
e5118069 5:869dc702b923 232 case(0):
e5118069 5:869dc702b923 233 G_cmd=120;
e5118069 5:869dc702b923 234 if(G_LIM1==1){
e5118069 5:869dc702b923 235 G_cmd=0;
e5118069 5:869dc702b923 236 }
e5118069 5:869dc702b923 237 if (sw_point2 == 2) modee = 1;
e5118069 5:869dc702b923 238
e5118069 5:869dc702b923 239 break;
e5118069 5:869dc702b923 240
e5118069 5:869dc702b923 241 case(1):
e5118069 5:869dc702b923 242 G_cmd=-120;
e5118069 5:869dc702b923 243 if (sw_point2 == 2) modee = 2;
e5118069 5:869dc702b923 244 if(G_LIM2==1){
e5118069 5:869dc702b923 245 G_cmd=0;
e5118069 5:869dc702b923 246 }
e5118069 5:869dc702b923 247 break;
e5118069 5:869dc702b923 248
e5118069 5:869dc702b923 249 case(2)://モータ停止と押し上げ機構の降下
e5118069 5:869dc702b923 250 G_cmd=0;
e5118069 5:869dc702b923 251 if (sw_point2 == 2) modee = 0;
e5118069 5:869dc702b923 252 break;
e5118069 5:869dc702b923 253 }
e5118069 5:869dc702b923 254
e5118069 5:869dc702b923 255
e5118069 5:869dc702b923 256 if(filtered_ref_spd>=25.5&&mode==5){
e5118069 4:017c55052d44 257 filtered_ref_spd=26;
e5118069 5:869dc702b923 258 }else if(filtered_ref_spd>=25.5&&mode==6){
e5118069 5:869dc702b923 259 filtered_ref_spd=26;
e5118069 5:869dc702b923 260 }else if(filtered_ref_spd<=5.0&&mode==7){
e5118069 4:017c55052d44 261 filtered_ref_spd=0;
e5118069 5:869dc702b923 262 }else if(mode==5||mode==6||mode==7){
e5118069 4:017c55052d44 263 filtered_ref_spd = velfilter.SecondOrderLag((double)ref_spd);
e5118069 4:017c55052d44 264 }
e5118069 1:94e15665b69f 265
e5118069 3:de0b5dc55627 266 if(cmd>20) cmd=20;
e5118069 2:965cba546262 267 if(cmd<-15)cmd=-15;
e5118069 1:94e15665b69f 268
e5118069 2:965cba546262 269 int F=1,FF=0;//向き
e5118069 0:d46cb1df87f1 270
e5118069 3:de0b5dc55627 271 if(cmd>=0) {
e5118069 2:965cba546262 272 Saber.putc(SABER_ADDR);
e5118069 2:965cba546262 273 Saber.putc(F);
e5118069 2:965cba546262 274 Saber.putc(abs(cmd));
e5118069 2:965cba546262 275 Saber.putc((SABER_ADDR + F + abs(cmd)) & 0b01111111);
e5118069 2:965cba546262 276 } else {
e5118069 2:965cba546262 277 Saber.putc(SABER_ADDR);
e5118069 2:965cba546262 278 Saber.putc(FF);
e5118069 2:965cba546262 279 Saber.putc(abs(cmd));
e5118069 2:965cba546262 280 Saber.putc((SABER_ADDR + FF + abs(cmd)) & 0b01111111);
e5118069 2:965cba546262 281 }
e5118069 0:d46cb1df87f1 282
e5118069 0:d46cb1df87f1 283 if (cmd2 > 0) {
e5118069 0:d46cb1df87f1 284 Saber.putc(SB_ADRS);
e5118069 0:d46cb1df87f1 285 Saber.putc(4);
e5118069 0:d46cb1df87f1 286 Saber.putc(cmd2);
e5118069 0:d46cb1df87f1 287 Saber.putc((SB_ADRS + 4 + cmd2) & 0b01111111);
e5118069 0:d46cb1df87f1 288 }
e5118069 0:d46cb1df87f1 289 else {
e5118069 0:d46cb1df87f1 290 Saber.putc(SB_ADRS);
e5118069 0:d46cb1df87f1 291 Saber.putc(5);
e5118069 0:d46cb1df87f1 292 Saber.putc(abs(cmd2));
e5118069 0:d46cb1df87f1 293 Saber.putc((SB_ADRS + 5 + abs(cmd2)) & 0b01111111);
e5118069 0:d46cb1df87f1 294 }
e5118069 0:d46cb1df87f1 295
e5118069 0:d46cb1df87f1 296 if (cmd3 > 0) {
e5118069 0:d46cb1df87f1 297 Saber.putc(SB_ADRS);
e5118069 0:d46cb1df87f1 298 Saber.putc(0);
e5118069 0:d46cb1df87f1 299 Saber.putc(cmd3);
e5118069 0:d46cb1df87f1 300 Saber.putc((SB_ADRS + 0 + cmd3) & 0b01111111);
e5118069 0:d46cb1df87f1 301 }
e5118069 0:d46cb1df87f1 302 else {
e5118069 0:d46cb1df87f1 303 Saber.putc(SB_ADRS);
e5118069 0:d46cb1df87f1 304 Saber.putc(1);
e5118069 0:d46cb1df87f1 305 Saber.putc(abs(cmd3));
e5118069 0:d46cb1df87f1 306 Saber.putc((SB_ADRS + 1 + abs(cmd3)) & 0b01111111);
e5118069 0:d46cb1df87f1 307 }
e5118069 5:869dc702b923 308 if (G_cmd > 0) {
e5118069 5:869dc702b923 309 Saber.putc(SABER_ADDR);
e5118069 5:869dc702b923 310 Saber.putc(4);
e5118069 5:869dc702b923 311 Saber.putc(G_cmd);
e5118069 5:869dc702b923 312 Saber.putc((SABER_ADDR + 4 + G_cmd) & 0b01111111);
e5118069 5:869dc702b923 313 }
e5118069 5:869dc702b923 314 else {
e5118069 5:869dc702b923 315 Saber.putc(SABER_ADDR);
e5118069 5:869dc702b923 316 Saber.putc(5);
e5118069 5:869dc702b923 317 Saber.putc(abs(G_cmd));
e5118069 5:869dc702b923 318 Saber.putc((SABER_ADDR + 5 + abs(G_cmd)) & 0b01111111);
e5118069 5:869dc702b923 319 }
e5118069 0:d46cb1df87f1 320
e5118069 3:de0b5dc55627 321 pre_spd2 = rot_sp2;
e5118069 3:de0b5dc55627 322 pre_err2 = spd_err2;
e5118069 3:de0b5dc55627 323 pre_spd3 = rot_sp3;
e5118069 3:de0b5dc55627 324 pre_err3 = spd_err3;
e5118069 1:94e15665b69f 325
e5118069 1:94e15665b69f 326
e5118069 3:de0b5dc55627 327 pre_e_D=e_D;
e5118069 1:94e15665b69f 328 pre_angle=angle;
e5118069 1:94e15665b69f 329 pre_e_V=e_V;
e5118069 2:965cba546262 330 B_SA1=SA1;
e5118069 0:d46cb1df87f1 331 }
e5118069 0:d46cb1df87f1 332
e5118069 0:d46cb1df87f1 333
e5118069 0:d46cb1df87f1 334 int main() {
e5118069 1:94e15665b69f 335 Saber.baud(19200);
e5118069 0:d46cb1df87f1 336 pc.baud(9600);
e5118069 3:de0b5dc55627 337 fet1=0;
e5118069 2:965cba546262 338 fet2=1;
e5118069 2:965cba546262 339
e5118069 5:869dc702b923 340 G_limit1.mode( PullUp ); // 内蔵プルアップを使う
e5118069 5:869dc702b923 341 G_limit2.mode( PullUp );
e5118069 5:869dc702b923 342
e5118069 1:94e15665b69f 343 velfilter.setSecondOrderPara(1.0, 0.9, 0.0);
e5118069 1:94e15665b69f 344
e5118069 0:d46cb1df87f1 345 timer.attach(timer_warikomi,INT_TIME);
e5118069 0:d46cb1df87f1 346
e5118069 0:d46cb1df87f1 347 while(1) {
e5118069 2:965cba546262 348 //pc.printf("%d\n",mode);
e5118069 5:869dc702b923 349 pc.printf("spd2%f\t spd3 %f\n",spd2,spd3);
e5118069 0:d46cb1df87f1 350 }
e5118069 1:94e15665b69f 351 }