project-R / Mbed 2 deprecated TOUTEKI_all_mbed

TOUTEKI

Dependencies:   mbed QEI2 UnderBody Filter

main.cpp@1:94e15665b69f, 2019-01-08 (annotated)

Committer:
e5118069
Date:
Tue Jan 08 11:47:56 2019 +0000
Revision:
1:94e15665b69f
Parent:
0:d46cb1df87f1
Child:
2:965cba546262
Description;

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 0:d46cb1df87f1 12
e5118069 1:94e15665b69f 13 QEI Enc2(p7,p8,NC,RESOLUTION,&T,QEI::X4_ENCODING);
e5118069 1:94e15665b69f 14 QEI Enc3(p5,p6,NC,RESOLUTION,&T,QEI::X4_ENCODING);
e5118069 0:d46cb1df87f1 15
e5118069 1:94e15665b69f 16 Filter velfilter(INT_TIME);
e5118069 0:d46cb1df87f1 17
e5118069 1:94e15665b69f 18 DigitalIn sw1(p26);//mode切替スイッチ
e5118069 0:d46cb1df87f1 19
e5118069 1:94e15665b69f 20 DigitalOut fet2(p21);//shagai押し出し用のair
e5118069 0:d46cb1df87f1 21
e5118069 1:94e15665b69f 22 QEI Enc(p12,p11,NC,48,&T,QEI::X4_ENCODING);//200は分解能
e5118069 0:d46cb1df87f1 23 Serial Saber(p13,p14);
e5118069 0:d46cb1df87f1 24 Serial pc(USBTX,USBRX);
e5118069 0:d46cb1df87f1 25
e5118069 1:94e15665b69f 26 DigitalIn limit1(p15);//hagai把持
e5118069 1:94e15665b69f 27 DigitalIn limit2(p16);//初期位置合わせのlimitスイッチ
e5118069 1:94e15665b69f 28
e5118069 1:94e15665b69f 29 DigitalOut air(p22);//電磁弁
e5118069 1:94e15665b69f 30
e5118069 1:94e15665b69f 31 DigitalIn SENS1(p18);//光電センサ
e5118069 1:94e15665b69f 32 DigitalIn SENS2(p17);
e5118069 1:94e15665b69f 33
e5118069 1:94e15665b69f 34 int cmd,A;//Aはairの表示のため
e5118069 1:94e15665b69f 35 int SA1,B_SA1,LIM1,LIM2;//SA1はsA1の仮スイッチを入れる関数 B_SA1はbefore sA1の略
e5118069 1:94e15665b69f 36 int S1,S2;//光電センサ
e5118069 1:94e15665b69f 37
e5118069 1:94e15665b69f 38 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 39 float goal_D=0,Kp=5,Ki=0.01,Kd=0.1;
e5118069 1:94e15665b69f 40
e5118069 1:94e15665b69f 41 float encount,b_encount;//初期位置合わせの際に用いる初期位置合わせるための角度
e5118069 1:94e15665b69f 42
e5118069 1:94e15665b69f 43
e5118069 0:d46cb1df87f1 44 int mode = 8;//スイッチを押したときのモード
e5118069 0:d46cb1df87f1 45
e5118069 0:d46cb1df87f1 46 int cmd2 = 0;
e5118069 0:d46cb1df87f1 47 int cmd3 = 0;
e5118069 0:d46cb1df87f1 48
e5118069 0:d46cb1df87f1 49 float spd2=0;
e5118069 0:d46cb1df87f1 50 float spd3=0;
e5118069 0:d46cb1df87f1 51
e5118069 0:d46cb1df87f1 52 float spd_err2=0;
e5118069 0:d46cb1df87f1 53 float spd_err3=0;
e5118069 0:d46cb1df87f1 54
e5118069 1:94e15665b69f 55 int tmp1;
e5118069 1:94e15665b69f 56 int tmp2;
e5118069 0:d46cb1df87f1 57
e5118069 1:94e15665b69f 58 double filtered_ref_spd;
e5118069 0:d46cb1df87f1 59
e5118069 1:94e15665b69f 60 int Button() {//スイッチのノイズをとる関数
e5118069 1:94e15665b69f 61
e5118069 0:d46cb1df87f1 62 int button_in = sw1.read();
e5118069 1:94e15665b69f 63
e5118069 1:94e15665b69f 64 static int pre_button = 0;
e5118069 1:94e15665b69f 65 static int sw_state = 3;
e5118069 0:d46cb1df87f1 66
e5118069 1:94e15665b69f 67 if(button_in && pre_button)sw_state = 0;
e5118069 1:94e15665b69f 68 if(!button_in && !pre_button)sw_state = 3;
e5118069 1:94e15665b69f 69 if(button_in && !pre_button)sw_state = 1;
e5118069 1:94e15665b69f 70 if(!button_in && pre_button)sw_state = 2;
e5118069 0:d46cb1df87f1 71
e5118069 0:d46cb1df87f1 72 pre_button = button_in;
e5118069 0:d46cb1df87f1 73
e5118069 0:d46cb1df87f1 74 return sw_state;
e5118069 0:d46cb1df87f1 75 }
e5118069 0:d46cb1df87f1 76
e5118069 0:d46cb1df87f1 77 void timer_warikomi()
e5118069 1:94e15665b69f 78 {
e5118069 1:94e15665b69f 79
e5118069 0:d46cb1df87f1 80 LIM1=!limit1.read();//pullupなので逆
e5118069 0:d46cb1df87f1 81 LIM2=!limit2.read();//pullupなので逆
e5118069 0:d46cb1df87f1 82 S1=SENS1.read();//光電センサ
e5118069 0:d46cb1df87f1 83 S2=SENS2.read();
e5118069 0:d46cb1df87f1 84 encount=Enc.getPulses()-b_encount;//初期位置を合わせるために進んだ距離を引いている
e5118069 0:d46cb1df87f1 85
e5118069 0:d46cb1df87f1 86 if(cmd>20) cmd=20;//速度の最大値をcmd=20とする
e5118069 1:94e15665b69f 87 if(cmd<-15)cmd=-15;
e5118069 0:d46cb1df87f1 88
e5118069 0:d46cb1df87f1 89 int F=1,FF=0;//向き
e5118069 0:d46cb1df87f1 90
e5118069 1:94e15665b69f 91 float Ksp2 = 7.0, Ksd2 = 0.4; //モータ2用のパラメータ
e5118069 1:94e15665b69f 92 float Ksp3 = 7.0, Ksd3 = 0.4; //モータ3用のパラメータ
e5118069 0:d46cb1df87f1 93
e5118069 0:d46cb1df87f1 94 float ppr = 1.0;//射出の速度測定に使っているが要検討
e5118069 0:d46cb1df87f1 95
e5118069 0:d46cb1df87f1 96 static float pre_spd2 = 0.0;
e5118069 0:d46cb1df87f1 97 static float pre_spd3 = 0.0;
e5118069 0:d46cb1df87f1 98
e5118069 0:d46cb1df87f1 99 static float pre_err2 = 0.0;
e5118069 0:d46cb1df87f1 100 static float pre_err3 = 0.0;
e5118069 0:d46cb1df87f1 101
e5118069 0:d46cb1df87f1 102 static float ref_spd = 0.0;//目標速度
e5118069 0:d46cb1df87f1 103
e5118069 0:d46cb1df87f1 104 static int lim_cmd2 = 80;//cmdの上限
e5118069 0:d46cb1df87f1 105 static int lim_cmd3 = 92;//cmdの上限
e5118069 0:d46cb1df87f1 106
e5118069 1:94e15665b69f 107 int sw_point = Button();//スイッチの関数からリターン
e5118069 0:d46cb1df87f1 108
e5118069 0:d46cb1df87f1 109
e5118069 1:94e15665b69f 110 if(sw_point != 0) switch(mode){
e5118069 1:94e15665b69f 111 case 0:
e5118069 0:d46cb1df87f1 112 goal_D=0;
e5118069 1:94e15665b69f 113 if(sw_point==2)mode=1;
e5118069 0:d46cb1df87f1 114 break;
e5118069 0:d46cb1df87f1 115 case 1:
e5118069 0:d46cb1df87f1 116 cmd=-15;//向きが逆だからマイナス
e5118069 0:d46cb1df87f1 117 //goal_D=30; //リミットスイッチを押さずに止まるように使う
e5118069 1:94e15665b69f 118 if(sw_point==2)mode=2;
e5118069 0:d46cb1df87f1 119 if(LIM2==1){
e5118069 0:d46cb1df87f1 120 cmd=0;
e5118069 0:d46cb1df87f1 121 //これって目標値固定のままcmd＝0しちゃっていいのか ゴールを現在のangleにする？
e5118069 0:d46cb1df87f1 122 b_encount=Enc.getPulses();
e5118069 0:d46cb1df87f1 123 }
e5118069 0:d46cb1df87f1 124 break;
e5118069 0:d46cb1df87f1 125 case 2:
e5118069 0:d46cb1df87f1 126 goal_D=120;
e5118069 1:94e15665b69f 127 if(sw_point==2)mode=3;
e5118069 0:d46cb1df87f1 128 break;
e5118069 0:d46cb1df87f1 129
e5118069 0:d46cb1df87f1 130 case 3:
e5118069 1:94e15665b69f 131 if(sw_point==2)mode=4;
e5118069 0:d46cb1df87f1 132 if(S1==0&&S2==0){
e5118069 0:d46cb1df87f1 133 air=1;
e5118069 0:d46cb1df87f1 134 A=1;
e5118069 0:d46cb1df87f1 135 }
e5118069 0:d46cb1df87f1 136 break;
e5118069 0:d46cb1df87f1 137
e5118069 0:d46cb1df87f1 138 case 4:
e5118069 0:d46cb1df87f1 139 goal_D=0;
e5118069 1:94e15665b69f 140 if(sw_point==2){
e5118069 0:d46cb1df87f1 141 cmd=0;
e5118069 0:d46cb1df87f1 142 b_encount=Enc.getPulses();
e5118069 0:d46cb1df87f1 143 mode=5;
e5118069 0:d46cb1df87f1 144 }
e5118069 0:d46cb1df87f1 145 break;
e5118069 0:d46cb1df87f1 146 case 5:
e5118069 0:d46cb1df87f1 147 air=0;
e5118069 0:d46cb1df87f1 148 A=0;
e5118069 1:94e15665b69f 149 if(sw_point==2)mode=6;
e5118069 1:94e15665b69f 150 break;
e5118069 1:94e15665b69f 151
e5118069 1:94e15665b69f 152
e5118069 0:d46cb1df87f1 153 case(6)://速度を上げる
e5118069 0:d46cb1df87f1 154 ref_spd = 26.0;
e5118069 0:d46cb1df87f1 155 if (sw_point == 2) mode = 7;
e5118069 0:d46cb1df87f1 156 break;
e5118069 0:d46cb1df87f1 157
e5118069 0:d46cb1df87f1 158 case(7)://airでshagaiを発射位置まで押し上げる
e5118069 1:94e15665b69f 159 fet2 = 1;
e5118069 0:d46cb1df87f1 160 if (sw_point == 2) mode = 8;
e5118069 0:d46cb1df87f1 161 break;
e5118069 0:d46cb1df87f1 162
e5118069 0:d46cb1df87f1 163 case(8)://モータ停止と押し上げ機構の降下
e5118069 0:d46cb1df87f1 164 ref_spd = 0.0;
e5118069 1:94e15665b69f 165 fet2 = 0;
e5118069 0:d46cb1df87f1 166 if (sw_point == 2) mode = 0;
e5118069 0:d46cb1df87f1 167 break;
e5118069 0:d46cb1df87f1 168 }
e5118069 0:d46cb1df87f1 169
e5118069 1:94e15665b69f 170 if(cmd>=0) {
e5118069 1:94e15665b69f 171 Saber.putc(SABER_ADDR);
e5118069 1:94e15665b69f 172 Saber.putc(F); //逆回転では０を1にすればよい
e5118069 1:94e15665b69f 173 Saber.putc(abs(cmd));
e5118069 1:94e15665b69f 174 Saber.putc((SABER_ADDR + F + abs(cmd)) & 0b01111111); //ここの０も１にする
e5118069 1:94e15665b69f 175 } else {
e5118069 1:94e15665b69f 176 Saber.putc(SABER_ADDR);
e5118069 1:94e15665b69f 177 Saber.putc(FF); //逆回転では０を1にすればよい
e5118069 1:94e15665b69f 178 Saber.putc(abs(cmd));
e5118069 1:94e15665b69f 179 Saber.putc((SABER_ADDR + FF + abs(cmd)) & 0b01111111); //ここの０も１にする
e5118069 1:94e15665b69f 180 }
e5118069 1:94e15665b69f 181
e5118069 1:94e15665b69f 182 if(filtered_ref_spd>=25.5&&mode==6){
e5118069 0:d46cb1df87f1 183 filtered_ref_spd=26;
e5118069 1:94e15665b69f 184 }else if(filtered_ref_spd>=25.5&&mode==7){
e5118069 1:94e15665b69f 185 filtered_ref_spd=26;
e5118069 1:94e15665b69f 186 }else if(filtered_ref_spd<=0.5&&mode==8){
e5118069 0:d46cb1df87f1 187 filtered_ref_spd=0;
e5118069 1:94e15665b69f 188 }else if(mode==6||mode==7||mode==8){
e5118069 0:d46cb1df87f1 189 filtered_ref_spd = velfilter.SecondOrderLag((double)ref_spd);
e5118069 0:d46cb1df87f1 190 }
e5118069 1:94e15665b69f 191
e5118069 1:94e15665b69f 192 angle=(float)(encount)*(360.0/48.0)/4.0;
e5118069 1:94e15665b69f 193 SOKUDO=(angle-pre_angle)/INT_TIME;//角度を一回微分で角速度に
e5118069 1:94e15665b69f 194
e5118069 1:94e15665b69f 195 e_D=(goal_D-angle);//距離のPID制御の差
e5118069 1:94e15665b69f 196 ed_D=(e_D-pre_e_D)/INT_TIME;//距離のPID制御
e5118069 1:94e15665b69f 197 ei_D+=(e_D+pre_e_D)*INT_TIME/2.0;
e5118069 1:94e15665b69f 198
e5118069 1:94e15665b69f 199 cmd=(int)((e_D*Kp)+(ed_D*Kd)+(ei_D*Ki));
e5118069 1:94e15665b69f 200
e5118069 1:94e15665b69f 201
e5118069 1:94e15665b69f 202
e5118069 0:d46cb1df87f1 203 float encount2 = Enc2.getPulses();//[pulse]
e5118069 0:d46cb1df87f1 204 float encount3 = Enc3.getPulses();
e5118069 0:d46cb1df87f1 205
e5118069 1:94e15665b69f 206 float rot_sp2 = encount2/MULTIPLU/ppr; //[rev] // encount2/(resolution*4) //[rev]
e5118069 0:d46cb1df87f1 207 spd2 = (rot_sp2 - pre_spd2)/INT_TIME/(48*4); // (crr-prev)/INT_TIME //[rps]
e5118069 1:94e15665b69f 208 float rot_sp3 = encount3/MULTIPLU/ppr;
e5118069 0:d46cb1df87f1 209 spd3 = (rot_sp3 - pre_spd3)/INT_TIME/(48*4);
e5118069 0:d46cb1df87f1 210
e5118069 0:d46cb1df87f1 211 spd_err2 = filtered_ref_spd - spd2;//徐々に速度が上がるようにした
e5118069 0:d46cb1df87f1 212 float spd_d2 = (spd_err2 - pre_err2)/INT_TIME;
e5118069 1:94e15665b69f 213 tmp1 = (int)((spd_err2 * Ksp2) + (spd_d2 * Ksd2));
e5118069 1:94e15665b69f 214 if(tmp1>=127)tmp1=127;//加速度の制限
e5118069 1:94e15665b69f 215 if(tmp1<=-127)tmp1=-127;
e5118069 1:94e15665b69f 216 cmd2 += tmp1;
e5118069 0:d46cb1df87f1 217
e5118069 0:d46cb1df87f1 218 spd_err3 = filtered_ref_spd - spd3;
e5118069 0:d46cb1df87f1 219 float spd_d3 = (spd_err3 - pre_err3)/INT_TIME;
e5118069 1:94e15665b69f 220 tmp2 = (int)((spd_err3 * Ksp3) + (spd_d3 * Ksd3));
e5118069 1:94e15665b69f 221 if(tmp2>=127)tmp2=127;//加速度の制限
e5118069 1:94e15665b69f 222 if(tmp2<=-127)tmp2=-127;
e5118069 1:94e15665b69f 223 cmd3 += tmp2;
e5118069 0:d46cb1df87f1 224
e5118069 0:d46cb1df87f1 225 if (cmd2 > lim_cmd2) cmd2 = lim_cmd2;//上限指定
e5118069 0:d46cb1df87f1 226 if (cmd2 < -lim_cmd2) cmd2 = -lim_cmd2;
e5118069 0:d46cb1df87f1 227
e5118069 0:d46cb1df87f1 228 if (cmd3 > lim_cmd3) cmd3 = lim_cmd3;//上限指定
e5118069 0:d46cb1df87f1 229 if (cmd3 < -lim_cmd3) cmd3 = -lim_cmd3;
e5118069 0:d46cb1df87f1 230
e5118069 0:d46cb1df87f1 231 if (cmd2 > 0) {
e5118069 0:d46cb1df87f1 232 Saber.putc(SB_ADRS);
e5118069 0:d46cb1df87f1 233 Saber.putc(4);
e5118069 0:d46cb1df87f1 234 Saber.putc(cmd2);
e5118069 0:d46cb1df87f1 235 Saber.putc((SB_ADRS + 4 + cmd2) & 0b01111111);
e5118069 0:d46cb1df87f1 236 }
e5118069 0:d46cb1df87f1 237 else {
e5118069 0:d46cb1df87f1 238 Saber.putc(SB_ADRS);
e5118069 0:d46cb1df87f1 239 Saber.putc(5);
e5118069 0:d46cb1df87f1 240 Saber.putc(abs(cmd2));
e5118069 0:d46cb1df87f1 241 Saber.putc((SB_ADRS + 5 + abs(cmd2)) & 0b01111111);
e5118069 0:d46cb1df87f1 242 }
e5118069 0:d46cb1df87f1 243
e5118069 0:d46cb1df87f1 244 if (cmd3 > 0) {
e5118069 0:d46cb1df87f1 245 Saber.putc(SB_ADRS);
e5118069 0:d46cb1df87f1 246 Saber.putc(0);
e5118069 0:d46cb1df87f1 247 Saber.putc(cmd3);
e5118069 0:d46cb1df87f1 248 Saber.putc((SB_ADRS + 0 + cmd3) & 0b01111111);
e5118069 0:d46cb1df87f1 249 }
e5118069 0:d46cb1df87f1 250 else {
e5118069 0:d46cb1df87f1 251 Saber.putc(SB_ADRS);
e5118069 0:d46cb1df87f1 252 Saber.putc(1);
e5118069 0:d46cb1df87f1 253 Saber.putc(abs(cmd3));
e5118069 0:d46cb1df87f1 254 Saber.putc((SB_ADRS + 1 + abs(cmd3)) & 0b01111111);
e5118069 0:d46cb1df87f1 255 }
e5118069 0:d46cb1df87f1 256
e5118069 0:d46cb1df87f1 257 pre_spd2 = rot_sp2;
e5118069 0:d46cb1df87f1 258 pre_err2 = spd_err2;
e5118069 0:d46cb1df87f1 259 pre_spd3 = rot_sp3;
e5118069 0:d46cb1df87f1 260 pre_err3 = spd_err3;
e5118069 1:94e15665b69f 261
e5118069 1:94e15665b69f 262
e5118069 1:94e15665b69f 263 pre_e_D=e_D;//前回の速度の保存
e5118069 1:94e15665b69f 264 pre_angle=angle;
e5118069 1:94e15665b69f 265 pre_e_V=e_V;
e5118069 1:94e15665b69f 266 B_SA1=SA1;//前回のスイッチの値
e5118069 0:d46cb1df87f1 267 }
e5118069 0:d46cb1df87f1 268
e5118069 0:d46cb1df87f1 269
e5118069 0:d46cb1df87f1 270 int main() {
e5118069 1:94e15665b69f 271 Saber.baud(19200);
e5118069 0:d46cb1df87f1 272 pc.baud(9600);
e5118069 1:94e15665b69f 273
e5118069 1:94e15665b69f 274 velfilter.setSecondOrderPara(1.0, 0.9, 0.0);
e5118069 1:94e15665b69f 275
e5118069 0:d46cb1df87f1 276 timer.attach(timer_warikomi,INT_TIME);
e5118069 0:d46cb1df87f1 277
e5118069 0:d46cb1df87f1 278 while(1) {
e5118069 1:94e15665b69f 279 pc.printf("%d\n",mode);
e5118069 0:d46cb1df87f1 280 }
e5118069 1:94e15665b69f 281 }