project-R
TOUTEKI
Dependencies: mbed QEI2 UnderBody Filter
Diff: main.cpp
- Revision:
- 2:965cba546262
- Parent:
- 1:94e15665b69f
- Child:
- 3:de0b5dc55627
--- a/main.cpp Tue Jan 08 11:47:56 2019 +0000
+++ b/main.cpp Thu Jan 10 13:27:13 2019 +0000
@@ -15,33 +15,32 @@
Filter velfilter(INT_TIME);
-DigitalIn sw1(p26);//mode切替スイッチ
+DigitalIn sw1(p26);
-DigitalOut fet2(p21);//shagai押し出し用のair
+DigitalOut fet2(p21);
-QEI Enc(p12,p11,NC,48,&T,QEI::X4_ENCODING);//200は分解能
+QEI Enc(p12,p11,NC,48,&T,QEI::X4_ENCODING);
Serial Saber(p13,p14);
Serial pc(USBTX,USBRX);
-DigitalIn limit1(p15);//hagai把持
-DigitalIn limit2(p16);//初期位置合わせのlimitスイッチ
+DigitalIn limit1(p15);
+DigitalIn limit2(p16);
-DigitalOut air(p22);//電磁弁
+DigitalOut air(p22);
-DigitalIn SENS1(p18);//光電センサ
+DigitalIn SENS1(p18);
DigitalIn SENS2(p17);
-int cmd,A;//Aはairの表示のため
-int SA1,B_SA1,LIM1,LIM2;//SA1はsA1の仮スイッチを入れる関数 B_SA1はbefore sA1の略
-int S1,S2;//光電センサ
+int cmd,A;
+int SA1,B_SA1,LIM1,LIM2;
+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;//初期位置合わせの際に用いる初期位置合わせるための角度
+float encount,b_encount;
-
-int mode = 8;//スイッチを押したときのモード
+int mode = 8;
int cmd2 = 0;
int cmd3 = 0;
@@ -57,7 +56,7 @@
double filtered_ref_spd;
-int Button() {//スイッチのノイズをとる関数
+int Button() {
int button_in = sw1.read();
@@ -77,21 +76,17 @@
void timer_warikomi()
{
- LIM1=!limit1.read();//pullupなので逆
- LIM2=!limit2.read();//pullupなので逆
- S1=SENS1.read();//光電センサ
+ LIM1=!limit1.read();
+ LIM2=!limit2.read();
+ S1=SENS1.read();
S2=SENS2.read();
- encount=Enc.getPulses()-b_encount;//初期位置を合わせるために進んだ距離を引いている
+ encount=Enc.getPulses()-b_encount;
+
- if(cmd>20) cmd=20;//速度の最大値をcmd=20とする
- if(cmd<-15)cmd=-15;
-
- int F=1,FF=0;//向き
+ float Ksp2 = 7.0, Ksd2 = 0.4;
+ float Ksp3 = 7.0, Ksd3 = 0.4;
- float Ksp2 = 7.0, Ksd2 = 0.4; //モータ2用のパラメータ
- float Ksp3 = 7.0, Ksd3 = 0.4; //モータ3用のパラメータ
-
- float ppr = 1.0;//射出の速度測定に使っているが要検討
+ float ppr = 1.0;
static float pre_spd2 = 0.0;
static float pre_spd3 = 0.0;
@@ -99,32 +94,80 @@
static float pre_err2 = 0.0;
static float pre_err3 = 0.0;
- static float ref_spd = 0.0;//目標速度
+ static float ref_spd = 0.0;
+
+ static int lim_cmd2 = 85;
+ static int lim_cmd3 = 92;
+
+ int sw_point = Button();
- static int lim_cmd2 = 80;//cmdの上限
- static int lim_cmd3 = 92;//cmdの上限
+
+
+ if(filtered_ref_spd>=25.5&&mode==6){
+ filtered_ref_spd=26;
+ }else if(filtered_ref_spd>=25.5&&mode==7){
+ filtered_ref_spd=26;
+ }else if(filtered_ref_spd<=5.0&&mode==8){
+ filtered_ref_spd=0;
+ }else if(mode==6||mode==7||mode==8){
+ filtered_ref_spd = velfilter.SecondOrderLag((double)ref_spd);
+ }
+
+ angle=(float)(encount)*(360.0/48.0)/4.0;
+ SOKUDO=(angle-pre_angle)/INT_TIME;
+
+ e_D=(goal_D-angle);
+ ed_D=(e_D-pre_e_D)/INT_TIME;
+ ei_D+=(e_D+pre_e_D)*INT_TIME/2.0;
+
+ cmd=(int)((e_D*Kp)+(ed_D*Kd)+(ei_D*Ki));
+
- int sw_point = Button();//スイッチの関数からリターン
+
+ float encount2 = Enc2.getPulses();
+ float encount3 = Enc3.getPulses();
+
+ float rot_sp2 = encount2/MULTIPLU/ppr;
+ spd2 = (rot_sp2 - pre_spd2)/INT_TIME/(48*4);
+ float rot_sp3 = encount3/MULTIPLU/ppr;
+ spd3 = (rot_sp3 - pre_spd3)/INT_TIME/(48*4);
+ spd_err2 = filtered_ref_spd - spd2;
+ float spd_d2 = (spd_err2 - pre_err2)/INT_TIME;
+ tmp1 = (int)((spd_err2 * Ksp2) + (spd_d2 * Ksd2));
+ if(tmp1>=127)tmp1=127;
+ if(tmp1<=-127)tmp1=-127;
+ cmd2 += tmp1;
- if(sw_point != 0) switch(mode){
+ spd_err3 = filtered_ref_spd - spd3;
+ float spd_d3 = (spd_err3 - pre_err3)/INT_TIME;
+ tmp2 = (int)((spd_err3 * Ksp3) + (spd_d3 * Ksd3));
+ if(tmp2>=127)tmp2=127;
+ if(tmp2<=-127)tmp2=-127;
+ cmd3 += tmp2;
+
+ 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(sw_point != 0) switch(mode){
case 0:
goal_D=0;
if(sw_point==2)mode=1;
break;
case 1:
- cmd=-15;//向きが逆だからマイナス
- //goal_D=30; //リミットスイッチを押さずに止まるように使う
+ cmd=-15;
if(sw_point==2)mode=2;
if(LIM2==1){
cmd=0;
- //これって目標値固定のままcmd＝0しちゃっていいのか ゴールを現在のangleにする？
b_encount=Enc.getPulses();
}
break;
case 2:
- goal_D=120;
+ goal_D=125;
if(sw_point==2)mode=3;
+ if(angle>=120)cmd=0;
break;
case 3:
@@ -150,83 +193,40 @@
break;
- case(6)://速度を上げる
+ case(6):
ref_spd = 26.0;
if (sw_point == 2) mode = 7;
break;
- case(7)://airでshagaiを発射位置まで押し上げる
- fet2 = 1;
+ case(7):
+ fet2 = 0;
if (sw_point == 2) mode = 8;
break;
- case(8)://モータ停止と押し上げ機構の降下
+ case(8):
ref_spd = 0.0;
- fet2 = 0;
+ fet2 = 1;
if (sw_point == 2) mode = 0;
+ if (spd3<=0.5) cmd = 0;
break;
}
-
- if(cmd>=0) {
- Saber.putc(SABER_ADDR);
- Saber.putc(F); //逆回転では０を1にすればよい
- Saber.putc(abs(cmd));
- Saber.putc((SABER_ADDR + F + abs(cmd)) & 0b01111111); //ここの０も１にする
- } else {
- Saber.putc(SABER_ADDR);
- Saber.putc(FF); //逆回転では０を1にすればよい
- Saber.putc(abs(cmd));
- Saber.putc((SABER_ADDR + FF + abs(cmd)) & 0b01111111); //ここの０も１にする
- }
-
- if(filtered_ref_spd>=25.5&&mode==6){
- filtered_ref_spd=26;
- }else if(filtered_ref_spd>=25.5&&mode==7){
- filtered_ref_spd=26;
- }else if(filtered_ref_spd<=0.5&&mode==8){
- filtered_ref_spd=0;
- }else if(mode==6||mode==7||mode==8){
- filtered_ref_spd = velfilter.SecondOrderLag((double)ref_spd);
- }
- angle=(float)(encount)*(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;
+ if(cmd>20) cmd=20;
+ if(cmd<-15)cmd=-15;
- cmd=(int)((e_D*Kp)+(ed_D*Kd)+(ei_D*Ki));
-
-
-
- float encount2 = Enc2.getPulses();//[pulse]
- float encount3 = Enc3.getPulses();
-
- float rot_sp2 = encount2/MULTIPLU/ppr; //[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;
- spd3 = (rot_sp3 - pre_spd3)/INT_TIME/(48*4);
+ int F=1,FF=0;//向き
- spd_err2 = filtered_ref_spd - spd2;//徐々に速度が上がるようにした
- float spd_d2 = (spd_err2 - pre_err2)/INT_TIME;
- tmp1 = (int)((spd_err2 * Ksp2) + (spd_d2 * Ksd2));
- if(tmp1>=127)tmp1=127;//加速度の制限
- if(tmp1<=-127)tmp1=-127;
- cmd2 += tmp1;
-
- spd_err3 = filtered_ref_spd - spd3;
- float spd_d3 = (spd_err3 - pre_err3)/INT_TIME;
- tmp2 = (int)((spd_err3 * Ksp3) + (spd_d3 * Ksd3));
- if(tmp2>=127)tmp2=127;//加速度の制限
- if(tmp2<=-127)tmp2=-127;
- cmd3 += tmp2;
-
- 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(cmd>=0) {
+ Saber.putc(SABER_ADDR);
+ Saber.putc(F);
+ Saber.putc(abs(cmd));
+ Saber.putc((SABER_ADDR + F + abs(cmd)) & 0b01111111);
+ } else {
+ Saber.putc(SABER_ADDR);
+ Saber.putc(FF);
+ Saber.putc(abs(cmd));
+ Saber.putc((SABER_ADDR + FF + abs(cmd)) & 0b01111111);
+ }
if (cmd2 > 0) {
Saber.putc(SB_ADRS);
@@ -260,10 +260,10 @@
pre_err3 = spd_err3;
- pre_e_D=e_D;//前回の速度の保存
+ pre_e_D=e_D;
pre_angle=angle;
pre_e_V=e_V;
- B_SA1=SA1;//前回のスイッチの値
+ B_SA1=SA1;
}
@@ -271,11 +271,16 @@
Saber.baud(19200);
pc.baud(9600);
+ fet2=1;
+ air=0;
+
+
velfilter.setSecondOrderPara(1.0, 0.9, 0.0);
timer.attach(timer_warikomi,INT_TIME);
while(1) {
- pc.printf("%d\n",mode);
+ //pc.printf("%d\n",mode);
+ pc.printf("spd2 %f\t spd3 %f\n",spd2,spd3);
}
}