手動bのほぼ全部
Dependents: kobayashi_rei 2019BSyudo Bsyudo 2019Bsyudo
Diff: states.cpp
- Revision:
- 4:eafd5a0c3351
- Parent:
- 3:2b909340be04
- Child:
- 5:3273db2411d8
- Child:
- 6:3cb78320d4db
diff -r 2b909340be04 -r eafd5a0c3351 states.cpp --- a/states.cpp Thu Jul 11 09:19:10 2019 +0000 +++ b/states.cpp Mon Sep 09 04:48:16 2019 +0000 @@ -2,47 +2,54 @@ States::States(): anglePID(KP, KI, KD,0.001), - pad(padTX,padRX,ADDRESS), + pad(padTX,padRX), gyro(GYROTX,GYRORX), serial(MDTX,MDRX,MDBAUD), omni(&serial), - emStop(STOP) + emStop(STOP), + mechanism(&serial) { anglePID.setMode(AUTO_MODE); anglePID.setInputLimits(-360, 360); anglePID.setOutputLimits(minLimit, maxLimit); anglePID.setSetPoint(0.0); anglePID.setBias(0.0); - state = 400; + check = 0; emStop = false; } - void States::tellAngle(){ gyro.update(); nowAngle = gyro.getAngle(); deviation = nowAngle - idealAngle; if(deviation > 180) deviation -= 360; if(deviation < -180) deviation += 360; - anglePID.setProcessValue(deviation); } void States::tellPad(){ for(int i=0; i<13; i++){ - b[i] = 1 - pad.getButton1(i); + b[i] = pad.getButton(i); b3[i] = b2[i] - b[i]; b2[i] = b[i]; + check += b[i]; } - stickRad = PI - pad.getRadian(0) + (nowAngle * (PI / 180)); - norm = pad.getNorm(0); - if(b[1] && norm) norm = 0.25; - X = norm * cos(stickRad); - Y = norm * sin(stickRad); - if(b3[6] == 1) state++; - if(b3[0] == 1) state--; - + for(int i=0; i<4; i++) stick[i] = pad.getStick(i); + stick[1] = 255-stick[1]; + stick[3] = 255-stick[3]; + norm[0] = hypot(stick[0],stick[1]); + norm[1] = hypot(stick[2],stick[3]); + if(norm[0] > 20) norm[0] = 0; + if(norm[1] > 20) norm[1] = 0; + for(int i=0; i<2; i++) trigger[i] = pad.getTrigger(i); + omni.setBrake(trigger[0]); + nano.sendTrigger(trigger[0]); + mechanism.getTrigger(trigger[1]); + rad[0] = atan2(stick[1],stick[0]) + (nowAngle * (PI / 180)); + rad[1] = atan2(stick[2],stick[3]); + if(b3[4] == 1) state++; + if(b3[5] == 1) state--; /* *move.tellState(方角)でロボの方角 * "front" or 0 で前 @@ -55,21 +62,39 @@ case 2: idealAngle = back; break; case 3: idealAngle = left; break; default: idealAngle = front; break; - } + } + /*昇降機構*/ + if(b[0] && check == 1) mechanism.up(); + else if(b[1] && check == 1) mechanism.down(); + /*回収機構*/ + else if(b[6] && check == 1) mechanism.collect(); + /*つり上げ機構*/ + else if(b[7] && check == 1) mechanism.lift(); + /*装填機構*/ + else if(b[2] && check == 1) mechanism.loadRight(); + else if(b[3]) mechanism.loadLeft(); + /*かけ機構*/ + else if(b[9] && check == 1) mechanism.hookRight(); + else if(b[8] && check == 1) mechanism.hookLeft(); + + else nano.sendMove(0x0f); } void States::move() { - if(pad.receiveState() == 0){ + tellPad(); + if(b[10]) emStop = true; + if(b[11]) emStop = false; + nano.sendHeader(); + if(emStop == false)nano.sendMove(0xff); + else if(!pad.status){ /*omni.stop()は止める*/ omni.stop(); - //emStop = false; + emStop = false; }else{ - if(b[12]) emStop = true; - if(b[11]) emStop = false; tellAngle(); - tellPad(); turnPower = anglePID.compute(); - omni.move(X, Y, -turnPower); + if(norm[0] && norm[0] < 200) omni.stop(); + else omni.move(stick[0], stick[1], -turnPower); } -} +} \ No newline at end of file