手動bのほぼ全部
Dependents: kobayashi_rei 2019BSyudo Bsyudo 2019Bsyudo
states.cpp
- Committer:
- THtakahiro702286
- Date:
- 2019-07-10
- Revision:
- 1:38dd7564c15f
- Parent:
- 0:8d012d469eb4
- Child:
- 2:860eb70d3656
File content as of revision 1:38dd7564c15f:
#include "states.h" States::States(): anglePID(KP, KI, KD,0.001), pad(padTX,padRX,ADDRESS), gyro(GYROTX,GYRORX), serial(MDTX,MDRX,MDBAUD), omni(&serial), emStop(STOP) { anglePID.setMode(AUTO_MODE); anglePID.setInputLimits(-360, 360); anglePID.setOutputLimits(minLimit, maxLimit); anglePID.setSetPoint(0.0); anglePID.setBias(0.0); state = 400; } 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); b3[i] = b2[i] - b[i]; b2[i] = b[i]; } stickRad = PI - pad.getRadian(0) + (nowAngle * (PI / 180)); norm = pad.getNorm(0); X = norm * cos(stickRad); Y = norm * sin(stickRad); if(b3[0] == 1) state++; if(b3[1] == 1) state--; /* *move.tellState(方角)でロボの方角 * "front" or 0 で前 * "right" or 90 で右 * "back" or 180 で後ろ * "left" or -90 で左 */ switch(state % 4){ case 1: idealAngle = right; break; case 2: idealAngle = back; break; case 3: idealAngle = left; break; default: idealAngle = front; break; } } void States::move() { if(pad.receiveState() == 0){ /*omni.stop()は止める*/ omni.stop(); emStop = false; }else{ emStop = true; tellAngle(); tellPad(); // tellState(); turnPower = anglePID.compute(); omni.move(X, Y, -turnPower); } }