CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v10_T_Main

ジャパンオープン用のメインプログラム

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy_parts/output.cpp@38:67bc78f3c0ab, 2016-03-27 (annotated)

Committer:
lilac0112_1
Date:
Sun Mar 27 13:04:39 2016 +0000
Revision:
38:67bc78f3c0ab
Parent:
34:1c86c1299ea4 
JapanSoccerOpen2016 CatPot Program(main)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lilac0112_1 0:ea35c18c85fc 1 #include "mbed.h"
lilac0112_1 0:ea35c18c85fc 2 #include "extern.h"
lilac0112_1 0:ea35c18c85fc 3
lilac0112_1 0:ea35c18c85fc 4 //pid&cmps
lilac0112_1 0:ea35c18c85fc 5 void PidUpdate(void)
lilac0112_1 18:3a42a931c95a 6 {
lilac0112_1 0:ea35c18c85fc 7 cmps_set.cmps = hmc.sample()/10.0;
lilac0112_1 32:367b16d69a32 8 cmps_set.InputPID =
lilac0112_1 32:367b16d69a32 9 fmod(
lilac0112_1 32:367b16d69a32 10 (
lilac0112_1 32:367b16d69a32 11 cmps_set.cmps+
lilac0112_1 32:367b16d69a32 12 (cmps_set.CmpsDiff
lilac0112_1 32:367b16d69a32 13 -cmps_set.FrontDeg
lilac0112_1 32:367b16d69a32 14 -cmps_set.AtkDeg*(sys.TurnAtkBlind==0)
lilac0112_1 32:367b16d69a32 15 -cmps_set.HoldDeg*(sys.TurnHoldBlind==0)
lilac0112_1 32:367b16d69a32 16 -cmps_set.GoalDeg*(sys.TurnDriBlind==0)
lilac0112_1 32:367b16d69a32 17 )+
lilac0112_1 32:367b16d69a32 18 7200.0
lilac0112_1 32:367b16d69a32 19 ),
lilac0112_1 32:367b16d69a32 20 360.0
lilac0112_1 32:367b16d69a32 21 );//0<cmps_set.cmps<359.0
lilac0112_1 0:ea35c18c85fc 22 pid.setProcessValue(cmps_set.InputPID);
lilac0112_1 0:ea35c18c85fc 23 cmps_set.OutputPID = -pid.compute();
lilac0112_1 18:3a42a931c95a 24 /*if((abs(cmps_set.OutputPID)<PID_OUT_MIN)&&(cmps_set.OutputPID!=0)){
lilac0112_1 10:6df631c39f9b 25 if(cmps_set.OutputPID>0) cmps_set.OutputPID=PID_OUT_MIN;
lilac0112_1 10:6df631c39f9b 26 else cmps_set.OutputPID=-PID_OUT_MIN;
lilac0112_1 18:3a42a931c95a 27 }*/
lilac0112_1 0:ea35c18c85fc 28 }
lilac0112_1 18:3a42a931c95a 29 void HmcReset(void){
lilac0112_1 18:3a42a931c95a 30 hmc_reset=HMC_RST;
lilac0112_1 18:3a42a931c95a 31 wait_us(100);
lilac0112_1 18:3a42a931c95a 32 hmc_reset=HMC_RUN;
lilac0112_1 18:3a42a931c95a 33 }
lilac0112_1 13:b20921316f3c 34 void TurnAttack(void){
lilac0112_1 19:967207de919d 35 cmps_set.AtkDeg = 30;
lilac0112_1 13:b20921316f3c 36 }
lilac0112_1 24:34ef6379b0df 37 void DriveTurn(void){
lilac0112_1 32:367b16d69a32 38 if(sys.TurnAtkBlind==1) return;
lilac0112_1 24:34ef6379b0df 39 if(sys.TurnStopFlag==1) return;
lilac0112_1 24:34ef6379b0df 40
lilac0112_1 30:5998ba42237e 41 if(
lilac0112_1 32:367b16d69a32 42 //(data.ping[L_PING]<data.ping[R_PING])||
lilac0112_1 32:367b16d69a32 43 ((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]!=A_SPOT)&&(data.lnRawOrder[0]==LINE_EMPTY))
lilac0112_1 30:5998ba42237e 44 ){
lilac0112_1 25:a7460e23e02e 45 cmps_set.AtkDeg = 150;
lilac0112_1 32:367b16d69a32 46 if((sys.TurnDriBlind==0)&&(cmps_set.GoalDeg>0)){
lilac0112_1 32:367b16d69a32 47 cmps_set.AtkDeg = -cmps_set.GoalDeg*2;
lilac0112_1 32:367b16d69a32 48 }
lilac0112_1 25:a7460e23e02e 49 }
lilac0112_1 30:5998ba42237e 50 else if(
lilac0112_1 32:367b16d69a32 51 //(data.ping[L_PING]>data.ping[R_PING])||
lilac0112_1 32:367b16d69a32 52 ((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]!=B_SPOT)&&(data.lnRawOrder[0]==LINE_EMPTY))
lilac0112_1 30:5998ba42237e 53 ){
lilac0112_1 25:a7460e23e02e 54 cmps_set.AtkDeg = -150;
lilac0112_1 32:367b16d69a32 55 if((sys.TurnDriBlind==0)&&(cmps_set.GoalDeg<0)){
lilac0112_1 32:367b16d69a32 56 cmps_set.AtkDeg = cmps_set.GoalDeg*2;
lilac0112_1 32:367b16d69a32 57 }
lilac0112_1 25:a7460e23e02e 58 }
lilac0112_1 25:a7460e23e02e 59 else{
lilac0112_1 25:a7460e23e02e 60 cmps_set.AtkDeg = 0;
lilac0112_1 25:a7460e23e02e 61 }
lilac0112_1 32:367b16d69a32 62 if((sys.TurnDriBlind==0)&&(cmps_set.GoalDeg!=0)){
lilac0112_1 32:367b16d69a32 63 Turn_timeout.attach(&TurnSignal, .25);
lilac0112_1 32:367b16d69a32 64 Turn_stop.attach(&ValidTurn, .5);
lilac0112_1 32:367b16d69a32 65 }
lilac0112_1 32:367b16d69a32 66 else{
lilac0112_1 32:367b16d69a32 67 Turn_timeout.attach(&TurnSignal, .25);
lilac0112_1 32:367b16d69a32 68 Turn_stop.attach(&ValidTurn, .5);
lilac0112_1 32:367b16d69a32 69 }
lilac0112_1 24:34ef6379b0df 70 sys.TurnStopFlag=1;
lilac0112_1 32:367b16d69a32 71
lilac0112_1 24:34ef6379b0df 72 }
lilac0112_1 24:34ef6379b0df 73 void TurnSignal(void){
lilac0112_1 24:34ef6379b0df 74 cmps_set.AtkDeg = 0;
lilac0112_1 24:34ef6379b0df 75 }
lilac0112_1 24:34ef6379b0df 76 void ValidTurn(void){
lilac0112_1 24:34ef6379b0df 77 sys.TurnStopFlag=0;
lilac0112_1 24:34ef6379b0df 78 }
lilac0112_1 21:378470320524 79 /*void ValidPidUpdate(void){
lilac0112_1 0:ea35c18c85fc 80 if(sys.PidFlag==0){
lilac0112_1 0:ea35c18c85fc 81 sys.PidFlag=1;
lilac0112_1 0:ea35c18c85fc 82 }
lilac0112_1 21:378470320524 83 }*/
lilac0112_1 19:967207de919d 84 void FaceToFront(void){
lilac0112_1 19:967207de919d 85 cmps_set.AtkDeg=0;
lilac0112_1 19:967207de919d 86 }
lilac0112_1 0:ea35c18c85fc 87 //motor
lilac0112_1 0:ea35c18c85fc 88 void ValidTx_motor(void){
lilac0112_1 0:ea35c18c85fc 89 if(sys.MotorFlag==0){
lilac0112_1 0:ea35c18c85fc 90 sys.MotorFlag=1;
lilac0112_1 0:ea35c18c85fc 91 }
lilac0112_1 0:ea35c18c85fc 92 }
lilac0112_1 0:ea35c18c85fc 93 void tx_motor(){//モーターへの送信
lilac0112_1 26:6ca88eeaa2b4 94 array2(MD_1CH*speed[1],MD_2CH*speed[0],MD_3CH*speed[2],MD_4CH*speed[3]);//右後左ド
lilac0112_1 30:5998ba42237e 95 //array2((1)*speed[1],(1)*speed[0],(-1)*speed[2],(1)*speed[3]);//右後左ドgreen
lilac0112_1 30:5998ba42237e 96 //array2((1)*speed[1],(1)*speed[0],(1)*speed[2],(-1)*speed[3]);//右後左ドred
lilac0112_1 0:ea35c18c85fc 97 motor.printf("%s",StringFIN.c_str());
lilac0112_1 0:ea35c18c85fc 98 }
lilac0112_1 0:ea35c18c85fc 99 void move(int vx, int vy, int vs){//三輪オムニの移動（基本の形）
lilac0112_1 0:ea35c18c85fc 100 uint8_t i;
lilac0112_1 0:ea35c18c85fc 101 double pwm[4] = {0};
lilac0112_1 0:ea35c18c85fc 102
lilac0112_1 0:ea35c18c85fc 103 pwm[0] = (double)((vx) + vs);
lilac0112_1 0:ea35c18c85fc 104 pwm[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + vs);
lilac0112_1 0:ea35c18c85fc 105 pwm[2] = (double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy) + vs);
lilac0112_1 34:1c86c1299ea4 106 pwm[3] = (sys.dri_pow)*(sys.DribbleFlag)*(sys.DriMotorBlind==0);
lilac0112_1 0:ea35c18c85fc 107
lilac0112_1 0:ea35c18c85fc 108 for(i = 0; i < 4; i++){
lilac0112_1 0:ea35c18c85fc 109 if(pwm[i] > 100){
lilac0112_1 0:ea35c18c85fc 110 pwm[i] = 100;
lilac0112_1 0:ea35c18c85fc 111 } else if(pwm[i] < -100){
lilac0112_1 0:ea35c18c85fc 112 pwm[i] = -100;
lilac0112_1 0:ea35c18c85fc 113 }
lilac0112_1 0:ea35c18c85fc 114 speed[i] = pwm[i];
lilac0112_1 0:ea35c18c85fc 115 }
lilac0112_1 0:ea35c18c85fc 116 }
lilac0112_1 0:ea35c18c85fc 117 //solenoid
lilac0112_1 0:ea35c18c85fc 118 void DriveSolenoid(void){
lilac0112_1 32:367b16d69a32 119 if(sys.KickBlind==1) return;
lilac0112_1 24:34ef6379b0df 120 if(sys.KickStopFlag==1) return;
lilac0112_1 24:34ef6379b0df 121
lilac0112_1 0:ea35c18c85fc 122 kicker=1;
lilac0112_1 24:34ef6379b0df 123 sys.KickStopFlag=1;
lilac0112_1 0:ea35c18c85fc 124 Solenoid_timeout.attach(&SolenoidSignal, .5);
lilac0112_1 24:34ef6379b0df 125 Kick_stop.attach(&ValidSolenoid, 2.0);
lilac0112_1 0:ea35c18c85fc 126 }
lilac0112_1 32:367b16d69a32 127 void DriveSolenoidJudge(void){
lilac0112_1 33:aa115c30892e 128 if(sys.DriBlind==0) return;
lilac0112_1 32:367b16d69a32 129 if((data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
lilac0112_1 32:367b16d69a32 130 DriveTurn();
lilac0112_1 32:367b16d69a32 131 DriveSolenoid();
lilac0112_1 32:367b16d69a32 132 }
lilac0112_1 32:367b16d69a32 133 }
lilac0112_1 0:ea35c18c85fc 134 void SolenoidSignal(void){
lilac0112_1 0:ea35c18c85fc 135 kicker=0;
lilac0112_1 2:635947de1583 136 }
lilac0112_1 24:34ef6379b0df 137 void ValidSolenoid(void){
lilac0112_1 24:34ef6379b0df 138 sys.KickStopFlag=0;
lilac0112_1 24:34ef6379b0df 139 }
lilac0112_1 19:967207de919d 140 //ball
lilac0112_1 25:a7460e23e02e 141 void JudgeBallHold(void){
lilac0112_1 25:a7460e23e02e 142 if(data.ball==1){
lilac0112_1 19:967207de919d 143 sys.BallHoldFlag=1;
lilac0112_1 19:967207de919d 144 }
lilac0112_1 19:967207de919d 145 else if(data.ball==0){
lilac0112_1 19:967207de919d 146 sys.BallHoldFlag=0;
lilac0112_1 19:967207de919d 147 }
lilac0112_1 25:a7460e23e02e 148 sys.BallHoldJudgeFlag=0;
lilac0112_1 19:967207de919d 149 }
lilac0112_1 11:3efae754e6ef 150 //line
lilac0112_1 11:3efae754e6ef 151 void LineLiberate(void){
lilac0112_1 11:3efae754e6ef 152 LineKeeper = LINE_FREE;
lilac0112_1 11:3efae754e6ef 153 wait_us(10);
lilac0112_1 11:3efae754e6ef 154 LineKeeper = LINE_FIX;
lilac0112_1 11:3efae754e6ef 155 }
lilac0112_1 2:635947de1583 156 //math
lilac0112_1 2:635947de1583 157 uint8_t GetBit(uint8_t n, uint8_t bit){
lilac0112_1 2:635947de1583 158 return (n>>(bit-1))%2;
lilac0112_1 0:ea35c18c85fc 159 }