手動bのほぼ全部
Dependents: kobayashi_rei 2019BSyudo Bsyudo 2019Bsyudo
states.cpp
- Committer:
- THtakahiro702286
- Date:
- 2019-09-09
- Revision:
- 4:eafd5a0c3351
- Parent:
- 3:2b909340be04
- Child:
- 5:3273db2411d8
- Child:
- 6:3cb78320d4db
File content as of revision 4:eafd5a0c3351:
#include "states.h" States::States(): anglePID(KP, KI, KD,0.001), pad(padTX,padRX), gyro(GYROTX,GYRORX), serial(MDTX,MDRX,MDBAUD), omni(&serial), 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] = pad.getButton(i); b3[i] = b2[i] - b[i]; b2[i] = b[i]; check += b[i]; } 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 で前 * "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; } /*昇降機構*/ 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() { 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; }else{ tellAngle(); turnPower = anglePID.compute(); if(norm[0] && norm[0] < 200) omni.stop(); else omni.move(stick[0], stick[1], -turnPower); } }