手動bのほぼ全部
Dependents: kobayashi_rei 2019BSyudo Bsyudo 2019Bsyudo
states.cpp@8:8ff6db217a17, 2019-10-04 (annotated)
- Committer:
- THtakahiro702286
- Date:
- Fri Oct 04 08:15:43 2019 +0000
- Revision:
- 8:8ff6db217a17
- Parent:
- 6:3cb78320d4db
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 | 4:eafd5a0c3351 | 8 | emStop(STOP), |
THtakahiro702286 | 4:eafd5a0c3351 | 9 | mechanism(&serial) |
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 | 6:3cb78320d4db | 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 | 6:3cb78320d4db | 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 | 6:3cb78320d4db | 36 | stick[0] -= 128; |
THtakahiro702286 | 6:3cb78320d4db | 37 | stick[1] = 128-stick[1]; |
THtakahiro702286 | 6:3cb78320d4db | 38 | norm[0] = hypotf(stick[0],stick[1]); |
THtakahiro702286 | 8:8ff6db217a17 | 39 | if(norm[0] < 50) norm[0] = 0; |
THtakahiro702286 | 4:eafd5a0c3351 | 40 | for(int i=0; i<2; i++) trigger[i] = pad.getTrigger(i); |
THtakahiro702286 | 6:3cb78320d4db | 41 | mechanism.setBrake(trigger[0]); |
THtakahiro702286 | 6:3cb78320d4db | 42 | |
THtakahiro702286 | 4:eafd5a0c3351 | 43 | mechanism.getTrigger(trigger[1]); |
THtakahiro702286 | 6:3cb78320d4db | 44 | if(b3[5] == 1) state++; |
THtakahiro702286 | 6:3cb78320d4db | 45 | if(b3[4] == 1) state--; |
THtakahiro702286 | 0:8d012d469eb4 | 46 | /* |
THtakahiro702286 | 6:3cb78320d4db | 47 | *ロボの方角 |
THtakahiro702286 | 0:8d012d469eb4 | 48 | * "front" or 0 で前 |
THtakahiro702286 | 0:8d012d469eb4 | 49 | * "right" or 90 で右 |
THtakahiro702286 | 0:8d012d469eb4 | 50 | * "back" or 180 で後ろ |
THtakahiro702286 | 0:8d012d469eb4 | 51 | * "left" or -90 で左 |
THtakahiro702286 | 0:8d012d469eb4 | 52 | */ |
THtakahiro702286 | 0:8d012d469eb4 | 53 | switch(state % 4){ |
THtakahiro702286 | 0:8d012d469eb4 | 54 | case 1: idealAngle = right; break; |
THtakahiro702286 | 0:8d012d469eb4 | 55 | case 2: idealAngle = back; break; |
THtakahiro702286 | 0:8d012d469eb4 | 56 | case 3: idealAngle = left; break; |
THtakahiro702286 | 0:8d012d469eb4 | 57 | default: idealAngle = front; break; |
THtakahiro702286 | 4:eafd5a0c3351 | 58 | } |
THtakahiro702286 | 0:8d012d469eb4 | 59 | } |
THtakahiro702286 | 0:8d012d469eb4 | 60 | |
THtakahiro702286 | 0:8d012d469eb4 | 61 | void States::move() |
THtakahiro702286 | 0:8d012d469eb4 | 62 | { |
THtakahiro702286 | 6:3cb78320d4db | 63 | while(1){ |
THtakahiro702286 | 6:3cb78320d4db | 64 | tellPad(); |
THtakahiro702286 | 0:8d012d469eb4 | 65 | tellAngle(); |
THtakahiro702286 | 6:3cb78320d4db | 66 | if(b[10]) emStop = true; |
THtakahiro702286 | 6:3cb78320d4db | 67 | if(b[11]) emStop = false; |
THtakahiro702286 | 6:3cb78320d4db | 68 | if(!pad.status){ |
THtakahiro702286 | 6:3cb78320d4db | 69 | /*mechanism.stop()は止める*/ |
THtakahiro702286 | 6:3cb78320d4db | 70 | mechanism.stop(); |
THtakahiro702286 | 6:3cb78320d4db | 71 | emStop = false; |
THtakahiro702286 | 6:3cb78320d4db | 72 | }else{ |
THtakahiro702286 | 6:3cb78320d4db | 73 | turnPower = anglePID.compute(); |
THtakahiro702286 | 8:8ff6db217a17 | 74 | //if(norm[0] < 70) mechanism.move(0,0,-turnPower,nowAngle, 0); |
THtakahiro702286 | 8:8ff6db217a17 | 75 | /*else*/ mechanism.move(stick[0], stick[1], -turnPower, nowAngle, norm[0]); |
THtakahiro702286 | 6:3cb78320d4db | 76 | /*昇降機構*/ |
THtakahiro702286 | 6:3cb78320d4db | 77 | if(b[0]) {mechanism.up();} |
THtakahiro702286 | 6:3cb78320d4db | 78 | else if(b[1]) {mechanism.down();} |
THtakahiro702286 | 6:3cb78320d4db | 79 | /*回収機構*/ |
THtakahiro702286 | 6:3cb78320d4db | 80 | else if(b[6]) {mechanism.collect();} |
THtakahiro702286 | 6:3cb78320d4db | 81 | /*つり上げ機構*/ |
THtakahiro702286 | 6:3cb78320d4db | 82 | else if(b[7]) {mechanism.lift();} |
THtakahiro702286 | 6:3cb78320d4db | 83 | /*装填機構*/ |
THtakahiro702286 | 6:3cb78320d4db | 84 | else if(b[2]) {mechanism.loadRight();} |
THtakahiro702286 | 6:3cb78320d4db | 85 | else if(b[3]) {mechanism.loadLeft();} |
THtakahiro702286 | 6:3cb78320d4db | 86 | /*かけ機構*/ |
THtakahiro702286 | 6:3cb78320d4db | 87 | else if(b[9]) {mechanism.hookRight();} |
THtakahiro702286 | 6:3cb78320d4db | 88 | else if(b[8]) {mechanism.hookLeft();} |
THtakahiro702286 | 6:3cb78320d4db | 89 | |
THtakahiro702286 | 6:3cb78320d4db | 90 | else mechanism.noAct(); |
THtakahiro702286 | 6:3cb78320d4db | 91 | } |
THtakahiro702286 | 0:8d012d469eb4 | 92 | } |
THtakahiro702286 | 4:eafd5a0c3351 | 93 | } |