![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
TOUTEKI
Dependencies: mbed QEI2 UnderBody Filter
Diff: main.cpp
- Revision:
- 0:d46cb1df87f1
- Child:
- 1:94e15665b69f
diff -r 000000000000 -r d46cb1df87f1 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Jan 07 11:42:27 2019 +0000 @@ -0,0 +1,330 @@ +#include "mbed.h" +#include "QEI.h" +#define SB_ADRS_A 129 +#define SB_ADRS_B 132 + +#define INT_TIME 0.02 +#define RESOLUTION 48 + +#define ON 1 +#define OFF 0 + +#define HIGH 0 +#define FALL 1 +#define RISE 2 +#define LOW 3 + +#define GEAR_RATE 1.0 +#define PULL_RATE 1.0 +#define MULTIPLU 4.0 + +Ticker timer; +Timer T; + +QEI Enc1(p12,p11,NC,RESOLUTION,&T,QEI::X4_ENCODING); +QEI Enc2(p7,p8,NC,RESOLUTION,&T,QEI::X4_ENCODING);//B +QEI Enc3(p5,p6,NC,RESOLUTION,&T,QEI::X4_ENCODING);//B + +Filter velfilter(INT_TIME);//B + +DigitalIn sw1(p26);//こっち + +DigitalIn limit1(p15);//shagai把持 +DigitalIn limit2(p16);//初期位置合わせ + +DigitalIn sensor1(p18);//光電センサ +DigitalIn sensor2(p17); + +DigitalOut fet1(p21);//shagai把持の電磁弁 + +DigitalOut fet2(p22);//shagai押し出し用 + +Serial Saber(p13,p14); +Serial pc(USBTX,USBRX); + +int mode = 8;//スイッチを押したときのモード + +int cmd2 = 0; +int cmd3 = 0; + +float spd2=0; +float spd3=0; + +float spd_err2=0; +float spd_err3=0; + +int tmp2;//加速度の上限指定 +int tmp3; + +double filtered_ref_spd;//B + +int cmd,A;//Aはairの表示のためA +int SA1,B_SA1,LIM1,LIM2;//SA1はsA1の仮スイッチを入れる関数 B_SA1はbefore sA1の略 +int S1,S2;//光電センサ + +float angle,pre_angle,SOKUDO,e_D,pre_e_D,ed_D,ei_D,e_V,ed_V,pre_e_V,bcmd; +float goal_D=0,Kp=5,Ki=0.01,Kd=0.1; + +float encount,b_encount;//初期位置合わせの際に用いる初期位置合わせるための角度 + +int Button() { + int button_in = sw1.read(); + static int pre_button = 1; + static int sw_state = LOW; + + if(button_in && pre_button)sw_state = HIGH; + if(!button_in && !pre_button)sw_state = LOW; + if(button_in && !pre_button)sw_state = FALL; + if(!button_in && pre_button)sw_state = RISE; + + pre_button = button_in; + + return sw_state; +} + +void timer_warikomi() +{ + LIM1=!limit1.read();//pullupなので逆 + LIM2=!limit2.read();//pullupなので逆 + S1=SENS1.read();//光電センサ + S2=SENS2.read(); + encount=Enc.getPulses()-b_encount;//初期位置を合わせるために進んだ距離を引いている + + angle=(float)(encount)*G_rate*(360.0/48.0)/4.0; + SOKUDO=(angle-pre_angle)/INT_TIME;//角度を一回微分で角速度に + + e_D=(goal_D-angle);//距離のPID制御の差 + ed_D=(e_D-pre_e_D)/INT_TIME;//距離のPID制御 + ei_D+=(e_D+pre_e_D)*INT_TIME/2.0; + + cmd=(int)((e_D*Kp)+(ed_D*Kd)+(ei_D*Ki)); + + if(cmd>20) cmd=20;//速度の最大値をcmd=20とする + if(cmd<-15)cmd=-15; + + int F=1,FF=0;//向き + + float Ksp2 = 7.0, Ksd2 = 0.4; //モータ2用の速度パラメータ + float Ksp3 = 7.0, Ksd3 = 0.4; //モータ3用の速度パラメータ + + float ppr = 1.0;//射出の速度測定に使っているが要検討 + + static float pre_spd2 = 0.0; + static float pre_spd3 = 0.0; + + static float pre_err2 = 0.0; + static float pre_err3 = 0.0; + + static float ref_spd = 0.0;//目標速度 + + static int lim_cmd2 = 80;//cmdの上限 + static int lim_cmd3 = 92;//cmdの上限 + + int sw_point = Button();//スイッチの関数からリターン + + int sw_point = Button(); + + int encount2 = Enc2.getPulses(); + int encount3 = Enc3.getPulses(); + + encount_ang = Enc1.getPulses()- pre_encount; + if (encount2 > encount3) encount_rot = encount2; + else encount_rot = encount3; + + float angle = encount_ang * GEAR_RATE * (360.0/48.0) / 4.0; + float ang_spd =(angle - pre_angle)/INT_TIME; + + float rot_sp = (float)encount_rot/MULTIPLU/ppr*PULL_RATE; + float spd = (rot_sp - pre_spd)/INT_TIME(RESOLUTION*MULTIPLU); + + float angle_P = (ref_angle - angle); + float angle_D=(angle_P - pre_angleE)/INT_TIME; + angle_I += (angle_P + pre_angleE)*INT_TIME/2.0; + + cmd_ang = (int)(angle_P * Kp + angle_D * Kd + angle_I * Ki); + + float spd_e = ref_spd - spd; + float spd_D = (spd_e - pre_spdE)/INT_TIME; + cmd_spd += (int)(spd_e * Ksp + spd_D * Ksd); + + if (cmd_ang > lim_cmdA) cmd_ang = lim_cmdA; + if (-cmd_ang < -lim_cmdA) cmd_ang = -lim_cmdA; + + if (cmd_spd > lim_cmdS) cmd_spd = lim_cmdS; + if (-cmd_spd < -lim_cmdS) cmd_spd = -lim_cmdS; + + if (sw_point != HIGH) switch (mode) {//スイッチを押したモード + case 0: + goal_D=0; + if(sw_point == 2)mode=1; + break; + case 1: + cmd=-15;//向きが逆だからマイナス + //goal_D=30; //リミットスイッチを押さずに止まるように使う + if(sw_point == 2)mode=2; + if(LIM2==1){ + cmd=0; + //これって目標値固定のままcmd=0しちゃっていいのか ゴールを現在のangleにする? + b_encount=Enc.getPulses(); + } + break; + case 2: + goal_D=120; + if(sw_point == 2)mode=3; + break; + + case 3: + if(sw_point == 2)mode=4; + if(S1==0&&S2==0){ + air=1; + A=1; + } + break; + + case 4: + goal_D=0; + if(sw_point == 2){ + cmd=0; + b_encount=Enc.getPulses(); + mode=5; + } + break; + case 5: + air=0; + A=0; + if(sw_point == 2)mode=6; + break; + + case(6)://速度を上げる + ref_spd = 26.0; + if (sw_point == 2) mode = 7; + break; + + case(7)://airでshagaiを発射位置まで押し上げる + fet2 = ON; + if (sw_point == 2) mode = 8; + break; + + case(8)://モータ停止と押し上げ機構の降下 + ref_spd = 0.0; + fet2 = OFF; + if (sw_point == 2) mode = 0; + break; + } + + if(filtered_ref_spd>=25.5&&mode==0){//投てきの目標値上昇 + filtered_ref_spd=26; + }else if(filtered_ref_spd>=25.5&&mode==1){ + filtered_ref_spd=26; + }else if(filtered_ref_spd<=0.5&&mode==2){ + filtered_ref_spd=0; + }else{ + filtered_ref_spd = velfilter.SecondOrderLag((double)ref_spd); + } + + float encount2 = Enc2.getPulses();//[pulse] + float encount3 = Enc3.getPulses(); + + float rot_sp2 = encount2/MULTIPLU/ppr*PULL_RATE; //[rev] // encount2/(resolution*4) //[rev] + spd2 = (rot_sp2 - pre_spd2)/INT_TIME/(48*4); // (crr-prev)/INT_TIME //[rps] + float rot_sp3 = encount3/MULTIPLU/ppr*PULL_RATE; + spd3 = (rot_sp3 - pre_spd3)/INT_TIME/(48*4); + + spd_err2 = filtered_ref_spd - spd2;//徐々に速度が上がるようにした + float spd_d2 = (spd_err2 - pre_err2)/INT_TIME; + tmp2 = (int)((spd_err2 * Ksp2) + (spd_d2 * Ksd2)); + if(tmp2>=127)tmp2=127;//加速度の制限 + if(tmp2<=-127)tmp2=-127; + cmd2 += tmp2; + + spd_err3 = filtered_ref_spd - spd3; + float spd_d3 = (spd_err3 - pre_err3)/INT_TIME; + tmp3 = (int)((spd_err3 * Ksp3) + (spd_d3 * Ksd3)); + if(tmp3>=127)tmp3=127;//加速度の制限 + if(tmp3<=-127)tmp3=-127; + cmd3 += tmp3; + + if (cmd2 > lim_cmd2) cmd2 = lim_cmd2;//上限指定 + if (cmd2 < -lim_cmd2) cmd2 = -lim_cmd2; + + if (cmd3 > lim_cmd3) cmd3 = lim_cmd3;//上限指定 + if (cmd3 < -lim_cmd3) cmd3 = -lim_cmd3; + + if (cmd2 > 0) { + Saber.putc(SB_ADRS); + Saber.putc(4); + Saber.putc(cmd2); + Saber.putc((SB_ADRS + 4 + cmd2) & 0b01111111); + } + else { + Saber.putc(SB_ADRS); + Saber.putc(5); + Saber.putc(abs(cmd2)); + Saber.putc((SB_ADRS + 5 + abs(cmd2)) & 0b01111111); + } + + if (cmd3 > 0) { + Saber.putc(SB_ADRS); + Saber.putc(0); + Saber.putc(cmd3); + Saber.putc((SB_ADRS + 0 + cmd3) & 0b01111111); + } + else { + Saber.putc(SB_ADRS); + Saber.putc(1); + Saber.putc(abs(cmd3)); + Saber.putc((SB_ADRS + 1 + abs(cmd3)) & 0b01111111); + } + + pre_spd2 = rot_sp2; + pre_err2 = spd_err2; + pre_spd3 = rot_sp3; + pre_err3 = spd_err3; + } + + + if (!sw2.read()) { + cmd_spd = 0; + cmd_ang = 0; + } + + if (cmd_ang >= 0) { + Saber.putc(SB_ADRS_A); + Saber.putc(1); + Saber.putc(cmd_ang); + Saber.putc((SB_ADRS_A + 1 + cmd_ang) & 0b01111111); + } + else { + Saber.putc(SB_ADRS_A); + Saber.putc(0); + Saber.putc(abs(cmd_ang)); + Saber.putc((SB_ADRS_A + 0 + abs(cmd_ang)) & 0b01111111); + } + + if (cmd_spd >= 0) { + Saber.putc(SB_ADRS_B); + Saber.putc(1); + Saber.putc(cmd_spd); + Saber.putc((SB_ADRS_B + 1 + cmd_spd) & 0b01111111); + } + else { + Saber.putc(SB_ADRS_B); + Saber.putc(0); + Saber.putc(abs(cmd_spd)); + Saber.putc((SB_ADRS_B + 0 + abs(cmd_spd)) & 0b01111111); + } + + pre_spd = spd; + pre_spdE = spd_e; + pre_angle = angle; + pre_angleE = angle_P; +} + +int main() { + Saber.baud(115200); + pc.baud(9600); + timer.attach(timer_warikomi,INT_TIME); + + while(1) { + } +} \ No newline at end of file