project-R / Mbed 2 deprecated TOUTEKI_all_mbed

TOUTEKI

Dependencies:   mbed QEI2 UnderBody Filter

main.cpp@3:de0b5dc55627, 2019-01-10 (annotated)

Committer:
e5118069
Date:
Thu Jan 10 13:55:47 2019 +0000
Revision:
3:de0b5dc55627
Parent:
2:965cba546262
Child:
4:017c55052d44
SHAGAI_HAZI_2019110

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 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 3:de0b5dc55627 12 QEI Enc(p12,p11,NC,RESOLUTION,&T,QEI::X4_ENCODING);
e5118069 1:94e15665b69f 13 QEI Enc2(p7,p8,NC,RESOLUTION,&T,QEI::X4_ENCODING);
e5118069 1:94e15665b69f 14 QEI Enc3(p5,p6,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 3:de0b5dc55627 19 DigitalIn sw1(p26);//モード切替
e5118069 3:de0b5dc55627 20 DigitalOut fet1(p22);
e5118069 2:965cba546262 21 DigitalOut fet2(p21);
e5118069 0:d46cb1df87f1 22
e5118069 2:965cba546262 23 DigitalIn limit1(p15);
e5118069 2:965cba546262 24 DigitalIn limit2(p16);
e5118069 1:94e15665b69f 25
e5118069 2:965cba546262 26 DigitalIn SENS1(p18);
e5118069 1:94e15665b69f 27 DigitalIn SENS2(p17);
e5118069 1:94e15665b69f 28
e5118069 2:965cba546262 29 int cmd,A;
e5118069 2:965cba546262 30 int SA1,B_SA1,LIM1,LIM2;
e5118069 2:965cba546262 31 int S1,S2;
e5118069 1:94e15665b69f 32
e5118069 1:94e15665b69f 33 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 34 float goal_D=0,Kp=5,Ki=0.01,Kd=0.1;
e5118069 3:de0b5dc55627 35 float Ksp2 = 7.0, Ksd2 = 0.4;
e5118069 3:de0b5dc55627 36 float Ksp3 = 7.0, Ksd3 = 0.4;
e5118069 1:94e15665b69f 37
e5118069 2:965cba546262 38 float encount,b_encount;
e5118069 1:94e15665b69f 39
e5118069 2:965cba546262 40 int mode = 8;
e5118069 0:d46cb1df87f1 41 int cmd2 = 0;
e5118069 0:d46cb1df87f1 42 int cmd3 = 0;
e5118069 0:d46cb1df87f1 43
e5118069 0:d46cb1df87f1 44 float spd2=0;
e5118069 0:d46cb1df87f1 45 float spd3=0;
e5118069 0:d46cb1df87f1 46
e5118069 0:d46cb1df87f1 47 float spd_err2=0;
e5118069 0:d46cb1df87f1 48 float spd_err3=0;
e5118069 0:d46cb1df87f1 49
e5118069 1:94e15665b69f 50 int tmp1;
e5118069 1:94e15665b69f 51 int tmp2;
e5118069 0:d46cb1df87f1 52
e5118069 1:94e15665b69f 53 double filtered_ref_spd;
e5118069 0:d46cb1df87f1 54
e5118069 2:965cba546262 55 int Button() {
e5118069 1:94e15665b69f 56
e5118069 0:d46cb1df87f1 57 int button_in = sw1.read();
e5118069 1:94e15665b69f 58
e5118069 1:94e15665b69f 59 static int pre_button = 0;
e5118069 1:94e15665b69f 60 static int sw_state = 3;
e5118069 0:d46cb1df87f1 61
e5118069 1:94e15665b69f 62 if(button_in && pre_button)sw_state = 0;
e5118069 1:94e15665b69f 63 if(!button_in && !pre_button)sw_state = 3;
e5118069 1:94e15665b69f 64 if(button_in && !pre_button)sw_state = 1;
e5118069 1:94e15665b69f 65 if(!button_in && pre_button)sw_state = 2;
e5118069 0:d46cb1df87f1 66
e5118069 0:d46cb1df87f1 67 pre_button = button_in;
e5118069 0:d46cb1df87f1 68
e5118069 0:d46cb1df87f1 69 return sw_state;
e5118069 0:d46cb1df87f1 70 }
e5118069 0:d46cb1df87f1 71
e5118069 0:d46cb1df87f1 72 void timer_warikomi()
e5118069 1:94e15665b69f 73 {
e5118069 1:94e15665b69f 74
e5118069 2:965cba546262 75 LIM1=!limit1.read();
e5118069 2:965cba546262 76 LIM2=!limit2.read();
e5118069 2:965cba546262 77 S1=SENS1.read();
e5118069 0:d46cb1df87f1 78 S2=SENS2.read();
e5118069 2:965cba546262 79 encount=Enc.getPulses()-b_encount;
e5118069 2:965cba546262 80
e5118069 2:965cba546262 81 float ppr = 1.0;
e5118069 0:d46cb1df87f1 82
e5118069 0:d46cb1df87f1 83 static float pre_spd2 = 0.0;
e5118069 0:d46cb1df87f1 84 static float pre_spd3 = 0.0;
e5118069 0:d46cb1df87f1 85
e5118069 0:d46cb1df87f1 86 static float pre_err2 = 0.0;
e5118069 0:d46cb1df87f1 87 static float pre_err3 = 0.0;
e5118069 0:d46cb1df87f1 88
e5118069 2:965cba546262 89 static float ref_spd = 0.0;
e5118069 2:965cba546262 90
e5118069 2:965cba546262 91 static int lim_cmd2 = 85;
e5118069 2:965cba546262 92 static int lim_cmd3 = 92;
e5118069 2:965cba546262 93
e5118069 2:965cba546262 94 int sw_point = Button();
e5118069 3:de0b5dc55627 95
e5118069 3:de0b5dc55627 96 if(filtered_ref_spd>=25.5&&mode==6){
e5118069 3:de0b5dc55627 97 filtered_ref_spd=26;
e5118069 3:de0b5dc55627 98 }else if(filtered_ref_spd>=25.5&&mode==7){
e5118069 3:de0b5dc55627 99 filtered_ref_spd=26;
e5118069 3:de0b5dc55627 100 }else if(filtered_ref_spd<=5.0&&mode==8){
e5118069 3:de0b5dc55627 101 filtered_ref_spd=0;
e5118069 3:de0b5dc55627 102 }else if(mode==6||mode==7||mode==8){
e5118069 3:de0b5dc55627 103 filtered_ref_spd = velfilter.SecondOrderLag((double)ref_spd);
e5118069 3:de0b5dc55627 104 }
e5118069 2:965cba546262 105
e5118069 2:965cba546262 106 angle=(float)(encount)*(360.0/48.0)/4.0;
e5118069 2:965cba546262 107 SOKUDO=(angle-pre_angle)/INT_TIME;
e5118069 2:965cba546262 108
e5118069 2:965cba546262 109 e_D=(goal_D-angle);
e5118069 2:965cba546262 110 ed_D=(e_D-pre_e_D)/INT_TIME;
e5118069 2:965cba546262 111 ei_D+=(e_D+pre_e_D)*INT_TIME/2.0;
e5118069 2:965cba546262 112
e5118069 2:965cba546262 113 cmd=(int)((e_D*Kp)+(ed_D*Kd)+(ei_D*Ki));
e5118069 2:965cba546262 114
e5118069 2:965cba546262 115 float encount2 = Enc2.getPulses();
e5118069 2:965cba546262 116 float encount3 = Enc3.getPulses();
e5118069 2:965cba546262 117
e5118069 2:965cba546262 118 float rot_sp2 = encount2/MULTIPLU/ppr;
e5118069 2:965cba546262 119 spd2 = (rot_sp2 - pre_spd2)/INT_TIME/(48*4);
e5118069 2:965cba546262 120 float rot_sp3 = encount3/MULTIPLU/ppr;
e5118069 2:965cba546262 121 spd3 = (rot_sp3 - pre_spd3)/INT_TIME/(48*4);
e5118069 0:d46cb1df87f1 122
e5118069 2:965cba546262 123 spd_err2 = filtered_ref_spd - spd2;
e5118069 2:965cba546262 124 float spd_d2 = (spd_err2 - pre_err2)/INT_TIME;
e5118069 2:965cba546262 125 tmp1 = (int)((spd_err2 * Ksp2) + (spd_d2 * Ksd2));
e5118069 2:965cba546262 126 if(tmp1>=127)tmp1=127;
e5118069 2:965cba546262 127 if(tmp1<=-127)tmp1=-127;
e5118069 2:965cba546262 128 cmd2 += tmp1;
e5118069 0:d46cb1df87f1 129
e5118069 2:965cba546262 130 spd_err3 = filtered_ref_spd - spd3;
e5118069 2:965cba546262 131 float spd_d3 = (spd_err3 - pre_err3)/INT_TIME;
e5118069 2:965cba546262 132 tmp2 = (int)((spd_err3 * Ksp3) + (spd_d3 * Ksd3));
e5118069 2:965cba546262 133 if(tmp2>=127)tmp2=127;
e5118069 2:965cba546262 134 if(tmp2<=-127)tmp2=-127;
e5118069 2:965cba546262 135 cmd3 += tmp2;
e5118069 2:965cba546262 136
e5118069 2:965cba546262 137 if (cmd2 > lim_cmd2) cmd2 = lim_cmd2;
e5118069 2:965cba546262 138 if (cmd2 < -lim_cmd2) cmd2 = -lim_cmd2;
e5118069 2:965cba546262 139
e5118069 2:965cba546262 140 if (cmd3 > lim_cmd3) cmd3 = lim_cmd3;
e5118069 2:965cba546262 141 if (cmd3 < -lim_cmd3) cmd3 = -lim_cmd3;
e5118069 3:de0b5dc55627 142
e5118069 3:de0b5dc55627 143 if(sw_point != 0) switch(mode){
e5118069 1:94e15665b69f 144 case 0:
e5118069 0:d46cb1df87f1 145 goal_D=0;
e5118069 1:94e15665b69f 146 if(sw_point==2)mode=1;
e5118069 0:d46cb1df87f1 147 break;
e5118069 0:d46cb1df87f1 148 case 1:
e5118069 2:965cba546262 149 cmd=-15;
e5118069 1:94e15665b69f 150 if(sw_point==2)mode=2;
e5118069 0:d46cb1df87f1 151 if(LIM2==1){
e5118069 0:d46cb1df87f1 152 cmd=0;
e5118069 0:d46cb1df87f1 153 b_encount=Enc.getPulses();
e5118069 0:d46cb1df87f1 154 }
e5118069 0:d46cb1df87f1 155 break;
e5118069 3:de0b5dc55627 156 case 2:
e5118069 3:de0b5dc55627 157 goal_D=125;
e5118069 1:94e15665b69f 158 if(sw_point==2)mode=3;
e5118069 2:965cba546262 159 if(angle>=120)cmd=0;
e5118069 0:d46cb1df87f1 160 break;
e5118069 0:d46cb1df87f1 161
e5118069 3:de0b5dc55627 162 case 3:
e5118069 1:94e15665b69f 163 if(sw_point==2)mode=4;
e5118069 0:d46cb1df87f1 164 if(S1==0&&S2==0){
e5118069 3:de0b5dc55627 165 fet1=1;
e5118069 0:d46cb1df87f1 166 A=1;
e5118069 0:d46cb1df87f1 167 }
e5118069 0:d46cb1df87f1 168 break;
e5118069 0:d46cb1df87f1 169
e5118069 3:de0b5dc55627 170 case 4:
e5118069 0:d46cb1df87f1 171 goal_D=0;
e5118069 1:94e15665b69f 172 if(sw_point==2){
e5118069 0:d46cb1df87f1 173 cmd=0;
e5118069 0:d46cb1df87f1 174 b_encount=Enc.getPulses();
e5118069 0:d46cb1df87f1 175 mode=5;
e5118069 0:d46cb1df87f1 176 }
e5118069 0:d46cb1df87f1 177 break;
e5118069 3:de0b5dc55627 178 case 5:
e5118069 3:de0b5dc55627 179 fet1=0;
e5118069 0:d46cb1df87f1 180 A=0;
e5118069 1:94e15665b69f 181 if(sw_point==2)mode=6;
e5118069 1:94e15665b69f 182 break;
e5118069 3:de0b5dc55627 183
e5118069 3:de0b5dc55627 184 case(6):
e5118069 3:de0b5dc55627 185 ref_spd = 26.0;
e5118069 3:de0b5dc55627 186 if (sw_point == 2) mode = 7;
e5118069 3:de0b5dc55627 187 break;
e5118069 0:d46cb1df87f1 188
e5118069 3:de0b5dc55627 189 case(7):
e5118069 3:de0b5dc55627 190 fet2 = 0;
e5118069 3:de0b5dc55627 191 if (sw_point == 2) mode = 8;
e5118069 3:de0b5dc55627 192 break;
e5118069 0:d46cb1df87f1 193
e5118069 3:de0b5dc55627 194 case(8):
e5118069 3:de0b5dc55627 195 ref_spd = 0.0;
e5118069 3:de0b5dc55627 196 fet2 = 1;
e5118069 3:de0b5dc55627 197 if (sw_point == 2) mode = 0;
e5118069 3:de0b5dc55627 198 if (spd3<=0.5) cmd = 0;
e5118069 3:de0b5dc55627 199 break;
e5118069 3:de0b5dc55627 200 }
e5118069 1:94e15665b69f 201
e5118069 3:de0b5dc55627 202 if(cmd>20) cmd=20;
e5118069 2:965cba546262 203 if(cmd<-15)cmd=-15;
e5118069 1:94e15665b69f 204
e5118069 2:965cba546262 205 int F=1,FF=0;//向き
e5118069 0:d46cb1df87f1 206
e5118069 3:de0b5dc55627 207 if(cmd>=0) {
e5118069 2:965cba546262 208 Saber.putc(SABER_ADDR);
e5118069 2:965cba546262 209 Saber.putc(F);
e5118069 2:965cba546262 210 Saber.putc(abs(cmd));
e5118069 2:965cba546262 211 Saber.putc((SABER_ADDR + F + abs(cmd)) & 0b01111111);
e5118069 2:965cba546262 212 } else {
e5118069 2:965cba546262 213 Saber.putc(SABER_ADDR);
e5118069 2:965cba546262 214 Saber.putc(FF);
e5118069 2:965cba546262 215 Saber.putc(abs(cmd));
e5118069 2:965cba546262 216 Saber.putc((SABER_ADDR + FF + abs(cmd)) & 0b01111111);
e5118069 2:965cba546262 217 }
e5118069 0:d46cb1df87f1 218
e5118069 0:d46cb1df87f1 219 if (cmd2 > 0) {
e5118069 0:d46cb1df87f1 220 Saber.putc(SB_ADRS);
e5118069 0:d46cb1df87f1 221 Saber.putc(4);
e5118069 0:d46cb1df87f1 222 Saber.putc(cmd2);
e5118069 0:d46cb1df87f1 223 Saber.putc((SB_ADRS + 4 + cmd2) & 0b01111111);
e5118069 0:d46cb1df87f1 224 }
e5118069 0:d46cb1df87f1 225 else {
e5118069 0:d46cb1df87f1 226 Saber.putc(SB_ADRS);
e5118069 0:d46cb1df87f1 227 Saber.putc(5);
e5118069 0:d46cb1df87f1 228 Saber.putc(abs(cmd2));
e5118069 0:d46cb1df87f1 229 Saber.putc((SB_ADRS + 5 + abs(cmd2)) & 0b01111111);
e5118069 0:d46cb1df87f1 230 }
e5118069 0:d46cb1df87f1 231
e5118069 0:d46cb1df87f1 232 if (cmd3 > 0) {
e5118069 0:d46cb1df87f1 233 Saber.putc(SB_ADRS);
e5118069 0:d46cb1df87f1 234 Saber.putc(0);
e5118069 0:d46cb1df87f1 235 Saber.putc(cmd3);
e5118069 0:d46cb1df87f1 236 Saber.putc((SB_ADRS + 0 + cmd3) & 0b01111111);
e5118069 0:d46cb1df87f1 237 }
e5118069 0:d46cb1df87f1 238 else {
e5118069 0:d46cb1df87f1 239 Saber.putc(SB_ADRS);
e5118069 0:d46cb1df87f1 240 Saber.putc(1);
e5118069 0:d46cb1df87f1 241 Saber.putc(abs(cmd3));
e5118069 0:d46cb1df87f1 242 Saber.putc((SB_ADRS + 1 + abs(cmd3)) & 0b01111111);
e5118069 0:d46cb1df87f1 243 }
e5118069 0:d46cb1df87f1 244
e5118069 3:de0b5dc55627 245 pre_spd2 = rot_sp2;
e5118069 3:de0b5dc55627 246 pre_err2 = spd_err2;
e5118069 3:de0b5dc55627 247 pre_spd3 = rot_sp3;
e5118069 3:de0b5dc55627 248 pre_err3 = spd_err3;
e5118069 1:94e15665b69f 249
e5118069 1:94e15665b69f 250
e5118069 3:de0b5dc55627 251 pre_e_D=e_D;
e5118069 1:94e15665b69f 252 pre_angle=angle;
e5118069 1:94e15665b69f 253 pre_e_V=e_V;
e5118069 2:965cba546262 254 B_SA1=SA1;
e5118069 0:d46cb1df87f1 255 }
e5118069 0:d46cb1df87f1 256
e5118069 0:d46cb1df87f1 257
e5118069 0:d46cb1df87f1 258 int main() {
e5118069 1:94e15665b69f 259 Saber.baud(19200);
e5118069 0:d46cb1df87f1 260 pc.baud(9600);
e5118069 3:de0b5dc55627 261 fet1=0;
e5118069 2:965cba546262 262 fet2=1;
e5118069 2:965cba546262 263
e5118069 1:94e15665b69f 264 velfilter.setSecondOrderPara(1.0, 0.9, 0.0);
e5118069 1:94e15665b69f 265
e5118069 0:d46cb1df87f1 266 timer.attach(timer_warikomi,INT_TIME);
e5118069 0:d46cb1df87f1 267
e5118069 0:d46cb1df87f1 268 while(1) {
e5118069 2:965cba546262 269 //pc.printf("%d\n",mode);
e5118069 2:965cba546262 270 pc.printf("spd2 %f\t spd3 %f\n",spd2,spd3);
e5118069 0:d46cb1df87f1 271 }
e5118069 1:94e15665b69f 272 }