hirromu takaki / states

手動bのほぼ全部

Dependents:   kobayashi_rei 2019BSyudo Bsyudo 2019Bsyudo

states.cpp@3:2b909340be04, 2019-07-11 (annotated)

Committer:
THtakahiro702286
Date:
Thu Jul 11 09:19:10 2019 +0000
Revision:
3:2b909340be04
Parent:
2:860eb70d3656
Child:
4:eafd5a0c3351
start work  together neko

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 0:8d012d469eb4 5 pad(padTX,padRX,ADDRESS),
THtakahiro702286 0:8d012d469eb4 6 gyro(GYROTX,GYRORX),
THtakahiro702286 0:8d012d469eb4 7 serial(MDTX,MDRX,MDBAUD),
THtakahiro702286 1:38dd7564c15f 8 omni(&serial),
THtakahiro702286 1:38dd7564c15f 9 emStop(STOP)
THtakahiro702286 0:8d012d469eb4 10 {
THtakahiro702286 0:8d012d469eb4 11 anglePID.setMode(AUTO_MODE);
THtakahiro702286 0:8d012d469eb4 12 anglePID.setInputLimits(-360, 360);
THtakahiro702286 0:8d012d469eb4 13 anglePID.setOutputLimits(minLimit, maxLimit);
THtakahiro702286 0:8d012d469eb4 14 anglePID.setSetPoint(0.0);
THtakahiro702286 0:8d012d469eb4 15 anglePID.setBias(0.0);
THtakahiro702286 0:8d012d469eb4 16
THtakahiro702286 0:8d012d469eb4 17 state = 400;
THtakahiro702286 3:2b909340be04 18 emStop = false;
THtakahiro702286 0:8d012d469eb4 19 }
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
THtakahiro702286 0:8d012d469eb4 29 anglePID.setProcessValue(deviation);
THtakahiro702286 0:8d012d469eb4 30 }
THtakahiro702286 0:8d012d469eb4 31
THtakahiro702286 0:8d012d469eb4 32 void States::tellPad(){
THtakahiro702286 0:8d012d469eb4 33 for(int i=0; i<13; i++){
THtakahiro702286 0:8d012d469eb4 34 b[i] = 1 - pad.getButton1(i);
THtakahiro702286 0:8d012d469eb4 35 b3[i] = b2[i] - b[i];
THtakahiro702286 0:8d012d469eb4 36 b2[i] = b[i];
THtakahiro702286 0:8d012d469eb4 37 }
THtakahiro702286 0:8d012d469eb4 38 stickRad = PI - pad.getRadian(0) + (nowAngle * (PI / 180));
THtakahiro702286 0:8d012d469eb4 39 norm = pad.getNorm(0);
THtakahiro702286 3:2b909340be04 40 if(b[1] && norm) norm = 0.25;
THtakahiro702286 0:8d012d469eb4 41 X = norm * cos(stickRad);
THtakahiro702286 0:8d012d469eb4 42 Y = norm * sin(stickRad);
THtakahiro702286 3:2b909340be04 43 if(b3[6] == 1) state++;
THtakahiro702286 3:2b909340be04 44 if(b3[0] == 1) state--;
THtakahiro702286 0:8d012d469eb4 45
THtakahiro702286 0:8d012d469eb4 46 /*
THtakahiro702286 0:8d012d469eb4 47 *move.tellState(方角)でロボの方角
THtakahiro702286 0:8d012d469eb4 48 * "front" or 0 　で前
THtakahiro702286 0:8d012d469eb4 49 * "right" or 90　で右
THtakahiro702286 0:8d012d469eb4 50 * "back" or 180 で後ろ
THtakahiro702286 0:8d012d469eb4 51 * "left" or -90 で左
THtakahiro702286 0:8d012d469eb4 52 */
THtakahiro702286 0:8d012d469eb4 53 switch(state % 4){
THtakahiro702286 0:8d012d469eb4 54 case 1: idealAngle = right; break;
THtakahiro702286 0:8d012d469eb4 55 case 2: idealAngle = back; break;
THtakahiro702286 0:8d012d469eb4 56 case 3: idealAngle = left; break;
THtakahiro702286 0:8d012d469eb4 57 default: idealAngle = front; break;
THtakahiro702286 0:8d012d469eb4 58 }
THtakahiro702286 0:8d012d469eb4 59 }
THtakahiro702286 0:8d012d469eb4 60
THtakahiro702286 0:8d012d469eb4 61 void States::move()
THtakahiro702286 0:8d012d469eb4 62 {
THtakahiro702286 0:8d012d469eb4 63 if(pad.receiveState() == 0){
THtakahiro702286 0:8d012d469eb4 64 /*omni.stop()は止める*/
THtakahiro702286 0:8d012d469eb4 65 omni.stop();
THtakahiro702286 3:2b909340be04 66 //emStop = false;
THtakahiro702286 0:8d012d469eb4 67 }else{
THtakahiro702286 3:2b909340be04 68 if(b[12]) emStop = true;
THtakahiro702286 3:2b909340be04 69 if(b[11]) emStop = false;
THtakahiro702286 0:8d012d469eb4 70 tellAngle();
THtakahiro702286 0:8d012d469eb4 71 tellPad();
THtakahiro702286 0:8d012d469eb4 72 turnPower = anglePID.compute();
THtakahiro702286 0:8d012d469eb4 73 omni.move(X, Y, -turnPower);
THtakahiro702286 0:8d012d469eb4 74 }
THtakahiro702286 0:8d012d469eb4 75 }