手動bのほぼ全部
Dependents: kobayashi_rei 2019BSyudo Bsyudo 2019Bsyudo
states.cpp@7:9d5746931f80, 2019-09-13 (annotated)
- Committer:
- THtakahiro702286
- Date:
- Fri Sep 13 03:22:42 2019 +0000
- Revision:
- 7:9d5746931f80
- Parent:
- 5:3273db2411d8
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 | 7:9d5746931f80 | 4 | emStop(STOP), |
THtakahiro702286 | 0:8d012d469eb4 | 5 | serial(MDTX,MDRX,MDBAUD), |
THtakahiro702286 | 7:9d5746931f80 | 6 | gyro(GYROTX,GYRORX), |
THtakahiro702286 | 7:9d5746931f80 | 7 | anglePID(KP, KI, KD,0.001), |
THtakahiro702286 | 7:9d5746931f80 | 8 | mechanism(&serial), |
THtakahiro702286 | 7:9d5746931f80 | 9 | pad(padTX,padRX) |
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 | state = 400; |
THtakahiro702286 | 3:2b909340be04 | 17 | emStop = false; |
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 | anglePID.setProcessValue(deviation); |
THtakahiro702286 | 0:8d012d469eb4 | 27 | } |
THtakahiro702286 | 0:8d012d469eb4 | 28 | |
THtakahiro702286 | 0:8d012d469eb4 | 29 | void States::tellPad(){ |
THtakahiro702286 | 7:9d5746931f80 | 30 | for(int i=0; i<12; i++){ |
THtakahiro702286 | 4:eafd5a0c3351 | 31 | b[i] = pad.getButton(i); |
THtakahiro702286 | 0:8d012d469eb4 | 32 | b3[i] = b2[i] - b[i]; |
THtakahiro702286 | 7:9d5746931f80 | 33 | b2[i] = b[i]; |
THtakahiro702286 | 0:8d012d469eb4 | 34 | } |
THtakahiro702286 | 4:eafd5a0c3351 | 35 | for(int i=0; i<4; i++) stick[i] = pad.getStick(i); |
THtakahiro702286 | 7:9d5746931f80 | 36 | stick[0] -= 128; |
THtakahiro702286 | 7:9d5746931f80 | 37 | stick[1] = 128-stick[1]; |
THtakahiro702286 | 7:9d5746931f80 | 38 | stick[2] -= 128; |
THtakahiro702286 | 7:9d5746931f80 | 39 | stick[3] = 128-stick[3]; |
THtakahiro702286 | 5:3273db2411d8 | 40 | norm[0] = hypotf(stick[0],stick[1]); |
THtakahiro702286 | 5:3273db2411d8 | 41 | norm[1] = hypotf(stick[2],stick[3]); |
THtakahiro702286 | 4:eafd5a0c3351 | 42 | if(norm[0] > 20) norm[0] = 0; |
THtakahiro702286 | 4:eafd5a0c3351 | 43 | if(norm[1] > 20) norm[1] = 0; |
THtakahiro702286 | 4:eafd5a0c3351 | 44 | for(int i=0; i<2; i++) trigger[i] = pad.getTrigger(i); |
THtakahiro702286 | 7:9d5746931f80 | 45 | mechanism.setBrake(trigger[0]); |
THtakahiro702286 | 7:9d5746931f80 | 46 | |
THtakahiro702286 | 4:eafd5a0c3351 | 47 | mechanism.getTrigger(trigger[1]); |
THtakahiro702286 | 7:9d5746931f80 | 48 | if(b3[5] == 1) state++; |
THtakahiro702286 | 7:9d5746931f80 | 49 | if(b3[4] == 1) state--; |
THtakahiro702286 | 0:8d012d469eb4 | 50 | /* |
THtakahiro702286 | 7:9d5746931f80 | 51 | *ロボの方角 |
THtakahiro702286 | 0:8d012d469eb4 | 52 | * "front" or 0 で前 |
THtakahiro702286 | 0:8d012d469eb4 | 53 | * "right" or 90 で右 |
THtakahiro702286 | 0:8d012d469eb4 | 54 | * "back" or 180 で後ろ |
THtakahiro702286 | 0:8d012d469eb4 | 55 | * "left" or -90 で左 |
THtakahiro702286 | 0:8d012d469eb4 | 56 | */ |
THtakahiro702286 | 0:8d012d469eb4 | 57 | switch(state % 4){ |
THtakahiro702286 | 0:8d012d469eb4 | 58 | case 1: idealAngle = right; break; |
THtakahiro702286 | 0:8d012d469eb4 | 59 | case 2: idealAngle = back; break; |
THtakahiro702286 | 0:8d012d469eb4 | 60 | case 3: idealAngle = left; break; |
THtakahiro702286 | 0:8d012d469eb4 | 61 | default: idealAngle = front; break; |
THtakahiro702286 | 4:eafd5a0c3351 | 62 | } |
THtakahiro702286 | 0:8d012d469eb4 | 63 | } |
THtakahiro702286 | 0:8d012d469eb4 | 64 | |
THtakahiro702286 | 0:8d012d469eb4 | 65 | void States::move() |
THtakahiro702286 | 0:8d012d469eb4 | 66 | { |
THtakahiro702286 | 7:9d5746931f80 | 67 | while(1){ |
THtakahiro702286 | 7:9d5746931f80 | 68 | tellPad(); |
THtakahiro702286 | 0:8d012d469eb4 | 69 | tellAngle(); |
THtakahiro702286 | 7:9d5746931f80 | 70 | if(b[10]) emStop = true; |
THtakahiro702286 | 7:9d5746931f80 | 71 | if(b[11]) emStop = false; |
THtakahiro702286 | 7:9d5746931f80 | 72 | if(!pad.status){ |
THtakahiro702286 | 7:9d5746931f80 | 73 | /*mechanism.stop()は止める*/ |
THtakahiro702286 | 7:9d5746931f80 | 74 | mechanism.stop(); |
THtakahiro702286 | 7:9d5746931f80 | 75 | emStop = false; |
THtakahiro702286 | 7:9d5746931f80 | 76 | }else{ |
THtakahiro702286 | 7:9d5746931f80 | 77 | turnPower = anglePID.compute(); |
THtakahiro702286 | 7:9d5746931f80 | 78 | if(norm[0] && norm[0] < 100) mechanism.move(0,0,-turnPower); |
THtakahiro702286 | 7:9d5746931f80 | 79 | else mechanism.move(stick[0], stick[1], -turnPower); |
THtakahiro702286 | 7:9d5746931f80 | 80 | /*昇降機構*/ |
THtakahiro702286 | 7:9d5746931f80 | 81 | if(b[0]) {mechanism.up();} |
THtakahiro702286 | 7:9d5746931f80 | 82 | else if(b[1]) {mechanism.down();} |
THtakahiro702286 | 7:9d5746931f80 | 83 | /*回収機構*/ |
THtakahiro702286 | 7:9d5746931f80 | 84 | else if(b[6]) {mechanism.collect();} |
THtakahiro702286 | 7:9d5746931f80 | 85 | /*つり上げ機構*/ |
THtakahiro702286 | 7:9d5746931f80 | 86 | else if(b[7]) {mechanism.lift();} |
THtakahiro702286 | 7:9d5746931f80 | 87 | /*装填機構*/ |
THtakahiro702286 | 7:9d5746931f80 | 88 | else if(b[2]) {mechanism.loadRight();} |
THtakahiro702286 | 7:9d5746931f80 | 89 | else if(b[3]) {mechanism.loadLeft();} |
THtakahiro702286 | 7:9d5746931f80 | 90 | /*かけ機構*/ |
THtakahiro702286 | 7:9d5746931f80 | 91 | else if(b[9]) {mechanism.hookRight();} |
THtakahiro702286 | 7:9d5746931f80 | 92 | else if(b[8]) {mechanism.hookLeft();} |
THtakahiro702286 | 7:9d5746931f80 | 93 | |
THtakahiro702286 | 7:9d5746931f80 | 94 | else mechanism.noAct(); |
THtakahiro702286 | 7:9d5746931f80 | 95 | } |
THtakahiro702286 | 0:8d012d469eb4 | 96 | } |
THtakahiro702286 | 4:eafd5a0c3351 | 97 | } |