project-R
TOUTEKI
Dependencies: mbed QEI2 UnderBody Filter
Diff: main.cpp
- Revision:
- 0:d46cb1df87f1
- Child:
- 1:94e15665b69f
--- /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