project-R / Mbed 2 deprecated TOUTEKI_all_mbed

TOUTEKI

Dependencies:   mbed QEI2 UnderBody Filter

main.cpp@6:7afdc6a81566, 2019-03-08 (annotated)

Committer:
sink
Date:
Fri Mar 08 07:29:25 2019 +0000
Revision:
6:7afdc6a81566
Parent:
5:869dc702b923
Child:
7:24a3e797e7a8
ver2

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"
sink 6:7afdc6a81566 4 #include "string"
sink 6:7afdc6a81566 5 #include "define.h"
sink 6:7afdc6a81566 6 #include "RoboClaw.h"
e5118069 0:d46cb1df87f1 7
e5118069 0:d46cb1df87f1 8 Ticker timer;
e5118069 0:d46cb1df87f1 9 Timer T;
sink 6:7afdc6a81566 10 /*QEI Enc2(p7,p8,NC,RESOLUTION,&T,QEI::X4_ENCODING);
e5118069 1:94e15665b69f 11 QEI Enc3(p5,p6,NC,RESOLUTION,&T,QEI::X4_ENCODING);
sink 6:7afdc6a81566 12 QEI Enc (p12,p11,NC,RESOLUTION,&T,QEI::X4_ENCODING);*/
sink 6:7afdc6a81566 13 RoboClaw MD(115200,p9,p10);
e5118069 3:de0b5dc55627 14 Serial Saber(p13,p14);
e5118069 3:de0b5dc55627 15 Serial pc(USBTX,USBRX);
sink 6:7afdc6a81566 16 RawSerial Master(p28,p27,115200);//(tx,rx,baud);
e5118069 1:94e15665b69f 17 Filter velfilter(INT_TIME);
e5118069 0:d46cb1df87f1 18
sink 6:7afdc6a81566 19 DigitalOut fet1(p22);//shagai把持
sink 6:7afdc6a81566 20 DigitalOut fet2(p21);//shagai押出
sink 6:7afdc6a81566 21 DigitalOut fet3(p23);//shagaiハンド昇降
e5118069 0:d46cb1df87f1 22
sink 6:7afdc6a81566 23 DigitalIn limit1(p15);//shagaiハンドlimit
e5118069 2:965cba546262 24 DigitalIn limit2(p16);
sink 6:7afdc6a81566 25 //SENSのどっちか減るかも
sink 6:7afdc6a81566 26 DigitalIn SENS1(p18);//shagai検出
e5118069 1:94e15665b69f 27 DigitalIn SENS2(p17);
e5118069 1:94e15665b69f 28
sink 6:7afdc6a81566 29 DigitalIn G_limit1(p9);//gerege limit
e5118069 5:869dc702b923 30 DigitalIn G_limit2(p10);
e5118069 5:869dc702b923 31
e5118069 2:965cba546262 32 int cmd,A;
e5118069 2:965cba546262 33 int SA1,B_SA1,LIM1,LIM2;
e5118069 2:965cba546262 34 int S1,S2;
e5118069 1:94e15665b69f 35
e5118069 1:94e15665b69f 36 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 37 float goal_D=0,Kp=5,Ki=0.01,Kd=0.1;
e5118069 4:017c55052d44 38 float Ksp2 = 6.5, Ksd2 = 0.4;
e5118069 4:017c55052d44 39 float Ksp3 = 6.5, Ksd3 = 0.4;
e5118069 1:94e15665b69f 40
sink 6:7afdc6a81566 41 //float encount,b_encount;
e5118069 1:94e15665b69f 42
sink 6:7afdc6a81566 43 char mode = 0x00;
sink 6:7afdc6a81566 44 /*int cmd2 = 0;
e5118069 0:d46cb1df87f1 45 int cmd3 = 0;
e5118069 0:d46cb1df87f1 46
e5118069 0:d46cb1df87f1 47 float spd2=0;
e5118069 0:d46cb1df87f1 48 float spd3=0;
e5118069 0:d46cb1df87f1 49
e5118069 0:d46cb1df87f1 50 float spd_err2=0;
e5118069 0:d46cb1df87f1 51 float spd_err3=0;
e5118069 0:d46cb1df87f1 52
e5118069 1:94e15665b69f 53 int tmp1;
e5118069 1:94e15665b69f 54 int tmp2;
sink 6:7afdc6a81566 55 */
sink 6:7afdc6a81566 56 double filtered_ref_qpps;
e5118069 0:d46cb1df87f1 57
e5118069 5:869dc702b923 58 int G_LIM1=0,G_LIM2=0;
e5118069 5:869dc702b923 59
e5118069 5:869dc702b923 60 int G_cmd;
e5118069 5:869dc702b923 61
sink 6:7afdc6a81566 62 int limit_MD(int qpps,int max_qpps){
sink 6:7afdc6a81566 63 if (qpps > max_qpps) qpps = max_qpps;
sink 6:7afdc6a81566 64 else if (-qpps < -max_qpps) qpps = -max_qpps;
sink 6:7afdc6a81566 65 return qpps;
sink 6:7afdc6a81566 66 }
e5118069 5:869dc702b923 67
sink 6:7afdc6a81566 68 void robo_serial(int adrs, int qpps1, int qpps2){
sink 6:7afdc6a81566 69 MD.SpeedM1(adrs,qpps1);
sink 6:7afdc6a81566 70 MD.SpeedM2(adrs,qpps2);
e5118069 0:d46cb1df87f1 71 }
sink 6:7afdc6a81566 72
sink 6:7afdc6a81566 73 void Saber_Serial (int adrs, int rot, int cmd){
sink 6:7afdc6a81566 74 Saber.putc(adrs);
sink 6:7afdc6a81566 75 Saber.putc(rot);
sink 6:7afdc6a81566 76 Saber.putc(abs(cmd));
sink 6:7afdc6a81566 77 Saber.putc((adrs + rot + abs(cmd)) & 0b01111111);
e5118069 5:869dc702b923 78 }
e5118069 5:869dc702b923 79
sink 6:7afdc6a81566 80 /*void Slave_tx(char tx_mode){ //処理の終了を送る
sink 6:7afdc6a81566 81 Master.printf("%c",tx_mode);
sink 6:7afdc6a81566 82 Master.printf("%c",tx_mode);
sink 6:7afdc6a81566 83 Master.printf("\n");
sink 6:7afdc6a81566 84 }*/
sink 6:7afdc6a81566 85
sink 6:7afdc6a81566 86 void Slave_rx() { //処理内容を受け取る
sink 6:7afdc6a81566 87 static string rx_mode = "";
sink 6:7afdc6a81566 88 char rx_c = Master.getc();
sink 6:7afdc6a81566 89
sink 6:7afdc6a81566 90 if (rx_c == '\n') {
sink 6:7afdc6a81566 91 if (rx_mode.size() == 2){
sink 6:7afdc6a81566 92 if (rx_mode[0] == rx_mode[1]){
sink 6:7afdc6a81566 93 mode = rx_mode[1]; //モード切替
sink 6:7afdc6a81566 94 }}
sink 6:7afdc6a81566 95 rx_mode = "";
sink 6:7afdc6a81566 96 }
sink 6:7afdc6a81566 97
sink 6:7afdc6a81566 98 else {
sink 6:7afdc6a81566 99 if (rx_mode.size() > 2) rx_mode = "";
sink 6:7afdc6a81566 100 rx_mode += rx_c;
sink 6:7afdc6a81566 101 }
sink 6:7afdc6a81566 102 //pc.printf("%x\n",mode);
sink 6:7afdc6a81566 103 }
e5118069 0:d46cb1df87f1 104
e5118069 0:d46cb1df87f1 105 void timer_warikomi()
e5118069 1:94e15665b69f 106 {
sink 6:7afdc6a81566 107 static int qpps1 = 0;
sink 6:7afdc6a81566 108 static int qpps2 = 0;
sink 6:7afdc6a81566 109 static int ref_qpps1 = 0;
sink 6:7afdc6a81566 110 static int ref_qpps2 = 0;
e5118069 1:94e15665b69f 111
e5118069 2:965cba546262 112 LIM1=!limit1.read();
e5118069 2:965cba546262 113 LIM2=!limit2.read();
e5118069 2:965cba546262 114 S1=SENS1.read();
e5118069 0:d46cb1df87f1 115 S2=SENS2.read();
sink 6:7afdc6a81566 116 static char slave_mode = 0x00;
sink 6:7afdc6a81566 117 static int spd_count = 0;
sink 6:7afdc6a81566 118 /*encount=Enc.getPulses()-b_encount;
e5118069 2:965cba546262 119
e5118069 2:965cba546262 120 float ppr = 1.0;
e5118069 0:d46cb1df87f1 121
e5118069 0:d46cb1df87f1 122 static float pre_spd2 = 0.0;
e5118069 0:d46cb1df87f1 123 static float pre_spd3 = 0.0;
e5118069 0:d46cb1df87f1 124
e5118069 0:d46cb1df87f1 125 static float pre_err2 = 0.0;
e5118069 0:d46cb1df87f1 126 static float pre_err3 = 0.0;
e5118069 0:d46cb1df87f1 127
e5118069 2:965cba546262 128 static float ref_spd = 0.0;
e5118069 2:965cba546262 129
e5118069 4:017c55052d44 130 static int lim_cmd2 = 87;
e5118069 2:965cba546262 131 static int lim_cmd3 = 92;
e5118069 2:965cba546262 132
sink 6:7afdc6a81566 133 static int count = 0;
sink 6:7afdc6a81566 134 static int count3 = 0;
e5118069 2:965cba546262 135
e5118069 2:965cba546262 136 angle=(float)(encount)*(360.0/48.0)/4.0;
e5118069 2:965cba546262 137 SOKUDO=(angle-pre_angle)/INT_TIME;
e5118069 2:965cba546262 138
e5118069 2:965cba546262 139 e_D=(goal_D-angle);
e5118069 2:965cba546262 140 ed_D=(e_D-pre_e_D)/INT_TIME;
e5118069 2:965cba546262 141 ei_D+=(e_D+pre_e_D)*INT_TIME/2.0;
e5118069 2:965cba546262 142
e5118069 2:965cba546262 143 cmd=(int)((e_D*Kp)+(ed_D*Kd)+(ei_D*Ki));
e5118069 2:965cba546262 144
e5118069 2:965cba546262 145 float encount2 = Enc2.getPulses();
e5118069 2:965cba546262 146 float encount3 = Enc3.getPulses();
e5118069 2:965cba546262 147
e5118069 2:965cba546262 148 float rot_sp2 = encount2/MULTIPLU/ppr;
e5118069 2:965cba546262 149 spd2 = (rot_sp2 - pre_spd2)/INT_TIME/(48*4);
e5118069 2:965cba546262 150 float rot_sp3 = encount3/MULTIPLU/ppr;
e5118069 2:965cba546262 151 spd3 = (rot_sp3 - pre_spd3)/INT_TIME/(48*4);
e5118069 0:d46cb1df87f1 152
e5118069 2:965cba546262 153 spd_err2 = filtered_ref_spd - spd2;
e5118069 2:965cba546262 154 float spd_d2 = (spd_err2 - pre_err2)/INT_TIME;
e5118069 2:965cba546262 155 tmp1 = (int)((spd_err2 * Ksp2) + (spd_d2 * Ksd2));
e5118069 2:965cba546262 156 if(tmp1>=127)tmp1=127;
e5118069 2:965cba546262 157 if(tmp1<=-127)tmp1=-127;
e5118069 2:965cba546262 158 cmd2 += tmp1;
e5118069 0:d46cb1df87f1 159
e5118069 2:965cba546262 160 spd_err3 = filtered_ref_spd - spd3;
e5118069 2:965cba546262 161 float spd_d3 = (spd_err3 - pre_err3)/INT_TIME;
e5118069 2:965cba546262 162 tmp2 = (int)((spd_err3 * Ksp3) + (spd_d3 * Ksd3));
e5118069 2:965cba546262 163 if(tmp2>=127)tmp2=127;
e5118069 2:965cba546262 164 if(tmp2<=-127)tmp2=-127;
e5118069 2:965cba546262 165 cmd3 += tmp2;
e5118069 2:965cba546262 166
e5118069 2:965cba546262 167 if (cmd2 > lim_cmd2) cmd2 = lim_cmd2;
e5118069 2:965cba546262 168 if (cmd2 < -lim_cmd2) cmd2 = -lim_cmd2;
e5118069 2:965cba546262 169
e5118069 2:965cba546262 170 if (cmd3 > lim_cmd3) cmd3 = lim_cmd3;
sink 6:7afdc6a81566 171 if (cmd3 < -lim_cmd3) cmd3 = -lim_cmd3;*/
sink 6:7afdc6a81566 172
sink 6:7afdc6a81566 173
e5118069 3:de0b5dc55627 174
sink 6:7afdc6a81566 175 G_LIM1=!G_limit1.read();//pullupなので逆
sink 6:7afdc6a81566 176 G_LIM2=!G_limit2.read();//pullupなので逆
sink 6:7afdc6a81566 177
sink 6:7afdc6a81566 178 switch (mode) {
sink 6:7afdc6a81566 179 case 0x99:
e5118069 0:d46cb1df87f1 180 break;
sink 6:7afdc6a81566 181
sink 6:7afdc6a81566 182 case 0x10://初期化処理
sink 6:7afdc6a81566 183 ref_qpps1 = 0.0;
sink 6:7afdc6a81566 184 ref_qpps2 = 0.0;
sink 6:7afdc6a81566 185
sink 6:7afdc6a81566 186 break;
sink 6:7afdc6a81566 187
sink 6:7afdc6a81566 188 case 0x20://ハンド降下
sink 6:7afdc6a81566 189 //goal_D=125;
e5118069 0:d46cb1df87f1 190 if(S1==0&&S2==0){
e5118069 3:de0b5dc55627 191 fet1=1;
e5118069 0:d46cb1df87f1 192 A=1;
e5118069 0:d46cb1df87f1 193 }
sink 6:7afdc6a81566 194 break;
sink 6:7afdc6a81566 195
sink 6:7afdc6a81566 196 case 0x30://ハンド上昇
sink 6:7afdc6a81566 197
sink 6:7afdc6a81566 198 break;
sink 6:7afdc6a81566 199
sink 6:7afdc6a81566 200 case 0x40://geregeハンド
sink 6:7afdc6a81566 201 G_cmd=120;
sink 6:7afdc6a81566 202 if(G_LIM1){
sink 6:7afdc6a81566 203 G_cmd=0;
sink 6:7afdc6a81566 204 }
sink 6:7afdc6a81566 205 break;
e5118069 0:d46cb1df87f1 206
sink 6:7afdc6a81566 207 case 0x50://geregeハンド
sink 6:7afdc6a81566 208 G_cmd=-120;
sink 6:7afdc6a81566 209 if(G_LIM2){
sink 6:7afdc6a81566 210 G_cmd=0;
sink 6:7afdc6a81566 211 slave_mode = 0x55;
sink 6:7afdc6a81566 212 }
sink 6:7afdc6a81566 213 break;
sink 6:7afdc6a81566 214
sink 6:7afdc6a81566 215 case 0x60://shagai発射
sink 6:7afdc6a81566 216 ref_qpps1 = REF_QPPS1;
sink 6:7afdc6a81566 217 ref_qpps2 = REF_QPPS2;
sink 6:7afdc6a81566 218 if (qpps1 == REF_QPPS1 && qpps2 == REF_QPPS2) {
sink 6:7afdc6a81566 219 spd_count++;
e5118069 0:d46cb1df87f1 220 }
e5118069 0:d46cb1df87f1 221 break;
sink 6:7afdc6a81566 222
sink 6:7afdc6a81566 223 case 0x90://全停止
sink 6:7afdc6a81566 224 ref_qpps1 = 0.0;
sink 6:7afdc6a81566 225 ref_qpps2 = 0.0;
sink 6:7afdc6a81566 226 //cmd = 0;
sink 6:7afdc6a81566 227 G_cmd = 0;
e5118069 1:94e15665b69f 228 break;
sink 6:7afdc6a81566 229
sink 6:7afdc6a81566 230 default://何もしない
sink 6:7afdc6a81566 231 break;
sink 6:7afdc6a81566 232 }
e5118069 5:869dc702b923 233
sink 6:7afdc6a81566 234 //Slave_tx(slave_mode);
sink 6:7afdc6a81566 235 //二次遅れ系
sink 6:7afdc6a81566 236 if(qpps1>=QPPS_HIGH_RANGE1 && ref_qpps1 == REF_QPPS1) qpps1=ref_qpps1;
sink 6:7afdc6a81566 237 else if(qpps1<=QPPS_HIGH_STOP && ref_qpps1 == 0) qpps1=0;
sink 6:7afdc6a81566 238 else qpps1 = velfilter.SecondOrderLag((double)ref_qpps1);
sink 6:7afdc6a81566 239 if(qpps2>=QPPS_HIGH_RANGE2 && ref_qpps2 == REF_QPPS2) qpps2=ref_qpps2;
sink 6:7afdc6a81566 240 else if(qpps2<=QPPS_HIGH_STOP && ref_qpps2 == 0) qpps2=0;
sink 6:7afdc6a81566 241 else qpps2 = velfilter.SecondOrderLag((double)ref_qpps2);
sink 6:7afdc6a81566 242 //ローラー強制停止
sink 6:7afdc6a81566 243 /*if (qpps <= 5.0 && ref_qpps == 0) {
sink 6:7afdc6a81566 244 cmd3 = 0;
sink 6:7afdc6a81566 245 }*/
sink 6:7afdc6a81566 246 //自動押し出し
sink 6:7afdc6a81566 247 if (spd_count >= 40){
sink 6:7afdc6a81566 248 fet2 = 0;
sink 6:7afdc6a81566 249 if (slave_mode != 0x60) spd_count = 0;
sink 6:7afdc6a81566 250 }
sink 6:7afdc6a81566 251 /*
sink 6:7afdc6a81566 252 if(angle>=124 && goal_D >= 120){
sink 6:7afdc6a81566 253 cmd=0;
sink 6:7afdc6a81566 254 goal_D = angle;
sink 6:7afdc6a81566 255 }
e5118069 5:869dc702b923 256
sink 6:7afdc6a81566 257 if(angle <= 2 && goal_D <= 10) {
sink 6:7afdc6a81566 258 cmd = 0;
sink 6:7afdc6a81566 259 goal_D = angle;
e5118069 4:017c55052d44 260 }
e5118069 1:94e15665b69f 261
e5118069 3:de0b5dc55627 262 if(cmd>20) cmd=20;
sink 6:7afdc6a81566 263 if(cmd<-15)cmd=-15;
e5118069 1:94e15665b69f 264
sink 6:7afdc6a81566 265 if(cmd>=0) Saber_Serial (SABER_ADDR, 1, cmd);
sink 6:7afdc6a81566 266 else Saber_Serial (SABER_ADDR, 0, cmd);
sink 6:7afdc6a81566 267 */
sink 6:7afdc6a81566 268 qpps1 = limit_MD( qpps1, MAX_QPPS1);
sink 6:7afdc6a81566 269 qpps2 = limit_MD( qpps2, MAX_QPPS2);
e5118069 0:d46cb1df87f1 270
sink 6:7afdc6a81566 271 robo_serial(ROBO_ADRS, qpps1, qpps2);
e5118069 0:d46cb1df87f1 272
sink 6:7afdc6a81566 273 if (G_cmd > 0) Saber_Serial (SABER_ADDR, 4, G_cmd);
sink 6:7afdc6a81566 274 else Saber_Serial (SABER_ADDR, 5, G_cmd);
sink 6:7afdc6a81566 275 /*
sink 6:7afdc6a81566 276 if (cmd3 > 0) Saber_Serial (SB_ADRS, 0, cmd3);
sink 6:7afdc6a81566 277 else Saber_Serial (SB_ADRS, 1, cmd3);
sink 6:7afdc6a81566 278
sink 6:7afdc6a81566 279 if (cmd2 > 0) Saber_Serial (SB_ADRS, 4, cmd2);
sink 6:7afdc6a81566 280 else Saber_Serial (SB_ADRS, 5, cmd2);*/
e5118069 0:d46cb1df87f1 281
sink 6:7afdc6a81566 282 /*pre_spd2 = rot_sp2;
sink 6:7afdc6a81566 283 pre_err2 = spd_err2;
sink 6:7afdc6a81566 284 pre_spd3 = rot_sp3;
sink 6:7afdc6a81566 285 pre_err3 = spd_err3;
sink 6:7afdc6a81566 286 pre_e_D = e_D;
sink 6:7afdc6a81566 287 pre_angle = angle;
sink 6:7afdc6a81566 288 pre_e_V = e_V;
sink 6:7afdc6a81566 289 B_SA1 = SA1;*/
e5118069 0:d46cb1df87f1 290 }
e5118069 0:d46cb1df87f1 291
e5118069 0:d46cb1df87f1 292
e5118069 0:d46cb1df87f1 293 int main() {
e5118069 1:94e15665b69f 294 Saber.baud(19200);
e5118069 0:d46cb1df87f1 295 pc.baud(9600);
e5118069 3:de0b5dc55627 296 fet1=0;
e5118069 2:965cba546262 297 fet2=1;
sink 6:7afdc6a81566 298 fet3=0;
e5118069 2:965cba546262 299
e5118069 5:869dc702b923 300 G_limit1.mode( PullUp ); // 内蔵プルアップを使う
e5118069 5:869dc702b923 301 G_limit2.mode( PullUp );
e5118069 5:869dc702b923 302
e5118069 1:94e15665b69f 303 velfilter.setSecondOrderPara(1.0, 0.9, 0.0);
e5118069 1:94e15665b69f 304
sink 6:7afdc6a81566 305 Master.attach(&Slave_rx, RawSerial::RxIrq);
e5118069 0:d46cb1df87f1 306 timer.attach(timer_warikomi,INT_TIME);
e5118069 0:d46cb1df87f1 307
e5118069 0:d46cb1df87f1 308 while(1) {
sink 6:7afdc6a81566 309 pc.printf("%lf\n", filtered_ref_qpps);
e5118069 0:d46cb1df87f1 310 }
e5118069 1:94e15665b69f 311 }