hirromu takaki / states

手動bのほぼ全部

Dependents:   kobayashi_rei 2019BSyudo Bsyudo 2019Bsyudo

states.cpp@4:eafd5a0c3351, 2019-09-09 (annotated)

Committer:
THtakahiro702286
Date:
Mon Sep 09 04:48:16 2019 +0000
Revision:
4:eafd5a0c3351
Parent:
3:2b909340be04
Child:
5:3273db2411d8
Child:
6:3cb78320d4db
states;

Who changed what in which revision?

UserRevisionLine numberNew 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 1:38dd7564c15f 8 omni(&serial),
THtakahiro702286 4:eafd5a0c3351 9 emStop(STOP),
THtakahiro702286 4:eafd5a0c3351 10 mechanism(&serial)
THtakahiro702286 0:8d012d469eb4 11 {
THtakahiro702286 0:8d012d469eb4 12 anglePID.setMode(AUTO_MODE);
THtakahiro702286 0:8d012d469eb4 13 anglePID.setInputLimits(-360, 360);
THtakahiro702286 0:8d012d469eb4 14 anglePID.setOutputLimits(minLimit, maxLimit);
THtakahiro702286 0:8d012d469eb4 15 anglePID.setSetPoint(0.0);
THtakahiro702286 0:8d012d469eb4 16 anglePID.setBias(0.0);
THtakahiro702286 0:8d012d469eb4 17 state = 400;
THtakahiro702286 4:eafd5a0c3351 18 check = 0;
THtakahiro702286 3:2b909340be04 19 emStop = false;
THtakahiro702286 0:8d012d469eb4 20 }
THtakahiro702286 0:8d012d469eb4 21
THtakahiro702286 0:8d012d469eb4 22 void States::tellAngle(){
THtakahiro702286 0:8d012d469eb4 23 gyro.update();
THtakahiro702286 0:8d012d469eb4 24 nowAngle = gyro.getAngle();
THtakahiro702286 0:8d012d469eb4 25 deviation = nowAngle - idealAngle;
THtakahiro702286 0:8d012d469eb4 26 if(deviation > 180) deviation -= 360;
THtakahiro702286 0:8d012d469eb4 27 if(deviation < -180) deviation += 360;
THtakahiro702286 0:8d012d469eb4 28 anglePID.setProcessValue(deviation);
THtakahiro702286 0:8d012d469eb4 29 }
THtakahiro702286 0:8d012d469eb4 30
THtakahiro702286 0:8d012d469eb4 31 void States::tellPad(){
THtakahiro702286 0:8d012d469eb4 32 for(int i=0; i<13; i++){
THtakahiro702286 4:eafd5a0c3351 33 b[i] = pad.getButton(i);
THtakahiro702286 0:8d012d469eb4 34 b3[i] = b2[i] - b[i];
THtakahiro702286 0:8d012d469eb4 35 b2[i] = b[i];
THtakahiro702286 4:eafd5a0c3351 36 check += b[i];
THtakahiro702286 0:8d012d469eb4 37 }
THtakahiro702286 4:eafd5a0c3351 38 for(int i=0; i<4; i++) stick[i] = pad.getStick(i);
THtakahiro702286 4:eafd5a0c3351 39 stick[1] = 255-stick[1];
THtakahiro702286 4:eafd5a0c3351 40 stick[3] = 255-stick[3];
THtakahiro702286 4:eafd5a0c3351 41 norm[0] = hypot(stick[0],stick[1]);
THtakahiro702286 4:eafd5a0c3351 42 norm[1] = hypot(stick[2],stick[3]);
THtakahiro702286 4:eafd5a0c3351 43 if(norm[0] > 20) norm[0] = 0;
THtakahiro702286 4:eafd5a0c3351 44 if(norm[1] > 20) norm[1] = 0;
THtakahiro702286 4:eafd5a0c3351 45 for(int i=0; i<2; i++) trigger[i] = pad.getTrigger(i);
THtakahiro702286 4:eafd5a0c3351 46 omni.setBrake(trigger[0]);
THtakahiro702286 4:eafd5a0c3351 47 nano.sendTrigger(trigger[0]);
THtakahiro702286 4:eafd5a0c3351 48 mechanism.getTrigger(trigger[1]);
THtakahiro702286 4:eafd5a0c3351 49 rad[0] = atan2(stick[1],stick[0]) + (nowAngle * (PI / 180));
THtakahiro702286 4:eafd5a0c3351 50 rad[1] = atan2(stick[2],stick[3]);
THtakahiro702286 4:eafd5a0c3351 51 if(b3[4] == 1) state++;
THtakahiro702286 4:eafd5a0c3351 52 if(b3[5] == 1) state--;
THtakahiro702286 0:8d012d469eb4 53 /*
THtakahiro702286 0:8d012d469eb4 54 *move.tellState(方角)でロボの方角
THtakahiro702286 0:8d012d469eb4 55 * "front" or 0 　で前
THtakahiro702286 0:8d012d469eb4 56 * "right" or 90　で右
THtakahiro702286 0:8d012d469eb4 57 * "back" or 180 で後ろ
THtakahiro702286 0:8d012d469eb4 58 * "left" or -90 で左
THtakahiro702286 0:8d012d469eb4 59 */
THtakahiro702286 0:8d012d469eb4 60 switch(state % 4){
THtakahiro702286 0:8d012d469eb4 61 case 1: idealAngle = right; break;
THtakahiro702286 0:8d012d469eb4 62 case 2: idealAngle = back; break;
THtakahiro702286 0:8d012d469eb4 63 case 3: idealAngle = left; break;
THtakahiro702286 0:8d012d469eb4 64 default: idealAngle = front; break;
THtakahiro702286 4:eafd5a0c3351 65 }
THtakahiro702286 4:eafd5a0c3351 66 /*昇降機構*/
THtakahiro702286 4:eafd5a0c3351 67 if(b[0] && check == 1) mechanism.up();
THtakahiro702286 4:eafd5a0c3351 68 else if(b[1] && check == 1) mechanism.down();
THtakahiro702286 4:eafd5a0c3351 69 /*回収機構*/
THtakahiro702286 4:eafd5a0c3351 70 else if(b[6] && check == 1) mechanism.collect();
THtakahiro702286 4:eafd5a0c3351 71 /*つり上げ機構*/
THtakahiro702286 4:eafd5a0c3351 72 else if(b[7] && check == 1) mechanism.lift();
THtakahiro702286 4:eafd5a0c3351 73 /*装填機構*/
THtakahiro702286 4:eafd5a0c3351 74 else if(b[2] && check == 1) mechanism.loadRight();
THtakahiro702286 4:eafd5a0c3351 75 else if(b[3]) mechanism.loadLeft();
THtakahiro702286 4:eafd5a0c3351 76 /*かけ機構*/
THtakahiro702286 4:eafd5a0c3351 77 else if(b[9] && check == 1) mechanism.hookRight();
THtakahiro702286 4:eafd5a0c3351 78 else if(b[8] && check == 1) mechanism.hookLeft();
THtakahiro702286 4:eafd5a0c3351 79
THtakahiro702286 4:eafd5a0c3351 80 else nano.sendMove(0x0f);
THtakahiro702286 0:8d012d469eb4 81 }
THtakahiro702286 0:8d012d469eb4 82
THtakahiro702286 0:8d012d469eb4 83 void States::move()
THtakahiro702286 0:8d012d469eb4 84 {
THtakahiro702286 4:eafd5a0c3351 85 tellPad();
THtakahiro702286 4:eafd5a0c3351 86 if(b[10]) emStop = true;
THtakahiro702286 4:eafd5a0c3351 87 if(b[11]) emStop = false;
THtakahiro702286 4:eafd5a0c3351 88 nano.sendHeader();
THtakahiro702286 4:eafd5a0c3351 89 if(emStop == false)nano.sendMove(0xff);
THtakahiro702286 4:eafd5a0c3351 90 else if(!pad.status){
THtakahiro702286 0:8d012d469eb4 91 /*omni.stop()は止める*/
THtakahiro702286 0:8d012d469eb4 92 omni.stop();
THtakahiro702286 4:eafd5a0c3351 93 emStop = false;
THtakahiro702286 0:8d012d469eb4 94 }else{
THtakahiro702286 0:8d012d469eb4 95 tellAngle();
THtakahiro702286 0:8d012d469eb4 96 turnPower = anglePID.compute();
THtakahiro702286 4:eafd5a0c3351 97 if(norm[0] && norm[0] < 200) omni.stop();
THtakahiro702286 4:eafd5a0c3351 98 else omni.move(stick[0], stick[1], -turnPower);
THtakahiro702286 0:8d012d469eb4 99 }
THtakahiro702286 4:eafd5a0c3351 100 }