TOUTEKI
Dependencies: mbed QEI2 UnderBody Filter
main.cpp@0:d46cb1df87f1, 2019-01-07 (annotated)
- Committer:
- e5118069
- Date:
- Mon Jan 07 11:42:27 2019 +0000
- Revision:
- 0:d46cb1df87f1
- Child:
- 1:94e15665b69f
201917 not yet;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
e5118069 | 0:d46cb1df87f1 | 1 | #include "mbed.h" |
e5118069 | 0:d46cb1df87f1 | 2 | #include "QEI.h" |
e5118069 | 0:d46cb1df87f1 | 3 | #define SB_ADRS_A 129 |
e5118069 | 0:d46cb1df87f1 | 4 | #define SB_ADRS_B 132 |
e5118069 | 0:d46cb1df87f1 | 5 | |
e5118069 | 0:d46cb1df87f1 | 6 | #define INT_TIME 0.02 |
e5118069 | 0:d46cb1df87f1 | 7 | #define RESOLUTION 48 |
e5118069 | 0:d46cb1df87f1 | 8 | |
e5118069 | 0:d46cb1df87f1 | 9 | #define ON 1 |
e5118069 | 0:d46cb1df87f1 | 10 | #define OFF 0 |
e5118069 | 0:d46cb1df87f1 | 11 | |
e5118069 | 0:d46cb1df87f1 | 12 | #define HIGH 0 |
e5118069 | 0:d46cb1df87f1 | 13 | #define FALL 1 |
e5118069 | 0:d46cb1df87f1 | 14 | #define RISE 2 |
e5118069 | 0:d46cb1df87f1 | 15 | #define LOW 3 |
e5118069 | 0:d46cb1df87f1 | 16 | |
e5118069 | 0:d46cb1df87f1 | 17 | #define GEAR_RATE 1.0 |
e5118069 | 0:d46cb1df87f1 | 18 | #define PULL_RATE 1.0 |
e5118069 | 0:d46cb1df87f1 | 19 | #define MULTIPLU 4.0 |
e5118069 | 0:d46cb1df87f1 | 20 | |
e5118069 | 0:d46cb1df87f1 | 21 | Ticker timer; |
e5118069 | 0:d46cb1df87f1 | 22 | Timer T; |
e5118069 | 0:d46cb1df87f1 | 23 | |
e5118069 | 0:d46cb1df87f1 | 24 | QEI Enc1(p12,p11,NC,RESOLUTION,&T,QEI::X4_ENCODING); |
e5118069 | 0:d46cb1df87f1 | 25 | QEI Enc2(p7,p8,NC,RESOLUTION,&T,QEI::X4_ENCODING);//B |
e5118069 | 0:d46cb1df87f1 | 26 | QEI Enc3(p5,p6,NC,RESOLUTION,&T,QEI::X4_ENCODING);//B |
e5118069 | 0:d46cb1df87f1 | 27 | |
e5118069 | 0:d46cb1df87f1 | 28 | Filter velfilter(INT_TIME);//B |
e5118069 | 0:d46cb1df87f1 | 29 | |
e5118069 | 0:d46cb1df87f1 | 30 | DigitalIn sw1(p26);//こっち |
e5118069 | 0:d46cb1df87f1 | 31 | |
e5118069 | 0:d46cb1df87f1 | 32 | DigitalIn limit1(p15);//shagai把持 |
e5118069 | 0:d46cb1df87f1 | 33 | DigitalIn limit2(p16);//初期位置合わせ |
e5118069 | 0:d46cb1df87f1 | 34 | |
e5118069 | 0:d46cb1df87f1 | 35 | DigitalIn sensor1(p18);//光電センサ |
e5118069 | 0:d46cb1df87f1 | 36 | DigitalIn sensor2(p17); |
e5118069 | 0:d46cb1df87f1 | 37 | |
e5118069 | 0:d46cb1df87f1 | 38 | DigitalOut fet1(p21);//shagai把持の電磁弁 |
e5118069 | 0:d46cb1df87f1 | 39 | |
e5118069 | 0:d46cb1df87f1 | 40 | DigitalOut fet2(p22);//shagai押し出し用 |
e5118069 | 0:d46cb1df87f1 | 41 | |
e5118069 | 0:d46cb1df87f1 | 42 | Serial Saber(p13,p14); |
e5118069 | 0:d46cb1df87f1 | 43 | Serial pc(USBTX,USBRX); |
e5118069 | 0:d46cb1df87f1 | 44 | |
e5118069 | 0:d46cb1df87f1 | 45 | int mode = 8;//スイッチを押したときのモード |
e5118069 | 0:d46cb1df87f1 | 46 | |
e5118069 | 0:d46cb1df87f1 | 47 | int cmd2 = 0; |
e5118069 | 0:d46cb1df87f1 | 48 | int cmd3 = 0; |
e5118069 | 0:d46cb1df87f1 | 49 | |
e5118069 | 0:d46cb1df87f1 | 50 | float spd2=0; |
e5118069 | 0:d46cb1df87f1 | 51 | float spd3=0; |
e5118069 | 0:d46cb1df87f1 | 52 | |
e5118069 | 0:d46cb1df87f1 | 53 | float spd_err2=0; |
e5118069 | 0:d46cb1df87f1 | 54 | float spd_err3=0; |
e5118069 | 0:d46cb1df87f1 | 55 | |
e5118069 | 0:d46cb1df87f1 | 56 | int tmp2;//加速度の上限指定 |
e5118069 | 0:d46cb1df87f1 | 57 | int tmp3; |
e5118069 | 0:d46cb1df87f1 | 58 | |
e5118069 | 0:d46cb1df87f1 | 59 | double filtered_ref_spd;//B |
e5118069 | 0:d46cb1df87f1 | 60 | |
e5118069 | 0:d46cb1df87f1 | 61 | int cmd,A;//Aはairの表示のためA |
e5118069 | 0:d46cb1df87f1 | 62 | int SA1,B_SA1,LIM1,LIM2;//SA1はsA1の仮スイッチを入れる関数 B_SA1はbefore sA1の略 |
e5118069 | 0:d46cb1df87f1 | 63 | int S1,S2;//光電センサ |
e5118069 | 0:d46cb1df87f1 | 64 | |
e5118069 | 0:d46cb1df87f1 | 65 | float angle,pre_angle,SOKUDO,e_D,pre_e_D,ed_D,ei_D,e_V,ed_V,pre_e_V,bcmd; |
e5118069 | 0:d46cb1df87f1 | 66 | float goal_D=0,Kp=5,Ki=0.01,Kd=0.1; |
e5118069 | 0:d46cb1df87f1 | 67 | |
e5118069 | 0:d46cb1df87f1 | 68 | float encount,b_encount;//初期位置合わせの際に用いる初期位置合わせるための角度 |
e5118069 | 0:d46cb1df87f1 | 69 | |
e5118069 | 0:d46cb1df87f1 | 70 | int Button() { |
e5118069 | 0:d46cb1df87f1 | 71 | int button_in = sw1.read(); |
e5118069 | 0:d46cb1df87f1 | 72 | static int pre_button = 1; |
e5118069 | 0:d46cb1df87f1 | 73 | static int sw_state = LOW; |
e5118069 | 0:d46cb1df87f1 | 74 | |
e5118069 | 0:d46cb1df87f1 | 75 | if(button_in && pre_button)sw_state = HIGH; |
e5118069 | 0:d46cb1df87f1 | 76 | if(!button_in && !pre_button)sw_state = LOW; |
e5118069 | 0:d46cb1df87f1 | 77 | if(button_in && !pre_button)sw_state = FALL; |
e5118069 | 0:d46cb1df87f1 | 78 | if(!button_in && pre_button)sw_state = RISE; |
e5118069 | 0:d46cb1df87f1 | 79 | |
e5118069 | 0:d46cb1df87f1 | 80 | pre_button = button_in; |
e5118069 | 0:d46cb1df87f1 | 81 | |
e5118069 | 0:d46cb1df87f1 | 82 | return sw_state; |
e5118069 | 0:d46cb1df87f1 | 83 | } |
e5118069 | 0:d46cb1df87f1 | 84 | |
e5118069 | 0:d46cb1df87f1 | 85 | void timer_warikomi() |
e5118069 | 0:d46cb1df87f1 | 86 | { |
e5118069 | 0:d46cb1df87f1 | 87 | LIM1=!limit1.read();//pullupなので逆 |
e5118069 | 0:d46cb1df87f1 | 88 | LIM2=!limit2.read();//pullupなので逆 |
e5118069 | 0:d46cb1df87f1 | 89 | S1=SENS1.read();//光電センサ |
e5118069 | 0:d46cb1df87f1 | 90 | S2=SENS2.read(); |
e5118069 | 0:d46cb1df87f1 | 91 | encount=Enc.getPulses()-b_encount;//初期位置を合わせるために進んだ距離を引いている |
e5118069 | 0:d46cb1df87f1 | 92 | |
e5118069 | 0:d46cb1df87f1 | 93 | angle=(float)(encount)*G_rate*(360.0/48.0)/4.0; |
e5118069 | 0:d46cb1df87f1 | 94 | SOKUDO=(angle-pre_angle)/INT_TIME;//角度を一回微分で角速度に |
e5118069 | 0:d46cb1df87f1 | 95 | |
e5118069 | 0:d46cb1df87f1 | 96 | e_D=(goal_D-angle);//距離のPID制御の差 |
e5118069 | 0:d46cb1df87f1 | 97 | ed_D=(e_D-pre_e_D)/INT_TIME;//距離のPID制御 |
e5118069 | 0:d46cb1df87f1 | 98 | ei_D+=(e_D+pre_e_D)*INT_TIME/2.0; |
e5118069 | 0:d46cb1df87f1 | 99 | |
e5118069 | 0:d46cb1df87f1 | 100 | cmd=(int)((e_D*Kp)+(ed_D*Kd)+(ei_D*Ki)); |
e5118069 | 0:d46cb1df87f1 | 101 | |
e5118069 | 0:d46cb1df87f1 | 102 | if(cmd>20) cmd=20;//速度の最大値をcmd=20とする |
e5118069 | 0:d46cb1df87f1 | 103 | if(cmd<-15)cmd=-15; |
e5118069 | 0:d46cb1df87f1 | 104 | |
e5118069 | 0:d46cb1df87f1 | 105 | int F=1,FF=0;//向き |
e5118069 | 0:d46cb1df87f1 | 106 | |
e5118069 | 0:d46cb1df87f1 | 107 | float Ksp2 = 7.0, Ksd2 = 0.4; //モータ2用の速度パラメータ |
e5118069 | 0:d46cb1df87f1 | 108 | float Ksp3 = 7.0, Ksd3 = 0.4; //モータ3用の速度パラメータ |
e5118069 | 0:d46cb1df87f1 | 109 | |
e5118069 | 0:d46cb1df87f1 | 110 | float ppr = 1.0;//射出の速度測定に使っているが要検討 |
e5118069 | 0:d46cb1df87f1 | 111 | |
e5118069 | 0:d46cb1df87f1 | 112 | static float pre_spd2 = 0.0; |
e5118069 | 0:d46cb1df87f1 | 113 | static float pre_spd3 = 0.0; |
e5118069 | 0:d46cb1df87f1 | 114 | |
e5118069 | 0:d46cb1df87f1 | 115 | static float pre_err2 = 0.0; |
e5118069 | 0:d46cb1df87f1 | 116 | static float pre_err3 = 0.0; |
e5118069 | 0:d46cb1df87f1 | 117 | |
e5118069 | 0:d46cb1df87f1 | 118 | static float ref_spd = 0.0;//目標速度 |
e5118069 | 0:d46cb1df87f1 | 119 | |
e5118069 | 0:d46cb1df87f1 | 120 | static int lim_cmd2 = 80;//cmdの上限 |
e5118069 | 0:d46cb1df87f1 | 121 | static int lim_cmd3 = 92;//cmdの上限 |
e5118069 | 0:d46cb1df87f1 | 122 | |
e5118069 | 0:d46cb1df87f1 | 123 | int sw_point = Button();//スイッチの関数からリターン |
e5118069 | 0:d46cb1df87f1 | 124 | |
e5118069 | 0:d46cb1df87f1 | 125 | int sw_point = Button(); |
e5118069 | 0:d46cb1df87f1 | 126 | |
e5118069 | 0:d46cb1df87f1 | 127 | int encount2 = Enc2.getPulses(); |
e5118069 | 0:d46cb1df87f1 | 128 | int encount3 = Enc3.getPulses(); |
e5118069 | 0:d46cb1df87f1 | 129 | |
e5118069 | 0:d46cb1df87f1 | 130 | encount_ang = Enc1.getPulses()- pre_encount; |
e5118069 | 0:d46cb1df87f1 | 131 | if (encount2 > encount3) encount_rot = encount2; |
e5118069 | 0:d46cb1df87f1 | 132 | else encount_rot = encount3; |
e5118069 | 0:d46cb1df87f1 | 133 | |
e5118069 | 0:d46cb1df87f1 | 134 | float angle = encount_ang * GEAR_RATE * (360.0/48.0) / 4.0; |
e5118069 | 0:d46cb1df87f1 | 135 | float ang_spd =(angle - pre_angle)/INT_TIME; |
e5118069 | 0:d46cb1df87f1 | 136 | |
e5118069 | 0:d46cb1df87f1 | 137 | float rot_sp = (float)encount_rot/MULTIPLU/ppr*PULL_RATE; |
e5118069 | 0:d46cb1df87f1 | 138 | float spd = (rot_sp - pre_spd)/INT_TIME(RESOLUTION*MULTIPLU); |
e5118069 | 0:d46cb1df87f1 | 139 | |
e5118069 | 0:d46cb1df87f1 | 140 | float angle_P = (ref_angle - angle); |
e5118069 | 0:d46cb1df87f1 | 141 | float angle_D=(angle_P - pre_angleE)/INT_TIME; |
e5118069 | 0:d46cb1df87f1 | 142 | angle_I += (angle_P + pre_angleE)*INT_TIME/2.0; |
e5118069 | 0:d46cb1df87f1 | 143 | |
e5118069 | 0:d46cb1df87f1 | 144 | cmd_ang = (int)(angle_P * Kp + angle_D * Kd + angle_I * Ki); |
e5118069 | 0:d46cb1df87f1 | 145 | |
e5118069 | 0:d46cb1df87f1 | 146 | float spd_e = ref_spd - spd; |
e5118069 | 0:d46cb1df87f1 | 147 | float spd_D = (spd_e - pre_spdE)/INT_TIME; |
e5118069 | 0:d46cb1df87f1 | 148 | cmd_spd += (int)(spd_e * Ksp + spd_D * Ksd); |
e5118069 | 0:d46cb1df87f1 | 149 | |
e5118069 | 0:d46cb1df87f1 | 150 | if (cmd_ang > lim_cmdA) cmd_ang = lim_cmdA; |
e5118069 | 0:d46cb1df87f1 | 151 | if (-cmd_ang < -lim_cmdA) cmd_ang = -lim_cmdA; |
e5118069 | 0:d46cb1df87f1 | 152 | |
e5118069 | 0:d46cb1df87f1 | 153 | if (cmd_spd > lim_cmdS) cmd_spd = lim_cmdS; |
e5118069 | 0:d46cb1df87f1 | 154 | if (-cmd_spd < -lim_cmdS) cmd_spd = -lim_cmdS; |
e5118069 | 0:d46cb1df87f1 | 155 | |
e5118069 | 0:d46cb1df87f1 | 156 | if (sw_point != HIGH) switch (mode) {//スイッチを押したモード |
e5118069 | 0:d46cb1df87f1 | 157 | case 0: |
e5118069 | 0:d46cb1df87f1 | 158 | goal_D=0; |
e5118069 | 0:d46cb1df87f1 | 159 | if(sw_point == 2)mode=1; |
e5118069 | 0:d46cb1df87f1 | 160 | break; |
e5118069 | 0:d46cb1df87f1 | 161 | case 1: |
e5118069 | 0:d46cb1df87f1 | 162 | cmd=-15;//向きが逆だからマイナス |
e5118069 | 0:d46cb1df87f1 | 163 | //goal_D=30; //リミットスイッチを押さずに止まるように使う |
e5118069 | 0:d46cb1df87f1 | 164 | if(sw_point == 2)mode=2; |
e5118069 | 0:d46cb1df87f1 | 165 | if(LIM2==1){ |
e5118069 | 0:d46cb1df87f1 | 166 | cmd=0; |
e5118069 | 0:d46cb1df87f1 | 167 | //これって目標値固定のままcmd=0しちゃっていいのか ゴールを現在のangleにする? |
e5118069 | 0:d46cb1df87f1 | 168 | b_encount=Enc.getPulses(); |
e5118069 | 0:d46cb1df87f1 | 169 | } |
e5118069 | 0:d46cb1df87f1 | 170 | break; |
e5118069 | 0:d46cb1df87f1 | 171 | case 2: |
e5118069 | 0:d46cb1df87f1 | 172 | goal_D=120; |
e5118069 | 0:d46cb1df87f1 | 173 | if(sw_point == 2)mode=3; |
e5118069 | 0:d46cb1df87f1 | 174 | break; |
e5118069 | 0:d46cb1df87f1 | 175 | |
e5118069 | 0:d46cb1df87f1 | 176 | case 3: |
e5118069 | 0:d46cb1df87f1 | 177 | if(sw_point == 2)mode=4; |
e5118069 | 0:d46cb1df87f1 | 178 | if(S1==0&&S2==0){ |
e5118069 | 0:d46cb1df87f1 | 179 | air=1; |
e5118069 | 0:d46cb1df87f1 | 180 | A=1; |
e5118069 | 0:d46cb1df87f1 | 181 | } |
e5118069 | 0:d46cb1df87f1 | 182 | break; |
e5118069 | 0:d46cb1df87f1 | 183 | |
e5118069 | 0:d46cb1df87f1 | 184 | case 4: |
e5118069 | 0:d46cb1df87f1 | 185 | goal_D=0; |
e5118069 | 0:d46cb1df87f1 | 186 | if(sw_point == 2){ |
e5118069 | 0:d46cb1df87f1 | 187 | cmd=0; |
e5118069 | 0:d46cb1df87f1 | 188 | b_encount=Enc.getPulses(); |
e5118069 | 0:d46cb1df87f1 | 189 | mode=5; |
e5118069 | 0:d46cb1df87f1 | 190 | } |
e5118069 | 0:d46cb1df87f1 | 191 | break; |
e5118069 | 0:d46cb1df87f1 | 192 | case 5: |
e5118069 | 0:d46cb1df87f1 | 193 | air=0; |
e5118069 | 0:d46cb1df87f1 | 194 | A=0; |
e5118069 | 0:d46cb1df87f1 | 195 | if(sw_point == 2)mode=6; |
e5118069 | 0:d46cb1df87f1 | 196 | break; |
e5118069 | 0:d46cb1df87f1 | 197 | |
e5118069 | 0:d46cb1df87f1 | 198 | case(6)://速度を上げる |
e5118069 | 0:d46cb1df87f1 | 199 | ref_spd = 26.0; |
e5118069 | 0:d46cb1df87f1 | 200 | if (sw_point == 2) mode = 7; |
e5118069 | 0:d46cb1df87f1 | 201 | break; |
e5118069 | 0:d46cb1df87f1 | 202 | |
e5118069 | 0:d46cb1df87f1 | 203 | case(7)://airでshagaiを発射位置まで押し上げる |
e5118069 | 0:d46cb1df87f1 | 204 | fet2 = ON; |
e5118069 | 0:d46cb1df87f1 | 205 | if (sw_point == 2) mode = 8; |
e5118069 | 0:d46cb1df87f1 | 206 | break; |
e5118069 | 0:d46cb1df87f1 | 207 | |
e5118069 | 0:d46cb1df87f1 | 208 | case(8)://モータ停止と押し上げ機構の降下 |
e5118069 | 0:d46cb1df87f1 | 209 | ref_spd = 0.0; |
e5118069 | 0:d46cb1df87f1 | 210 | fet2 = OFF; |
e5118069 | 0:d46cb1df87f1 | 211 | if (sw_point == 2) mode = 0; |
e5118069 | 0:d46cb1df87f1 | 212 | break; |
e5118069 | 0:d46cb1df87f1 | 213 | } |
e5118069 | 0:d46cb1df87f1 | 214 | |
e5118069 | 0:d46cb1df87f1 | 215 | if(filtered_ref_spd>=25.5&&mode==0){//投てきの目標値上昇 |
e5118069 | 0:d46cb1df87f1 | 216 | filtered_ref_spd=26; |
e5118069 | 0:d46cb1df87f1 | 217 | }else if(filtered_ref_spd>=25.5&&mode==1){ |
e5118069 | 0:d46cb1df87f1 | 218 | filtered_ref_spd=26; |
e5118069 | 0:d46cb1df87f1 | 219 | }else if(filtered_ref_spd<=0.5&&mode==2){ |
e5118069 | 0:d46cb1df87f1 | 220 | filtered_ref_spd=0; |
e5118069 | 0:d46cb1df87f1 | 221 | }else{ |
e5118069 | 0:d46cb1df87f1 | 222 | filtered_ref_spd = velfilter.SecondOrderLag((double)ref_spd); |
e5118069 | 0:d46cb1df87f1 | 223 | } |
e5118069 | 0:d46cb1df87f1 | 224 | |
e5118069 | 0:d46cb1df87f1 | 225 | float encount2 = Enc2.getPulses();//[pulse] |
e5118069 | 0:d46cb1df87f1 | 226 | float encount3 = Enc3.getPulses(); |
e5118069 | 0:d46cb1df87f1 | 227 | |
e5118069 | 0:d46cb1df87f1 | 228 | float rot_sp2 = encount2/MULTIPLU/ppr*PULL_RATE; //[rev] // encount2/(resolution*4) //[rev] |
e5118069 | 0:d46cb1df87f1 | 229 | spd2 = (rot_sp2 - pre_spd2)/INT_TIME/(48*4); // (crr-prev)/INT_TIME //[rps] |
e5118069 | 0:d46cb1df87f1 | 230 | float rot_sp3 = encount3/MULTIPLU/ppr*PULL_RATE; |
e5118069 | 0:d46cb1df87f1 | 231 | spd3 = (rot_sp3 - pre_spd3)/INT_TIME/(48*4); |
e5118069 | 0:d46cb1df87f1 | 232 | |
e5118069 | 0:d46cb1df87f1 | 233 | spd_err2 = filtered_ref_spd - spd2;//徐々に速度が上がるようにした |
e5118069 | 0:d46cb1df87f1 | 234 | float spd_d2 = (spd_err2 - pre_err2)/INT_TIME; |
e5118069 | 0:d46cb1df87f1 | 235 | tmp2 = (int)((spd_err2 * Ksp2) + (spd_d2 * Ksd2)); |
e5118069 | 0:d46cb1df87f1 | 236 | if(tmp2>=127)tmp2=127;//加速度の制限 |
e5118069 | 0:d46cb1df87f1 | 237 | if(tmp2<=-127)tmp2=-127; |
e5118069 | 0:d46cb1df87f1 | 238 | cmd2 += tmp2; |
e5118069 | 0:d46cb1df87f1 | 239 | |
e5118069 | 0:d46cb1df87f1 | 240 | spd_err3 = filtered_ref_spd - spd3; |
e5118069 | 0:d46cb1df87f1 | 241 | float spd_d3 = (spd_err3 - pre_err3)/INT_TIME; |
e5118069 | 0:d46cb1df87f1 | 242 | tmp3 = (int)((spd_err3 * Ksp3) + (spd_d3 * Ksd3)); |
e5118069 | 0:d46cb1df87f1 | 243 | if(tmp3>=127)tmp3=127;//加速度の制限 |
e5118069 | 0:d46cb1df87f1 | 244 | if(tmp3<=-127)tmp3=-127; |
e5118069 | 0:d46cb1df87f1 | 245 | cmd3 += tmp3; |
e5118069 | 0:d46cb1df87f1 | 246 | |
e5118069 | 0:d46cb1df87f1 | 247 | if (cmd2 > lim_cmd2) cmd2 = lim_cmd2;//上限指定 |
e5118069 | 0:d46cb1df87f1 | 248 | if (cmd2 < -lim_cmd2) cmd2 = -lim_cmd2; |
e5118069 | 0:d46cb1df87f1 | 249 | |
e5118069 | 0:d46cb1df87f1 | 250 | if (cmd3 > lim_cmd3) cmd3 = lim_cmd3;//上限指定 |
e5118069 | 0:d46cb1df87f1 | 251 | if (cmd3 < -lim_cmd3) cmd3 = -lim_cmd3; |
e5118069 | 0:d46cb1df87f1 | 252 | |
e5118069 | 0:d46cb1df87f1 | 253 | if (cmd2 > 0) { |
e5118069 | 0:d46cb1df87f1 | 254 | Saber.putc(SB_ADRS); |
e5118069 | 0:d46cb1df87f1 | 255 | Saber.putc(4); |
e5118069 | 0:d46cb1df87f1 | 256 | Saber.putc(cmd2); |
e5118069 | 0:d46cb1df87f1 | 257 | Saber.putc((SB_ADRS + 4 + cmd2) & 0b01111111); |
e5118069 | 0:d46cb1df87f1 | 258 | } |
e5118069 | 0:d46cb1df87f1 | 259 | else { |
e5118069 | 0:d46cb1df87f1 | 260 | Saber.putc(SB_ADRS); |
e5118069 | 0:d46cb1df87f1 | 261 | Saber.putc(5); |
e5118069 | 0:d46cb1df87f1 | 262 | Saber.putc(abs(cmd2)); |
e5118069 | 0:d46cb1df87f1 | 263 | Saber.putc((SB_ADRS + 5 + abs(cmd2)) & 0b01111111); |
e5118069 | 0:d46cb1df87f1 | 264 | } |
e5118069 | 0:d46cb1df87f1 | 265 | |
e5118069 | 0:d46cb1df87f1 | 266 | if (cmd3 > 0) { |
e5118069 | 0:d46cb1df87f1 | 267 | Saber.putc(SB_ADRS); |
e5118069 | 0:d46cb1df87f1 | 268 | Saber.putc(0); |
e5118069 | 0:d46cb1df87f1 | 269 | Saber.putc(cmd3); |
e5118069 | 0:d46cb1df87f1 | 270 | Saber.putc((SB_ADRS + 0 + cmd3) & 0b01111111); |
e5118069 | 0:d46cb1df87f1 | 271 | } |
e5118069 | 0:d46cb1df87f1 | 272 | else { |
e5118069 | 0:d46cb1df87f1 | 273 | Saber.putc(SB_ADRS); |
e5118069 | 0:d46cb1df87f1 | 274 | Saber.putc(1); |
e5118069 | 0:d46cb1df87f1 | 275 | Saber.putc(abs(cmd3)); |
e5118069 | 0:d46cb1df87f1 | 276 | Saber.putc((SB_ADRS + 1 + abs(cmd3)) & 0b01111111); |
e5118069 | 0:d46cb1df87f1 | 277 | } |
e5118069 | 0:d46cb1df87f1 | 278 | |
e5118069 | 0:d46cb1df87f1 | 279 | pre_spd2 = rot_sp2; |
e5118069 | 0:d46cb1df87f1 | 280 | pre_err2 = spd_err2; |
e5118069 | 0:d46cb1df87f1 | 281 | pre_spd3 = rot_sp3; |
e5118069 | 0:d46cb1df87f1 | 282 | pre_err3 = spd_err3; |
e5118069 | 0:d46cb1df87f1 | 283 | } |
e5118069 | 0:d46cb1df87f1 | 284 | |
e5118069 | 0:d46cb1df87f1 | 285 | |
e5118069 | 0:d46cb1df87f1 | 286 | if (!sw2.read()) { |
e5118069 | 0:d46cb1df87f1 | 287 | cmd_spd = 0; |
e5118069 | 0:d46cb1df87f1 | 288 | cmd_ang = 0; |
e5118069 | 0:d46cb1df87f1 | 289 | } |
e5118069 | 0:d46cb1df87f1 | 290 | |
e5118069 | 0:d46cb1df87f1 | 291 | if (cmd_ang >= 0) { |
e5118069 | 0:d46cb1df87f1 | 292 | Saber.putc(SB_ADRS_A); |
e5118069 | 0:d46cb1df87f1 | 293 | Saber.putc(1); |
e5118069 | 0:d46cb1df87f1 | 294 | Saber.putc(cmd_ang); |
e5118069 | 0:d46cb1df87f1 | 295 | Saber.putc((SB_ADRS_A + 1 + cmd_ang) & 0b01111111); |
e5118069 | 0:d46cb1df87f1 | 296 | } |
e5118069 | 0:d46cb1df87f1 | 297 | else { |
e5118069 | 0:d46cb1df87f1 | 298 | Saber.putc(SB_ADRS_A); |
e5118069 | 0:d46cb1df87f1 | 299 | Saber.putc(0); |
e5118069 | 0:d46cb1df87f1 | 300 | Saber.putc(abs(cmd_ang)); |
e5118069 | 0:d46cb1df87f1 | 301 | Saber.putc((SB_ADRS_A + 0 + abs(cmd_ang)) & 0b01111111); |
e5118069 | 0:d46cb1df87f1 | 302 | } |
e5118069 | 0:d46cb1df87f1 | 303 | |
e5118069 | 0:d46cb1df87f1 | 304 | if (cmd_spd >= 0) { |
e5118069 | 0:d46cb1df87f1 | 305 | Saber.putc(SB_ADRS_B); |
e5118069 | 0:d46cb1df87f1 | 306 | Saber.putc(1); |
e5118069 | 0:d46cb1df87f1 | 307 | Saber.putc(cmd_spd); |
e5118069 | 0:d46cb1df87f1 | 308 | Saber.putc((SB_ADRS_B + 1 + cmd_spd) & 0b01111111); |
e5118069 | 0:d46cb1df87f1 | 309 | } |
e5118069 | 0:d46cb1df87f1 | 310 | else { |
e5118069 | 0:d46cb1df87f1 | 311 | Saber.putc(SB_ADRS_B); |
e5118069 | 0:d46cb1df87f1 | 312 | Saber.putc(0); |
e5118069 | 0:d46cb1df87f1 | 313 | Saber.putc(abs(cmd_spd)); |
e5118069 | 0:d46cb1df87f1 | 314 | Saber.putc((SB_ADRS_B + 0 + abs(cmd_spd)) & 0b01111111); |
e5118069 | 0:d46cb1df87f1 | 315 | } |
e5118069 | 0:d46cb1df87f1 | 316 | |
e5118069 | 0:d46cb1df87f1 | 317 | pre_spd = spd; |
e5118069 | 0:d46cb1df87f1 | 318 | pre_spdE = spd_e; |
e5118069 | 0:d46cb1df87f1 | 319 | pre_angle = angle; |
e5118069 | 0:d46cb1df87f1 | 320 | pre_angleE = angle_P; |
e5118069 | 0:d46cb1df87f1 | 321 | } |
e5118069 | 0:d46cb1df87f1 | 322 | |
e5118069 | 0:d46cb1df87f1 | 323 | int main() { |
e5118069 | 0:d46cb1df87f1 | 324 | Saber.baud(115200); |
e5118069 | 0:d46cb1df87f1 | 325 | pc.baud(9600); |
e5118069 | 0:d46cb1df87f1 | 326 | timer.attach(timer_warikomi,INT_TIME); |
e5118069 | 0:d46cb1df87f1 | 327 | |
e5118069 | 0:d46cb1df87f1 | 328 | while(1) { |
e5118069 | 0:d46cb1df87f1 | 329 | } |
e5118069 | 0:d46cb1df87f1 | 330 | } |