hirromu takaki / states

手動bのほぼ全部

Dependents:   kobayashi_rei 2019BSyudo Bsyudo 2019Bsyudo

states.cpp@7:9d5746931f80, 2019-09-13 (annotated)

Committer:
THtakahiro702286
Date:
Fri Sep 13 03:22:42 2019 +0000
Revision:
7:9d5746931f80
Parent:
5:3273db2411d8 
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 7:9d5746931f80 4 emStop(STOP),
THtakahiro702286 0:8d012d469eb4 5 serial(MDTX,MDRX,MDBAUD),
THtakahiro702286 7:9d5746931f80 6 gyro(GYROTX,GYRORX),
THtakahiro702286 7:9d5746931f80 7 anglePID(KP, KI, KD,0.001),
THtakahiro702286 7:9d5746931f80 8 mechanism(&serial),
THtakahiro702286 7:9d5746931f80 9 pad(padTX,padRX)
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 state = 400;
THtakahiro702286 3:2b909340be04 17 emStop = false;
THtakahiro702286 0:8d012d469eb4 18 }
THtakahiro702286 0:8d012d469eb4 19
THtakahiro702286 0:8d012d469eb4 20 void States::tellAngle(){
THtakahiro702286 0:8d012d469eb4 21 gyro.update();
THtakahiro702286 0:8d012d469eb4 22 nowAngle = gyro.getAngle();
THtakahiro702286 0:8d012d469eb4 23 deviation = nowAngle - idealAngle;
THtakahiro702286 0:8d012d469eb4 24 if(deviation > 180) deviation -= 360;
THtakahiro702286 0:8d012d469eb4 25 if(deviation < -180) deviation += 360;
THtakahiro702286 0:8d012d469eb4 26 anglePID.setProcessValue(deviation);
THtakahiro702286 0:8d012d469eb4 27 }
THtakahiro702286 0:8d012d469eb4 28
THtakahiro702286 0:8d012d469eb4 29 void States::tellPad(){
THtakahiro702286 7:9d5746931f80 30 for(int i=0; i<12; i++){
THtakahiro702286 4:eafd5a0c3351 31 b[i] = pad.getButton(i);
THtakahiro702286 0:8d012d469eb4 32 b3[i] = b2[i] - b[i];
THtakahiro702286 7:9d5746931f80 33 b2[i] = b[i];
THtakahiro702286 0:8d012d469eb4 34 }
THtakahiro702286 4:eafd5a0c3351 35 for(int i=0; i<4; i++) stick[i] = pad.getStick(i);
THtakahiro702286 7:9d5746931f80 36 stick[0] -= 128;
THtakahiro702286 7:9d5746931f80 37 stick[1] = 128-stick[1];
THtakahiro702286 7:9d5746931f80 38 stick[2] -= 128;
THtakahiro702286 7:9d5746931f80 39 stick[3] = 128-stick[3];
THtakahiro702286 5:3273db2411d8 40 norm[0] = hypotf(stick[0],stick[1]);
THtakahiro702286 5:3273db2411d8 41 norm[1] = hypotf(stick[2],stick[3]);
THtakahiro702286 4:eafd5a0c3351 42 if(norm[0] > 20) norm[0] = 0;
THtakahiro702286 4:eafd5a0c3351 43 if(norm[1] > 20) norm[1] = 0;
THtakahiro702286 4:eafd5a0c3351 44 for(int i=0; i<2; i++) trigger[i] = pad.getTrigger(i);
THtakahiro702286 7:9d5746931f80 45 mechanism.setBrake(trigger[0]);
THtakahiro702286 7:9d5746931f80 46
THtakahiro702286 4:eafd5a0c3351 47 mechanism.getTrigger(trigger[1]);
THtakahiro702286 7:9d5746931f80 48 if(b3[5] == 1) state++;
THtakahiro702286 7:9d5746931f80 49 if(b3[4] == 1) state--;
THtakahiro702286 0:8d012d469eb4 50 /*
THtakahiro702286 7:9d5746931f80 51 *ロボの方角
THtakahiro702286 0:8d012d469eb4 52 * "front" or 0 　で前
THtakahiro702286 0:8d012d469eb4 53 * "right" or 90　で右
THtakahiro702286 0:8d012d469eb4 54 * "back" or 180 で後ろ
THtakahiro702286 0:8d012d469eb4 55 * "left" or -90 で左
THtakahiro702286 0:8d012d469eb4 56 */
THtakahiro702286 0:8d012d469eb4 57 switch(state % 4){
THtakahiro702286 0:8d012d469eb4 58 case 1: idealAngle = right; break;
THtakahiro702286 0:8d012d469eb4 59 case 2: idealAngle = back; break;
THtakahiro702286 0:8d012d469eb4 60 case 3: idealAngle = left; break;
THtakahiro702286 0:8d012d469eb4 61 default: idealAngle = front; break;
THtakahiro702286 4:eafd5a0c3351 62 }
THtakahiro702286 0:8d012d469eb4 63 }
THtakahiro702286 0:8d012d469eb4 64
THtakahiro702286 0:8d012d469eb4 65 void States::move()
THtakahiro702286 0:8d012d469eb4 66 {
THtakahiro702286 7:9d5746931f80 67 while(1){
THtakahiro702286 7:9d5746931f80 68 tellPad();
THtakahiro702286 0:8d012d469eb4 69 tellAngle();
THtakahiro702286 7:9d5746931f80 70 if(b[10]) emStop = true;
THtakahiro702286 7:9d5746931f80 71 if(b[11]) emStop = false;
THtakahiro702286 7:9d5746931f80 72 if(!pad.status){
THtakahiro702286 7:9d5746931f80 73 /*mechanism.stop()は止める*/
THtakahiro702286 7:9d5746931f80 74 mechanism.stop();
THtakahiro702286 7:9d5746931f80 75 emStop = false;
THtakahiro702286 7:9d5746931f80 76 }else{
THtakahiro702286 7:9d5746931f80 77 turnPower = anglePID.compute();
THtakahiro702286 7:9d5746931f80 78 if(norm[0] && norm[0] < 100) mechanism.move(0,0,-turnPower);
THtakahiro702286 7:9d5746931f80 79 else mechanism.move(stick[0], stick[1], -turnPower);
THtakahiro702286 7:9d5746931f80 80 /*昇降機構*/
THtakahiro702286 7:9d5746931f80 81 if(b[0]) {mechanism.up();}
THtakahiro702286 7:9d5746931f80 82 else if(b[1]) {mechanism.down();}
THtakahiro702286 7:9d5746931f80 83 /*回収機構*/
THtakahiro702286 7:9d5746931f80 84 else if(b[6]) {mechanism.collect();}
THtakahiro702286 7:9d5746931f80 85 /*つり上げ機構*/
THtakahiro702286 7:9d5746931f80 86 else if(b[7]) {mechanism.lift();}
THtakahiro702286 7:9d5746931f80 87 /*装填機構*/
THtakahiro702286 7:9d5746931f80 88 else if(b[2]) {mechanism.loadRight();}
THtakahiro702286 7:9d5746931f80 89 else if(b[3]) {mechanism.loadLeft();}
THtakahiro702286 7:9d5746931f80 90 /*かけ機構*/
THtakahiro702286 7:9d5746931f80 91 else if(b[9]) {mechanism.hookRight();}
THtakahiro702286 7:9d5746931f80 92 else if(b[8]) {mechanism.hookLeft();}
THtakahiro702286 7:9d5746931f80 93
THtakahiro702286 7:9d5746931f80 94 else mechanism.noAct();
THtakahiro702286 7:9d5746931f80 95 }
THtakahiro702286 0:8d012d469eb4 96 }
THtakahiro702286 4:eafd5a0c3351 97 }