手動bのほぼ全部
Dependents: kobayashi_rei 2019BSyudo Bsyudo 2019Bsyudo
states.cpp
- Committer:
- THtakahiro702286
- Date:
- 2019-09-13
- Revision:
- 6:3cb78320d4db
- Parent:
- 4:eafd5a0c3351
- Child:
- 8:8ff6db217a17
File content as of revision 6:3cb78320d4db:
#include "states.h" States::States(): anglePID(KP, KI, KD,0.001), pad(padTX,padRX), gyro(GYROTX,GYRORX), serial(MDTX,MDRX,MDBAUD), 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; 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<12; i++){ b[i] = pad.getButton(i); b3[i] = b2[i] - b[i]; b2[i] = b[i]; } for(int i=0; i<4; i++) stick[i] = pad.getStick(i); stick[0] -= 128; stick[1] = 128-stick[1]; stick[2] -= 128; stick[3] = 128-stick[3]; norm[0] = hypotf(stick[0],stick[1]); norm[1] = hypotf(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); mechanism.setBrake(trigger[0]); mechanism.getTrigger(trigger[1]); if(b3[5] == 1) state++; if(b3[4] == 1) state--; /* *ロボの方角 * "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() { while(1){ tellPad(); tellAngle(); if(b[10]) emStop = true; if(b[11]) emStop = false; if(!pad.status){ /*mechanism.stop()は止める*/ mechanism.stop(); emStop = false; }else{ turnPower = anglePID.compute(); if(norm[0] && norm[0] < 100) mechanism.move(0,0,-turnPower); else mechanism.move(stick[0], stick[1], -turnPower); /*昇降機構*/ if(b[0]) {mechanism.up();} else if(b[1]) {mechanism.down();} /*回収機構*/ else if(b[6]) {mechanism.collect();} /*つり上げ機構*/ else if(b[7]) {mechanism.lift();} /*装填機構*/ else if(b[2]) {mechanism.loadRight();} else if(b[3]) {mechanism.loadLeft();} /*かけ機構*/ else if(b[9]) {mechanism.hookRight();} else if(b[8]) {mechanism.hookLeft();} else mechanism.noAct(); } } }