手動bのほぼ全部
Dependents: kobayashi_rei 2019BSyudo Bsyudo 2019Bsyudo
states.cpp@5:3273db2411d8, 2019-09-10 (annotated)
- Committer:
- THtakahiro702286
- Date:
- Tue Sep 10 01:49:31 2019 +0000
- Revision:
- 5:3273db2411d8
- Parent:
- 4:eafd5a0c3351
- Child:
- 7:9d5746931f80
states;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
THtakahiro702286 | 0:8d012d469eb4 | 1 | #include "states.h" |
THtakahiro702286 | 0:8d012d469eb4 | 2 | |
THtakahiro702286 | 0:8d012d469eb4 | 3 | States::States(): |
THtakahiro702286 | 0:8d012d469eb4 | 4 | anglePID(KP, KI, KD,0.001), |
THtakahiro702286 | 4:eafd5a0c3351 | 5 | pad(padTX,padRX), |
THtakahiro702286 | 0:8d012d469eb4 | 6 | gyro(GYROTX,GYRORX), |
THtakahiro702286 | 0:8d012d469eb4 | 7 | serial(MDTX,MDRX,MDBAUD), |
THtakahiro702286 | 1:38dd7564c15f | 8 | omni(&serial), |
THtakahiro702286 | 4:eafd5a0c3351 | 9 | emStop(STOP), |
THtakahiro702286 | 4:eafd5a0c3351 | 10 | mechanism(&serial) |
THtakahiro702286 | 0:8d012d469eb4 | 11 | { |
THtakahiro702286 | 0:8d012d469eb4 | 12 | anglePID.setMode(AUTO_MODE); |
THtakahiro702286 | 0:8d012d469eb4 | 13 | anglePID.setInputLimits(-360, 360); |
THtakahiro702286 | 0:8d012d469eb4 | 14 | anglePID.setOutputLimits(minLimit, maxLimit); |
THtakahiro702286 | 0:8d012d469eb4 | 15 | anglePID.setSetPoint(0.0); |
THtakahiro702286 | 0:8d012d469eb4 | 16 | anglePID.setBias(0.0); |
THtakahiro702286 | 0:8d012d469eb4 | 17 | state = 400; |
THtakahiro702286 | 4:eafd5a0c3351 | 18 | check = 0; |
THtakahiro702286 | 3:2b909340be04 | 19 | emStop = false; |
THtakahiro702286 | 0:8d012d469eb4 | 20 | } |
THtakahiro702286 | 0:8d012d469eb4 | 21 | |
THtakahiro702286 | 0:8d012d469eb4 | 22 | void States::tellAngle(){ |
THtakahiro702286 | 0:8d012d469eb4 | 23 | gyro.update(); |
THtakahiro702286 | 0:8d012d469eb4 | 24 | nowAngle = gyro.getAngle(); |
THtakahiro702286 | 0:8d012d469eb4 | 25 | deviation = nowAngle - idealAngle; |
THtakahiro702286 | 0:8d012d469eb4 | 26 | if(deviation > 180) deviation -= 360; |
THtakahiro702286 | 0:8d012d469eb4 | 27 | if(deviation < -180) deviation += 360; |
THtakahiro702286 | 0:8d012d469eb4 | 28 | anglePID.setProcessValue(deviation); |
THtakahiro702286 | 0:8d012d469eb4 | 29 | } |
THtakahiro702286 | 0:8d012d469eb4 | 30 | |
THtakahiro702286 | 0:8d012d469eb4 | 31 | void States::tellPad(){ |
THtakahiro702286 | 0:8d012d469eb4 | 32 | for(int i=0; i<13; i++){ |
THtakahiro702286 | 4:eafd5a0c3351 | 33 | b[i] = pad.getButton(i); |
THtakahiro702286 | 0:8d012d469eb4 | 34 | b3[i] = b2[i] - b[i]; |
THtakahiro702286 | 0:8d012d469eb4 | 35 | b2[i] = b[i]; |
THtakahiro702286 | 4:eafd5a0c3351 | 36 | check += b[i]; |
THtakahiro702286 | 0:8d012d469eb4 | 37 | } |
THtakahiro702286 | 4:eafd5a0c3351 | 38 | for(int i=0; i<4; i++) stick[i] = pad.getStick(i); |
THtakahiro702286 | 4:eafd5a0c3351 | 39 | stick[1] = 255-stick[1]; |
THtakahiro702286 | 4:eafd5a0c3351 | 40 | stick[3] = 255-stick[3]; |
THtakahiro702286 | 5:3273db2411d8 | 41 | norm[0] = hypotf(stick[0],stick[1]); |
THtakahiro702286 | 5:3273db2411d8 | 42 | norm[1] = hypotf(stick[2],stick[3]); |
THtakahiro702286 | 4:eafd5a0c3351 | 43 | if(norm[0] > 20) norm[0] = 0; |
THtakahiro702286 | 4:eafd5a0c3351 | 44 | if(norm[1] > 20) norm[1] = 0; |
THtakahiro702286 | 4:eafd5a0c3351 | 45 | for(int i=0; i<2; i++) trigger[i] = pad.getTrigger(i); |
THtakahiro702286 | 4:eafd5a0c3351 | 46 | omni.setBrake(trigger[0]); |
THtakahiro702286 | 4:eafd5a0c3351 | 47 | nano.sendTrigger(trigger[0]); |
THtakahiro702286 | 4:eafd5a0c3351 | 48 | mechanism.getTrigger(trigger[1]); |
THtakahiro702286 | 5:3273db2411d8 | 49 | rad[0] = atan2f(stick[1],stick[0]) + (nowAngle * (PI / 180)); |
THtakahiro702286 | 5:3273db2411d8 | 50 | rad[1] = atan2f(stick[2],stick[3]); |
THtakahiro702286 | 4:eafd5a0c3351 | 51 | if(b3[4] == 1) state++; |
THtakahiro702286 | 4:eafd5a0c3351 | 52 | if(b3[5] == 1) state--; |
THtakahiro702286 | 0:8d012d469eb4 | 53 | /* |
THtakahiro702286 | 0:8d012d469eb4 | 54 | *move.tellState(方角)でロボの方角 |
THtakahiro702286 | 0:8d012d469eb4 | 55 | * "front" or 0 で前 |
THtakahiro702286 | 0:8d012d469eb4 | 56 | * "right" or 90 で右 |
THtakahiro702286 | 0:8d012d469eb4 | 57 | * "back" or 180 で後ろ |
THtakahiro702286 | 0:8d012d469eb4 | 58 | * "left" or -90 で左 |
THtakahiro702286 | 0:8d012d469eb4 | 59 | */ |
THtakahiro702286 | 0:8d012d469eb4 | 60 | switch(state % 4){ |
THtakahiro702286 | 0:8d012d469eb4 | 61 | case 1: idealAngle = right; break; |
THtakahiro702286 | 0:8d012d469eb4 | 62 | case 2: idealAngle = back; break; |
THtakahiro702286 | 0:8d012d469eb4 | 63 | case 3: idealAngle = left; break; |
THtakahiro702286 | 0:8d012d469eb4 | 64 | default: idealAngle = front; break; |
THtakahiro702286 | 4:eafd5a0c3351 | 65 | } |
THtakahiro702286 | 0:8d012d469eb4 | 66 | } |
THtakahiro702286 | 0:8d012d469eb4 | 67 | |
THtakahiro702286 | 0:8d012d469eb4 | 68 | void States::move() |
THtakahiro702286 | 0:8d012d469eb4 | 69 | { |
THtakahiro702286 | 4:eafd5a0c3351 | 70 | tellPad(); |
THtakahiro702286 | 4:eafd5a0c3351 | 71 | if(b[10]) emStop = true; |
THtakahiro702286 | 4:eafd5a0c3351 | 72 | if(b[11]) emStop = false; |
THtakahiro702286 | 4:eafd5a0c3351 | 73 | nano.sendHeader(); |
THtakahiro702286 | 4:eafd5a0c3351 | 74 | if(emStop == false)nano.sendMove(0xff); |
THtakahiro702286 | 4:eafd5a0c3351 | 75 | else if(!pad.status){ |
THtakahiro702286 | 0:8d012d469eb4 | 76 | /*omni.stop()は止める*/ |
THtakahiro702286 | 0:8d012d469eb4 | 77 | omni.stop(); |
THtakahiro702286 | 4:eafd5a0c3351 | 78 | emStop = false; |
THtakahiro702286 | 0:8d012d469eb4 | 79 | }else{ |
THtakahiro702286 | 0:8d012d469eb4 | 80 | tellAngle(); |
THtakahiro702286 | 0:8d012d469eb4 | 81 | turnPower = anglePID.compute(); |
THtakahiro702286 | 4:eafd5a0c3351 | 82 | if(norm[0] && norm[0] < 200) omni.stop(); |
THtakahiro702286 | 4:eafd5a0c3351 | 83 | else omni.move(stick[0], stick[1], -turnPower); |
THtakahiro702286 | 5:3273db2411d8 | 84 | /*昇降機構*/ |
THtakahiro702286 | 5:3273db2411d8 | 85 | if(b[0] && check == 1) mechanism.up(); |
THtakahiro702286 | 5:3273db2411d8 | 86 | else if(b[1] && check == 1) mechanism.down(); |
THtakahiro702286 | 5:3273db2411d8 | 87 | /*回収機構*/ |
THtakahiro702286 | 5:3273db2411d8 | 88 | else if(b[6] && check == 1) mechanism.collect(); |
THtakahiro702286 | 5:3273db2411d8 | 89 | /*つり上げ機構*/ |
THtakahiro702286 | 5:3273db2411d8 | 90 | else if(b[7] && check == 1) mechanism.lift(); |
THtakahiro702286 | 5:3273db2411d8 | 91 | /*装填機構*/ |
THtakahiro702286 | 5:3273db2411d8 | 92 | else if(b[2] && check == 1) mechanism.loadRight(); |
THtakahiro702286 | 5:3273db2411d8 | 93 | else if(b[3]) mechanism.loadLeft(); |
THtakahiro702286 | 5:3273db2411d8 | 94 | /*かけ機構*/ |
THtakahiro702286 | 5:3273db2411d8 | 95 | else if(b[9] && check == 1) mechanism.hookRight(); |
THtakahiro702286 | 5:3273db2411d8 | 96 | else if(b[8] && check == 1) mechanism.hookLeft(); |
THtakahiro702286 | 5:3273db2411d8 | 97 | |
THtakahiro702286 | 5:3273db2411d8 | 98 | else nano.sendMove(0x0f); |
THtakahiro702286 | 0:8d012d469eb4 | 99 | } |
THtakahiro702286 | 4:eafd5a0c3351 | 100 | } |