project-R / Mbed 2 deprecated TOUTEKI_all_mbed

TOUTEKI

Dependencies:   mbed QEI2 UnderBody Filter

main.cpp@4:017c55052d44, 2019-01-11 (annotated)

Committer:
e5118069
Date:
Fri Jan 11 07:28:44 2019 +0000
Revision:
4:017c55052d44
Parent:
3:de0b5dc55627
Child:
5:869dc702b923
2019111_complete;

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 4:017c55052d44 35 float Ksp2 = 6.5, Ksd2 = 0.4;
e5118069 4:017c55052d44 36 float Ksp3 = 6.5, 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 4:017c55052d44 91 static int lim_cmd2 = 87;
e5118069 2:965cba546262 92 static int lim_cmd3 = 92;
e5118069 2:965cba546262 93
e5118069 2:965cba546262 94 int sw_point = Button();
e5118069 2:965cba546262 95
e5118069 2:965cba546262 96 angle=(float)(encount)*(360.0/48.0)/4.0;
e5118069 2:965cba546262 97 SOKUDO=(angle-pre_angle)/INT_TIME;
e5118069 2:965cba546262 98
e5118069 2:965cba546262 99 e_D=(goal_D-angle);
e5118069 2:965cba546262 100 ed_D=(e_D-pre_e_D)/INT_TIME;
e5118069 2:965cba546262 101 ei_D+=(e_D+pre_e_D)*INT_TIME/2.0;
e5118069 2:965cba546262 102
e5118069 2:965cba546262 103 cmd=(int)((e_D*Kp)+(ed_D*Kd)+(ei_D*Ki));
e5118069 2:965cba546262 104
e5118069 2:965cba546262 105 float encount2 = Enc2.getPulses();
e5118069 2:965cba546262 106 float encount3 = Enc3.getPulses();
e5118069 2:965cba546262 107
e5118069 2:965cba546262 108 float rot_sp2 = encount2/MULTIPLU/ppr;
e5118069 2:965cba546262 109 spd2 = (rot_sp2 - pre_spd2)/INT_TIME/(48*4);
e5118069 2:965cba546262 110 float rot_sp3 = encount3/MULTIPLU/ppr;
e5118069 2:965cba546262 111 spd3 = (rot_sp3 - pre_spd3)/INT_TIME/(48*4);
e5118069 0:d46cb1df87f1 112
e5118069 2:965cba546262 113 spd_err2 = filtered_ref_spd - spd2;
e5118069 2:965cba546262 114 float spd_d2 = (spd_err2 - pre_err2)/INT_TIME;
e5118069 2:965cba546262 115 tmp1 = (int)((spd_err2 * Ksp2) + (spd_d2 * Ksd2));
e5118069 2:965cba546262 116 if(tmp1>=127)tmp1=127;
e5118069 2:965cba546262 117 if(tmp1<=-127)tmp1=-127;
e5118069 2:965cba546262 118 cmd2 += tmp1;
e5118069 0:d46cb1df87f1 119
e5118069 2:965cba546262 120 spd_err3 = filtered_ref_spd - spd3;
e5118069 2:965cba546262 121 float spd_d3 = (spd_err3 - pre_err3)/INT_TIME;
e5118069 2:965cba546262 122 tmp2 = (int)((spd_err3 * Ksp3) + (spd_d3 * Ksd3));
e5118069 2:965cba546262 123 if(tmp2>=127)tmp2=127;
e5118069 2:965cba546262 124 if(tmp2<=-127)tmp2=-127;
e5118069 2:965cba546262 125 cmd3 += tmp2;
e5118069 2:965cba546262 126
e5118069 2:965cba546262 127 if (cmd2 > lim_cmd2) cmd2 = lim_cmd2;
e5118069 2:965cba546262 128 if (cmd2 < -lim_cmd2) cmd2 = -lim_cmd2;
e5118069 2:965cba546262 129
e5118069 2:965cba546262 130 if (cmd3 > lim_cmd3) cmd3 = lim_cmd3;
e5118069 2:965cba546262 131 if (cmd3 < -lim_cmd3) cmd3 = -lim_cmd3;
e5118069 3:de0b5dc55627 132
e5118069 3:de0b5dc55627 133 if(sw_point != 0) switch(mode){
e5118069 1:94e15665b69f 134 case 0:
e5118069 0:d46cb1df87f1 135 goal_D=0;
e5118069 1:94e15665b69f 136 if(sw_point==2)mode=1;
e5118069 0:d46cb1df87f1 137 break;
e5118069 0:d46cb1df87f1 138 case 1:
e5118069 2:965cba546262 139 cmd=-15;
e5118069 1:94e15665b69f 140 if(sw_point==2)mode=2;
e5118069 0:d46cb1df87f1 141 if(LIM2==1){
e5118069 0:d46cb1df87f1 142 cmd=0;
e5118069 0:d46cb1df87f1 143 b_encount=Enc.getPulses();
e5118069 0:d46cb1df87f1 144 }
e5118069 0:d46cb1df87f1 145 break;
e5118069 3:de0b5dc55627 146 case 2:
e5118069 3:de0b5dc55627 147 goal_D=125;
e5118069 1:94e15665b69f 148 if(sw_point==2)mode=3;
e5118069 2:965cba546262 149 if(angle>=120)cmd=0;
e5118069 0:d46cb1df87f1 150 break;
e5118069 0:d46cb1df87f1 151
e5118069 3:de0b5dc55627 152 case 3:
e5118069 1:94e15665b69f 153 if(sw_point==2)mode=4;
e5118069 0:d46cb1df87f1 154 if(S1==0&&S2==0){
e5118069 3:de0b5dc55627 155 fet1=1;
e5118069 0:d46cb1df87f1 156 A=1;
e5118069 0:d46cb1df87f1 157 }
e5118069 0:d46cb1df87f1 158 break;
e5118069 0:d46cb1df87f1 159
e5118069 3:de0b5dc55627 160 case 4:
e5118069 0:d46cb1df87f1 161 goal_D=0;
e5118069 1:94e15665b69f 162 if(sw_point==2){
e5118069 0:d46cb1df87f1 163 cmd=0;
e5118069 0:d46cb1df87f1 164 b_encount=Enc.getPulses();
e5118069 0:d46cb1df87f1 165 mode=5;
e5118069 0:d46cb1df87f1 166 }
e5118069 0:d46cb1df87f1 167 break;
e5118069 3:de0b5dc55627 168 case 5:
e5118069 3:de0b5dc55627 169 fet1=0;
e5118069 0:d46cb1df87f1 170 A=0;
e5118069 1:94e15665b69f 171 if(sw_point==2)mode=6;
e5118069 1:94e15665b69f 172 break;
e5118069 3:de0b5dc55627 173
e5118069 3:de0b5dc55627 174 case(6):
e5118069 3:de0b5dc55627 175 ref_spd = 26.0;
e5118069 3:de0b5dc55627 176 if (sw_point == 2) mode = 7;
e5118069 3:de0b5dc55627 177 break;
e5118069 0:d46cb1df87f1 178
e5118069 3:de0b5dc55627 179 case(7):
e5118069 3:de0b5dc55627 180 fet2 = 0;
e5118069 3:de0b5dc55627 181 if (sw_point == 2) mode = 8;
e5118069 3:de0b5dc55627 182 break;
e5118069 0:d46cb1df87f1 183
e5118069 3:de0b5dc55627 184 case(8):
e5118069 3:de0b5dc55627 185 ref_spd = 0.0;
e5118069 3:de0b5dc55627 186 fet2 = 1;
e5118069 3:de0b5dc55627 187 if (sw_point == 2) mode = 0;
e5118069 3:de0b5dc55627 188 if (spd3<=0.5) cmd = 0;
e5118069 3:de0b5dc55627 189 break;
e5118069 3:de0b5dc55627 190 }
e5118069 4:017c55052d44 191
e5118069 4:017c55052d44 192 if(filtered_ref_spd>=25.5&&mode==6){
e5118069 4:017c55052d44 193 filtered_ref_spd=26;
e5118069 4:017c55052d44 194 }else if(filtered_ref_spd>=25.5&&mode==7){
e5118069 4:017c55052d44 195 filtered_ref_spd=26;
e5118069 4:017c55052d44 196 }else if(filtered_ref_spd<=5.0&&mode==8){
e5118069 4:017c55052d44 197 filtered_ref_spd=0;
e5118069 4:017c55052d44 198 }else if(mode==6||mode==7||mode==8){
e5118069 4:017c55052d44 199 filtered_ref_spd = velfilter.SecondOrderLag((double)ref_spd);
e5118069 4:017c55052d44 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 }