project-R / Mbed 2 deprecated TOUTEKI_all_mbed

TOUTEKI

Dependencies:   mbed QEI2 UnderBody Filter

main.cpp@2:965cba546262, 2019-01-10 (annotated)

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