手動bのほぼ全部
Dependents: kobayashi_rei 2019BSyudo Bsyudo 2019Bsyudo
states.cpp@1:38dd7564c15f, 2019-07-10 (annotated)
- Committer:
- THtakahiro702286
- Date:
- Wed Jul 10 07:38:28 2019 +0000
- Revision:
- 1:38dd7564c15f
- Parent:
- 0:8d012d469eb4
- Child:
- 2:860eb70d3656
new plate
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 | 0:8d012d469eb4 | 5 | pad(padTX,padRX,ADDRESS), |
THtakahiro702286 | 0:8d012d469eb4 | 6 | gyro(GYROTX,GYRORX), |
THtakahiro702286 | 0:8d012d469eb4 | 7 | serial(MDTX,MDRX,MDBAUD), |
THtakahiro702286 | 1:38dd7564c15f | 8 | omni(&serial), |
THtakahiro702286 | 1:38dd7564c15f | 9 | emStop(STOP) |
THtakahiro702286 | 0:8d012d469eb4 | 10 | { |
THtakahiro702286 | 0:8d012d469eb4 | 11 | anglePID.setMode(AUTO_MODE); |
THtakahiro702286 | 0:8d012d469eb4 | 12 | anglePID.setInputLimits(-360, 360); |
THtakahiro702286 | 0:8d012d469eb4 | 13 | anglePID.setOutputLimits(minLimit, maxLimit); |
THtakahiro702286 | 0:8d012d469eb4 | 14 | anglePID.setSetPoint(0.0); |
THtakahiro702286 | 0:8d012d469eb4 | 15 | anglePID.setBias(0.0); |
THtakahiro702286 | 0:8d012d469eb4 | 16 | |
THtakahiro702286 | 0:8d012d469eb4 | 17 | state = 400; |
THtakahiro702286 | 0:8d012d469eb4 | 18 | } |
THtakahiro702286 | 0:8d012d469eb4 | 19 | |
THtakahiro702286 | 0:8d012d469eb4 | 20 | |
THtakahiro702286 | 0:8d012d469eb4 | 21 | void States::tellAngle(){ |
THtakahiro702286 | 0:8d012d469eb4 | 22 | gyro.update(); |
THtakahiro702286 | 0:8d012d469eb4 | 23 | nowAngle = gyro.getAngle(); |
THtakahiro702286 | 0:8d012d469eb4 | 24 | deviation = nowAngle - idealAngle; |
THtakahiro702286 | 0:8d012d469eb4 | 25 | if(deviation > 180) deviation -= 360; |
THtakahiro702286 | 0:8d012d469eb4 | 26 | if(deviation < -180) deviation += 360; |
THtakahiro702286 | 0:8d012d469eb4 | 27 | |
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 | 0:8d012d469eb4 | 33 | b[i] = 1 - pad.getButton1(i); |
THtakahiro702286 | 0:8d012d469eb4 | 34 | b3[i] = b2[i] - b[i]; |
THtakahiro702286 | 0:8d012d469eb4 | 35 | b2[i] = b[i]; |
THtakahiro702286 | 0:8d012d469eb4 | 36 | } |
THtakahiro702286 | 0:8d012d469eb4 | 37 | stickRad = PI - pad.getRadian(0) + (nowAngle * (PI / 180)); |
THtakahiro702286 | 0:8d012d469eb4 | 38 | norm = pad.getNorm(0); |
THtakahiro702286 | 0:8d012d469eb4 | 39 | X = norm * cos(stickRad); |
THtakahiro702286 | 0:8d012d469eb4 | 40 | Y = norm * sin(stickRad); |
THtakahiro702286 | 0:8d012d469eb4 | 41 | if(b3[0] == 1) state++; |
THtakahiro702286 | 0:8d012d469eb4 | 42 | if(b3[1] == 1) state--; |
THtakahiro702286 | 0:8d012d469eb4 | 43 | |
THtakahiro702286 | 0:8d012d469eb4 | 44 | /* |
THtakahiro702286 | 0:8d012d469eb4 | 45 | *move.tellState(方角)でロボの方角 |
THtakahiro702286 | 0:8d012d469eb4 | 46 | * "front" or 0 で前 |
THtakahiro702286 | 0:8d012d469eb4 | 47 | * "right" or 90 で右 |
THtakahiro702286 | 0:8d012d469eb4 | 48 | * "back" or 180 で後ろ |
THtakahiro702286 | 0:8d012d469eb4 | 49 | * "left" or -90 で左 |
THtakahiro702286 | 0:8d012d469eb4 | 50 | */ |
THtakahiro702286 | 0:8d012d469eb4 | 51 | switch(state % 4){ |
THtakahiro702286 | 0:8d012d469eb4 | 52 | case 1: idealAngle = right; break; |
THtakahiro702286 | 0:8d012d469eb4 | 53 | case 2: idealAngle = back; break; |
THtakahiro702286 | 0:8d012d469eb4 | 54 | case 3: idealAngle = left; break; |
THtakahiro702286 | 0:8d012d469eb4 | 55 | default: idealAngle = front; break; |
THtakahiro702286 | 0:8d012d469eb4 | 56 | } |
THtakahiro702286 | 0:8d012d469eb4 | 57 | } |
THtakahiro702286 | 0:8d012d469eb4 | 58 | |
THtakahiro702286 | 0:8d012d469eb4 | 59 | void States::move() |
THtakahiro702286 | 0:8d012d469eb4 | 60 | { |
THtakahiro702286 | 0:8d012d469eb4 | 61 | if(pad.receiveState() == 0){ |
THtakahiro702286 | 0:8d012d469eb4 | 62 | /*omni.stop()は止める*/ |
THtakahiro702286 | 0:8d012d469eb4 | 63 | omni.stop(); |
THtakahiro702286 | 1:38dd7564c15f | 64 | emStop = false; |
THtakahiro702286 | 0:8d012d469eb4 | 65 | }else{ |
THtakahiro702286 | 1:38dd7564c15f | 66 | emStop = true; |
THtakahiro702286 | 0:8d012d469eb4 | 67 | tellAngle(); |
THtakahiro702286 | 0:8d012d469eb4 | 68 | tellPad(); |
THtakahiro702286 | 0:8d012d469eb4 | 69 | // tellState(); |
THtakahiro702286 | 0:8d012d469eb4 | 70 | turnPower = anglePID.compute(); |
THtakahiro702286 | 0:8d012d469eb4 | 71 | omni.move(X, Y, -turnPower); |
THtakahiro702286 | 0:8d012d469eb4 | 72 | } |
THtakahiro702286 | 0:8d012d469eb4 | 73 | } |