project-R / Mbed 2 deprecated TOUTEKI_all_mbed

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?

UserRevisionLine numberNew 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 }