TOUTEKI
Dependencies: mbed QEI2 UnderBody Filter
main.cpp@5:869dc702b923, 2019-01-11 (annotated)
- Committer:
- e5118069
- Date:
- Fri Jan 11 09:33:04 2019 +0000
- Revision:
- 5:869dc702b923
- Parent:
- 4:017c55052d44
- Child:
- 6:7afdc6a81566
2019111_complete
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 | 1:94e15665b69f | 3 | #include "Filter.h" |
e5118069 | 1:94e15665b69f | 4 | #define SB_ADRS 132 |
e5118069 | 1:94e15665b69f | 5 | #define SABER_ADDR 128 |
e5118069 | 0:d46cb1df87f1 | 6 | #define INT_TIME 0.02 |
e5118069 | 0:d46cb1df87f1 | 7 | #define RESOLUTION 48 |
e5118069 | 0:d46cb1df87f1 | 8 | #define MULTIPLU 4.0 |
e5118069 | 0:d46cb1df87f1 | 9 | |
e5118069 | 0:d46cb1df87f1 | 10 | Ticker timer; |
e5118069 | 0:d46cb1df87f1 | 11 | Timer T; |
e5118069 | 1:94e15665b69f | 12 | QEI Enc2(p7,p8,NC,RESOLUTION,&T,QEI::X4_ENCODING); |
e5118069 | 1:94e15665b69f | 13 | QEI Enc3(p5,p6,NC,RESOLUTION,&T,QEI::X4_ENCODING); |
e5118069 | 5:869dc702b923 | 14 | QEI Enc(p12,p11,NC,RESOLUTION,&T,QEI::X4_ENCODING); |
e5118069 | 3:de0b5dc55627 | 15 | Serial Saber(p13,p14); |
e5118069 | 3:de0b5dc55627 | 16 | Serial pc(USBTX,USBRX); |
e5118069 | 1:94e15665b69f | 17 | Filter velfilter(INT_TIME); |
e5118069 | 0:d46cb1df87f1 | 18 | |
e5118069 | 5:869dc702b923 | 19 | DigitalIn sw2(p25); |
e5118069 | 3:de0b5dc55627 | 20 | DigitalIn sw1(p26);//モード切替 |
e5118069 | 3:de0b5dc55627 | 21 | DigitalOut fet1(p22); |
e5118069 | 2:965cba546262 | 22 | DigitalOut fet2(p21); |
e5118069 | 0:d46cb1df87f1 | 23 | |
e5118069 | 2:965cba546262 | 24 | DigitalIn limit1(p15); |
e5118069 | 2:965cba546262 | 25 | DigitalIn limit2(p16); |
e5118069 | 1:94e15665b69f | 26 | |
e5118069 | 2:965cba546262 | 27 | DigitalIn SENS1(p18); |
e5118069 | 1:94e15665b69f | 28 | DigitalIn SENS2(p17); |
e5118069 | 1:94e15665b69f | 29 | |
e5118069 | 5:869dc702b923 | 30 | DigitalIn G_limit1(p9); |
e5118069 | 5:869dc702b923 | 31 | DigitalIn G_limit2(p10); |
e5118069 | 5:869dc702b923 | 32 | |
e5118069 | 2:965cba546262 | 33 | int cmd,A; |
e5118069 | 2:965cba546262 | 34 | int SA1,B_SA1,LIM1,LIM2; |
e5118069 | 2:965cba546262 | 35 | int S1,S2; |
e5118069 | 1:94e15665b69f | 36 | |
e5118069 | 1:94e15665b69f | 37 | float angle,pre_angle,SOKUDO,e_D,pre_e_D,ed_D,ei_D,e_V,ed_V,pre_e_V,bcmd; |
e5118069 | 1:94e15665b69f | 38 | float goal_D=0,Kp=5,Ki=0.01,Kd=0.1; |
e5118069 | 4:017c55052d44 | 39 | float Ksp2 = 6.5, Ksd2 = 0.4; |
e5118069 | 4:017c55052d44 | 40 | float Ksp3 = 6.5, Ksd3 = 0.4; |
e5118069 | 1:94e15665b69f | 41 | |
e5118069 | 2:965cba546262 | 42 | float encount,b_encount; |
e5118069 | 1:94e15665b69f | 43 | |
e5118069 | 5:869dc702b923 | 44 | int mode = 7; |
e5118069 | 0:d46cb1df87f1 | 45 | int cmd2 = 0; |
e5118069 | 0:d46cb1df87f1 | 46 | int cmd3 = 0; |
e5118069 | 0:d46cb1df87f1 | 47 | |
e5118069 | 0:d46cb1df87f1 | 48 | float spd2=0; |
e5118069 | 0:d46cb1df87f1 | 49 | float spd3=0; |
e5118069 | 0:d46cb1df87f1 | 50 | |
e5118069 | 0:d46cb1df87f1 | 51 | float spd_err2=0; |
e5118069 | 0:d46cb1df87f1 | 52 | float spd_err3=0; |
e5118069 | 0:d46cb1df87f1 | 53 | |
e5118069 | 1:94e15665b69f | 54 | int tmp1; |
e5118069 | 1:94e15665b69f | 55 | int tmp2; |
e5118069 | 0:d46cb1df87f1 | 56 | |
e5118069 | 1:94e15665b69f | 57 | double filtered_ref_spd; |
e5118069 | 0:d46cb1df87f1 | 58 | |
e5118069 | 5:869dc702b923 | 59 | int G_LIM1=0,G_LIM2=0; |
e5118069 | 5:869dc702b923 | 60 | |
e5118069 | 5:869dc702b923 | 61 | int G_cmd; |
e5118069 | 5:869dc702b923 | 62 | |
e5118069 | 5:869dc702b923 | 63 | int modee=2; |
e5118069 | 5:869dc702b923 | 64 | int button_in_2=0; |
e5118069 | 5:869dc702b923 | 65 | |
e5118069 | 2:965cba546262 | 66 | int Button() { |
e5118069 | 1:94e15665b69f | 67 | |
e5118069 | 0:d46cb1df87f1 | 68 | int button_in = sw1.read(); |
e5118069 | 1:94e15665b69f | 69 | |
e5118069 | 1:94e15665b69f | 70 | static int pre_button = 0; |
e5118069 | 1:94e15665b69f | 71 | static int sw_state = 3; |
e5118069 | 0:d46cb1df87f1 | 72 | |
e5118069 | 1:94e15665b69f | 73 | if(button_in && pre_button)sw_state = 0; |
e5118069 | 1:94e15665b69f | 74 | if(!button_in && !pre_button)sw_state = 3; |
e5118069 | 1:94e15665b69f | 75 | if(button_in && !pre_button)sw_state = 1; |
e5118069 | 1:94e15665b69f | 76 | if(!button_in && pre_button)sw_state = 2; |
e5118069 | 0:d46cb1df87f1 | 77 | |
e5118069 | 0:d46cb1df87f1 | 78 | pre_button = button_in; |
e5118069 | 0:d46cb1df87f1 | 79 | |
e5118069 | 0:d46cb1df87f1 | 80 | return sw_state; |
e5118069 | 0:d46cb1df87f1 | 81 | } |
e5118069 | 5:869dc702b923 | 82 | int Button2() {//スイッチのノイズをとる関数 |
e5118069 | 5:869dc702b923 | 83 | |
e5118069 | 5:869dc702b923 | 84 | button_in_2 = sw2.read(); |
e5118069 | 5:869dc702b923 | 85 | |
e5118069 | 5:869dc702b923 | 86 | static int pre_button_2 = 0; |
e5118069 | 5:869dc702b923 | 87 | static int sw_state_2 = 3; |
e5118069 | 5:869dc702b923 | 88 | |
e5118069 | 5:869dc702b923 | 89 | if(button_in_2 && pre_button_2)sw_state_2 = 0; |
e5118069 | 5:869dc702b923 | 90 | if(!button_in_2 && !pre_button_2)sw_state_2 = 3; |
e5118069 | 5:869dc702b923 | 91 | if(button_in_2 && !pre_button_2)sw_state_2 = 1; |
e5118069 | 5:869dc702b923 | 92 | if(!button_in_2 && pre_button_2)sw_state_2 = 2; |
e5118069 | 5:869dc702b923 | 93 | |
e5118069 | 5:869dc702b923 | 94 | pre_button_2 = button_in_2; |
e5118069 | 5:869dc702b923 | 95 | |
e5118069 | 5:869dc702b923 | 96 | return sw_state_2; |
e5118069 | 5:869dc702b923 | 97 | } |
e5118069 | 5:869dc702b923 | 98 | |
e5118069 | 0:d46cb1df87f1 | 99 | |
e5118069 | 0:d46cb1df87f1 | 100 | void timer_warikomi() |
e5118069 | 1:94e15665b69f | 101 | { |
e5118069 | 1:94e15665b69f | 102 | |
e5118069 | 2:965cba546262 | 103 | LIM1=!limit1.read(); |
e5118069 | 2:965cba546262 | 104 | LIM2=!limit2.read(); |
e5118069 | 2:965cba546262 | 105 | S1=SENS1.read(); |
e5118069 | 0:d46cb1df87f1 | 106 | S2=SENS2.read(); |
e5118069 | 2:965cba546262 | 107 | encount=Enc.getPulses()-b_encount; |
e5118069 | 2:965cba546262 | 108 | |
e5118069 | 2:965cba546262 | 109 | float ppr = 1.0; |
e5118069 | 0:d46cb1df87f1 | 110 | |
e5118069 | 0:d46cb1df87f1 | 111 | static float pre_spd2 = 0.0; |
e5118069 | 0:d46cb1df87f1 | 112 | static float pre_spd3 = 0.0; |
e5118069 | 0:d46cb1df87f1 | 113 | |
e5118069 | 0:d46cb1df87f1 | 114 | static float pre_err2 = 0.0; |
e5118069 | 0:d46cb1df87f1 | 115 | static float pre_err3 = 0.0; |
e5118069 | 0:d46cb1df87f1 | 116 | |
e5118069 | 2:965cba546262 | 117 | static float ref_spd = 0.0; |
e5118069 | 2:965cba546262 | 118 | |
e5118069 | 4:017c55052d44 | 119 | static int lim_cmd2 = 87; |
e5118069 | 2:965cba546262 | 120 | static int lim_cmd3 = 92; |
e5118069 | 2:965cba546262 | 121 | |
e5118069 | 2:965cba546262 | 122 | int sw_point = Button(); |
e5118069 | 2:965cba546262 | 123 | |
e5118069 | 2:965cba546262 | 124 | angle=(float)(encount)*(360.0/48.0)/4.0; |
e5118069 | 2:965cba546262 | 125 | SOKUDO=(angle-pre_angle)/INT_TIME; |
e5118069 | 2:965cba546262 | 126 | |
e5118069 | 2:965cba546262 | 127 | e_D=(goal_D-angle); |
e5118069 | 2:965cba546262 | 128 | ed_D=(e_D-pre_e_D)/INT_TIME; |
e5118069 | 2:965cba546262 | 129 | ei_D+=(e_D+pre_e_D)*INT_TIME/2.0; |
e5118069 | 2:965cba546262 | 130 | |
e5118069 | 2:965cba546262 | 131 | cmd=(int)((e_D*Kp)+(ed_D*Kd)+(ei_D*Ki)); |
e5118069 | 2:965cba546262 | 132 | |
e5118069 | 2:965cba546262 | 133 | float encount2 = Enc2.getPulses(); |
e5118069 | 2:965cba546262 | 134 | float encount3 = Enc3.getPulses(); |
e5118069 | 2:965cba546262 | 135 | |
e5118069 | 2:965cba546262 | 136 | float rot_sp2 = encount2/MULTIPLU/ppr; |
e5118069 | 2:965cba546262 | 137 | spd2 = (rot_sp2 - pre_spd2)/INT_TIME/(48*4); |
e5118069 | 2:965cba546262 | 138 | float rot_sp3 = encount3/MULTIPLU/ppr; |
e5118069 | 2:965cba546262 | 139 | spd3 = (rot_sp3 - pre_spd3)/INT_TIME/(48*4); |
e5118069 | 0:d46cb1df87f1 | 140 | |
e5118069 | 2:965cba546262 | 141 | spd_err2 = filtered_ref_spd - spd2; |
e5118069 | 2:965cba546262 | 142 | float spd_d2 = (spd_err2 - pre_err2)/INT_TIME; |
e5118069 | 2:965cba546262 | 143 | tmp1 = (int)((spd_err2 * Ksp2) + (spd_d2 * Ksd2)); |
e5118069 | 2:965cba546262 | 144 | if(tmp1>=127)tmp1=127; |
e5118069 | 2:965cba546262 | 145 | if(tmp1<=-127)tmp1=-127; |
e5118069 | 2:965cba546262 | 146 | cmd2 += tmp1; |
e5118069 | 0:d46cb1df87f1 | 147 | |
e5118069 | 2:965cba546262 | 148 | spd_err3 = filtered_ref_spd - spd3; |
e5118069 | 2:965cba546262 | 149 | float spd_d3 = (spd_err3 - pre_err3)/INT_TIME; |
e5118069 | 2:965cba546262 | 150 | tmp2 = (int)((spd_err3 * Ksp3) + (spd_d3 * Ksd3)); |
e5118069 | 2:965cba546262 | 151 | if(tmp2>=127)tmp2=127; |
e5118069 | 2:965cba546262 | 152 | if(tmp2<=-127)tmp2=-127; |
e5118069 | 2:965cba546262 | 153 | cmd3 += tmp2; |
e5118069 | 2:965cba546262 | 154 | |
e5118069 | 2:965cba546262 | 155 | if (cmd2 > lim_cmd2) cmd2 = lim_cmd2; |
e5118069 | 2:965cba546262 | 156 | if (cmd2 < -lim_cmd2) cmd2 = -lim_cmd2; |
e5118069 | 2:965cba546262 | 157 | |
e5118069 | 2:965cba546262 | 158 | if (cmd3 > lim_cmd3) cmd3 = lim_cmd3; |
e5118069 | 2:965cba546262 | 159 | if (cmd3 < -lim_cmd3) cmd3 = -lim_cmd3; |
e5118069 | 3:de0b5dc55627 | 160 | |
e5118069 | 3:de0b5dc55627 | 161 | if(sw_point != 0) switch(mode){ |
e5118069 | 1:94e15665b69f | 162 | case 0: |
e5118069 | 0:d46cb1df87f1 | 163 | goal_D=0; |
e5118069 | 1:94e15665b69f | 164 | if(sw_point==2)mode=1; |
e5118069 | 0:d46cb1df87f1 | 165 | break; |
e5118069 | 0:d46cb1df87f1 | 166 | case 1: |
e5118069 | 2:965cba546262 | 167 | cmd=-15; |
e5118069 | 1:94e15665b69f | 168 | if(sw_point==2)mode=2; |
e5118069 | 0:d46cb1df87f1 | 169 | if(LIM2==1){ |
e5118069 | 0:d46cb1df87f1 | 170 | cmd=0; |
e5118069 | 0:d46cb1df87f1 | 171 | b_encount=Enc.getPulses(); |
e5118069 | 0:d46cb1df87f1 | 172 | } |
e5118069 | 0:d46cb1df87f1 | 173 | break; |
e5118069 | 3:de0b5dc55627 | 174 | case 2: |
e5118069 | 3:de0b5dc55627 | 175 | goal_D=125; |
e5118069 | 1:94e15665b69f | 176 | if(sw_point==2)mode=3; |
e5118069 | 2:965cba546262 | 177 | if(angle>=120)cmd=0; |
e5118069 | 5:869dc702b923 | 178 | if(S1==0&&S2==0){ |
e5118069 | 5:869dc702b923 | 179 | fet1=1; |
e5118069 | 5:869dc702b923 | 180 | A=1; |
e5118069 | 5:869dc702b923 | 181 | } |
e5118069 | 0:d46cb1df87f1 | 182 | break; |
e5118069 | 0:d46cb1df87f1 | 183 | |
e5118069 | 5:869dc702b923 | 184 | /*case 3: |
e5118069 | 1:94e15665b69f | 185 | if(sw_point==2)mode=4; |
e5118069 | 0:d46cb1df87f1 | 186 | if(S1==0&&S2==0){ |
e5118069 | 3:de0b5dc55627 | 187 | fet1=1; |
e5118069 | 0:d46cb1df87f1 | 188 | A=1; |
e5118069 | 0:d46cb1df87f1 | 189 | } |
e5118069 | 5:869dc702b923 | 190 | break;*/ |
e5118069 | 0:d46cb1df87f1 | 191 | |
e5118069 | 5:869dc702b923 | 192 | case 3: |
e5118069 | 0:d46cb1df87f1 | 193 | goal_D=0; |
e5118069 | 1:94e15665b69f | 194 | if(sw_point==2){ |
e5118069 | 0:d46cb1df87f1 | 195 | cmd=0; |
e5118069 | 0:d46cb1df87f1 | 196 | b_encount=Enc.getPulses(); |
e5118069 | 5:869dc702b923 | 197 | mode=4; |
e5118069 | 0:d46cb1df87f1 | 198 | } |
e5118069 | 0:d46cb1df87f1 | 199 | break; |
e5118069 | 5:869dc702b923 | 200 | case 4: |
e5118069 | 3:de0b5dc55627 | 201 | fet1=0; |
e5118069 | 5:869dc702b923 | 202 | if(sw_point==2)mode=5; |
e5118069 | 1:94e15665b69f | 203 | break; |
e5118069 | 3:de0b5dc55627 | 204 | |
e5118069 | 5:869dc702b923 | 205 | case 5: |
e5118069 | 3:de0b5dc55627 | 206 | ref_spd = 26.0; |
e5118069 | 5:869dc702b923 | 207 | if (sw_point == 2) mode = 6; |
e5118069 | 5:869dc702b923 | 208 | break; |
e5118069 | 5:869dc702b923 | 209 | |
e5118069 | 5:869dc702b923 | 210 | case 6: |
e5118069 | 5:869dc702b923 | 211 | fet2 = 0; |
e5118069 | 3:de0b5dc55627 | 212 | if (sw_point == 2) mode = 7; |
e5118069 | 3:de0b5dc55627 | 213 | break; |
e5118069 | 0:d46cb1df87f1 | 214 | |
e5118069 | 5:869dc702b923 | 215 | case 7: |
e5118069 | 3:de0b5dc55627 | 216 | ref_spd = 0.0; |
e5118069 | 3:de0b5dc55627 | 217 | fet2 = 1; |
e5118069 | 3:de0b5dc55627 | 218 | if (sw_point == 2) mode = 0; |
e5118069 | 5:869dc702b923 | 219 | if (spd3<=5.0) { |
e5118069 | 5:869dc702b923 | 220 | cmd2 = 0; |
e5118069 | 5:869dc702b923 | 221 | cmd3 = 0; |
e5118069 | 5:869dc702b923 | 222 | } |
e5118069 | 3:de0b5dc55627 | 223 | break; |
e5118069 | 3:de0b5dc55627 | 224 | } |
e5118069 | 4:017c55052d44 | 225 | |
e5118069 | 5:869dc702b923 | 226 | G_LIM1=!G_limit1.read();//pullupなので逆 |
e5118069 | 5:869dc702b923 | 227 | G_LIM2=!G_limit2.read();//pullupなので逆 |
e5118069 | 5:869dc702b923 | 228 | |
e5118069 | 5:869dc702b923 | 229 | int sw_point2 = Button2();//スイッチの関数からリターン |
e5118069 | 5:869dc702b923 | 230 | |
e5118069 | 5:869dc702b923 | 231 | if(sw_point2 != 0) switch(modee){ |
e5118069 | 5:869dc702b923 | 232 | case(0): |
e5118069 | 5:869dc702b923 | 233 | G_cmd=120; |
e5118069 | 5:869dc702b923 | 234 | if(G_LIM1==1){ |
e5118069 | 5:869dc702b923 | 235 | G_cmd=0; |
e5118069 | 5:869dc702b923 | 236 | } |
e5118069 | 5:869dc702b923 | 237 | if (sw_point2 == 2) modee = 1; |
e5118069 | 5:869dc702b923 | 238 | |
e5118069 | 5:869dc702b923 | 239 | break; |
e5118069 | 5:869dc702b923 | 240 | |
e5118069 | 5:869dc702b923 | 241 | case(1): |
e5118069 | 5:869dc702b923 | 242 | G_cmd=-120; |
e5118069 | 5:869dc702b923 | 243 | if (sw_point2 == 2) modee = 2; |
e5118069 | 5:869dc702b923 | 244 | if(G_LIM2==1){ |
e5118069 | 5:869dc702b923 | 245 | G_cmd=0; |
e5118069 | 5:869dc702b923 | 246 | } |
e5118069 | 5:869dc702b923 | 247 | break; |
e5118069 | 5:869dc702b923 | 248 | |
e5118069 | 5:869dc702b923 | 249 | case(2)://モータ停止と押し上げ機構の降下 |
e5118069 | 5:869dc702b923 | 250 | G_cmd=0; |
e5118069 | 5:869dc702b923 | 251 | if (sw_point2 == 2) modee = 0; |
e5118069 | 5:869dc702b923 | 252 | break; |
e5118069 | 5:869dc702b923 | 253 | } |
e5118069 | 5:869dc702b923 | 254 | |
e5118069 | 5:869dc702b923 | 255 | |
e5118069 | 5:869dc702b923 | 256 | if(filtered_ref_spd>=25.5&&mode==5){ |
e5118069 | 4:017c55052d44 | 257 | filtered_ref_spd=26; |
e5118069 | 5:869dc702b923 | 258 | }else if(filtered_ref_spd>=25.5&&mode==6){ |
e5118069 | 5:869dc702b923 | 259 | filtered_ref_spd=26; |
e5118069 | 5:869dc702b923 | 260 | }else if(filtered_ref_spd<=5.0&&mode==7){ |
e5118069 | 4:017c55052d44 | 261 | filtered_ref_spd=0; |
e5118069 | 5:869dc702b923 | 262 | }else if(mode==5||mode==6||mode==7){ |
e5118069 | 4:017c55052d44 | 263 | filtered_ref_spd = velfilter.SecondOrderLag((double)ref_spd); |
e5118069 | 4:017c55052d44 | 264 | } |
e5118069 | 1:94e15665b69f | 265 | |
e5118069 | 3:de0b5dc55627 | 266 | if(cmd>20) cmd=20; |
e5118069 | 2:965cba546262 | 267 | if(cmd<-15)cmd=-15; |
e5118069 | 1:94e15665b69f | 268 | |
e5118069 | 2:965cba546262 | 269 | int F=1,FF=0;//向き |
e5118069 | 0:d46cb1df87f1 | 270 | |
e5118069 | 3:de0b5dc55627 | 271 | if(cmd>=0) { |
e5118069 | 2:965cba546262 | 272 | Saber.putc(SABER_ADDR); |
e5118069 | 2:965cba546262 | 273 | Saber.putc(F); |
e5118069 | 2:965cba546262 | 274 | Saber.putc(abs(cmd)); |
e5118069 | 2:965cba546262 | 275 | Saber.putc((SABER_ADDR + F + abs(cmd)) & 0b01111111); |
e5118069 | 2:965cba546262 | 276 | } else { |
e5118069 | 2:965cba546262 | 277 | Saber.putc(SABER_ADDR); |
e5118069 | 2:965cba546262 | 278 | Saber.putc(FF); |
e5118069 | 2:965cba546262 | 279 | Saber.putc(abs(cmd)); |
e5118069 | 2:965cba546262 | 280 | Saber.putc((SABER_ADDR + FF + abs(cmd)) & 0b01111111); |
e5118069 | 2:965cba546262 | 281 | } |
e5118069 | 0:d46cb1df87f1 | 282 | |
e5118069 | 0:d46cb1df87f1 | 283 | if (cmd2 > 0) { |
e5118069 | 0:d46cb1df87f1 | 284 | Saber.putc(SB_ADRS); |
e5118069 | 0:d46cb1df87f1 | 285 | Saber.putc(4); |
e5118069 | 0:d46cb1df87f1 | 286 | Saber.putc(cmd2); |
e5118069 | 0:d46cb1df87f1 | 287 | Saber.putc((SB_ADRS + 4 + cmd2) & 0b01111111); |
e5118069 | 0:d46cb1df87f1 | 288 | } |
e5118069 | 0:d46cb1df87f1 | 289 | else { |
e5118069 | 0:d46cb1df87f1 | 290 | Saber.putc(SB_ADRS); |
e5118069 | 0:d46cb1df87f1 | 291 | Saber.putc(5); |
e5118069 | 0:d46cb1df87f1 | 292 | Saber.putc(abs(cmd2)); |
e5118069 | 0:d46cb1df87f1 | 293 | Saber.putc((SB_ADRS + 5 + abs(cmd2)) & 0b01111111); |
e5118069 | 0:d46cb1df87f1 | 294 | } |
e5118069 | 0:d46cb1df87f1 | 295 | |
e5118069 | 0:d46cb1df87f1 | 296 | if (cmd3 > 0) { |
e5118069 | 0:d46cb1df87f1 | 297 | Saber.putc(SB_ADRS); |
e5118069 | 0:d46cb1df87f1 | 298 | Saber.putc(0); |
e5118069 | 0:d46cb1df87f1 | 299 | Saber.putc(cmd3); |
e5118069 | 0:d46cb1df87f1 | 300 | Saber.putc((SB_ADRS + 0 + cmd3) & 0b01111111); |
e5118069 | 0:d46cb1df87f1 | 301 | } |
e5118069 | 0:d46cb1df87f1 | 302 | else { |
e5118069 | 0:d46cb1df87f1 | 303 | Saber.putc(SB_ADRS); |
e5118069 | 0:d46cb1df87f1 | 304 | Saber.putc(1); |
e5118069 | 0:d46cb1df87f1 | 305 | Saber.putc(abs(cmd3)); |
e5118069 | 0:d46cb1df87f1 | 306 | Saber.putc((SB_ADRS + 1 + abs(cmd3)) & 0b01111111); |
e5118069 | 0:d46cb1df87f1 | 307 | } |
e5118069 | 5:869dc702b923 | 308 | if (G_cmd > 0) { |
e5118069 | 5:869dc702b923 | 309 | Saber.putc(SABER_ADDR); |
e5118069 | 5:869dc702b923 | 310 | Saber.putc(4); |
e5118069 | 5:869dc702b923 | 311 | Saber.putc(G_cmd); |
e5118069 | 5:869dc702b923 | 312 | Saber.putc((SABER_ADDR + 4 + G_cmd) & 0b01111111); |
e5118069 | 5:869dc702b923 | 313 | } |
e5118069 | 5:869dc702b923 | 314 | else { |
e5118069 | 5:869dc702b923 | 315 | Saber.putc(SABER_ADDR); |
e5118069 | 5:869dc702b923 | 316 | Saber.putc(5); |
e5118069 | 5:869dc702b923 | 317 | Saber.putc(abs(G_cmd)); |
e5118069 | 5:869dc702b923 | 318 | Saber.putc((SABER_ADDR + 5 + abs(G_cmd)) & 0b01111111); |
e5118069 | 5:869dc702b923 | 319 | } |
e5118069 | 0:d46cb1df87f1 | 320 | |
e5118069 | 3:de0b5dc55627 | 321 | pre_spd2 = rot_sp2; |
e5118069 | 3:de0b5dc55627 | 322 | pre_err2 = spd_err2; |
e5118069 | 3:de0b5dc55627 | 323 | pre_spd3 = rot_sp3; |
e5118069 | 3:de0b5dc55627 | 324 | pre_err3 = spd_err3; |
e5118069 | 1:94e15665b69f | 325 | |
e5118069 | 1:94e15665b69f | 326 | |
e5118069 | 3:de0b5dc55627 | 327 | pre_e_D=e_D; |
e5118069 | 1:94e15665b69f | 328 | pre_angle=angle; |
e5118069 | 1:94e15665b69f | 329 | pre_e_V=e_V; |
e5118069 | 2:965cba546262 | 330 | B_SA1=SA1; |
e5118069 | 0:d46cb1df87f1 | 331 | } |
e5118069 | 0:d46cb1df87f1 | 332 | |
e5118069 | 0:d46cb1df87f1 | 333 | |
e5118069 | 0:d46cb1df87f1 | 334 | int main() { |
e5118069 | 1:94e15665b69f | 335 | Saber.baud(19200); |
e5118069 | 0:d46cb1df87f1 | 336 | pc.baud(9600); |
e5118069 | 3:de0b5dc55627 | 337 | fet1=0; |
e5118069 | 2:965cba546262 | 338 | fet2=1; |
e5118069 | 2:965cba546262 | 339 | |
e5118069 | 5:869dc702b923 | 340 | G_limit1.mode( PullUp ); // 内蔵プルアップを使う |
e5118069 | 5:869dc702b923 | 341 | G_limit2.mode( PullUp ); |
e5118069 | 5:869dc702b923 | 342 | |
e5118069 | 1:94e15665b69f | 343 | velfilter.setSecondOrderPara(1.0, 0.9, 0.0); |
e5118069 | 1:94e15665b69f | 344 | |
e5118069 | 0:d46cb1df87f1 | 345 | timer.attach(timer_warikomi,INT_TIME); |
e5118069 | 0:d46cb1df87f1 | 346 | |
e5118069 | 0:d46cb1df87f1 | 347 | while(1) { |
e5118069 | 2:965cba546262 | 348 | //pc.printf("%d\n",mode); |
e5118069 | 5:869dc702b923 | 349 | pc.printf("spd2%f\t spd3 %f\n",spd2,spd3); |
e5118069 | 0:d46cb1df87f1 | 350 | } |
e5118069 | 1:94e15665b69f | 351 | } |