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