output.cpp

00001 #include "mbed.h"
00002 #include "extern.h"
00003 
00004 //pid&cmps
00005 void PidUpdate(void)
00006 {
00007     cmps_set.cmps = hmc.sample()/10.0;
00008     cmps_set.InputPID = 
00009     fmod(
00010         (
00011             cmps_set.cmps+
00012             (cmps_set.CmpsDiff
00013                 -cmps_set.FrontDeg
00014                 -cmps_set.AtkDeg*(sys.TurnAtkBlind==0)
00015                 -cmps_set.HoldDeg*(sys.TurnHoldBlind==0)
00016                 -cmps_set.GoalDeg*(sys.TurnDriBlind==0)
00017             )+
00018             7200.0
00019         ),
00020         360.0
00021     );//0<cmps_set.cmps<359.0
00022     pid.setProcessValue(cmps_set.InputPID);
00023     cmps_set.OutputPID = -pid.compute();
00024     /*if((abs(cmps_set.OutputPID)<PID_OUT_MIN)&&(cmps_set.OutputPID!=0)){
00025         if(cmps_set.OutputPID>0) cmps_set.OutputPID=PID_OUT_MIN;
00026         else cmps_set.OutputPID=-PID_OUT_MIN;
00027     }*/
00028 }
00029 void HmcReset(void){
00030     hmc_reset=HMC_RST;
00031     wait_us(100);
00032     hmc_reset=HMC_RUN;
00033 }
00034 void TurnAttack(void){
00035     cmps_set.AtkDeg = 30;
00036 }
00037 void DriveTurn(void){
00038     if(sys.TurnAtkBlind==1) return;
00039     if(sys.TurnStopFlag==1) return;
00040     
00041     if(
00042         //(data.ping[L_PING]<data.ping[R_PING])||
00043         ((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]!=A_SPOT)&&(data.lnRawOrder[0]==LINE_EMPTY))
00044     ){
00045         cmps_set.AtkDeg = 150;
00046         if((sys.TurnDriBlind==0)&&(cmps_set.GoalDeg>0)){
00047             cmps_set.AtkDeg = -cmps_set.GoalDeg*2;
00048         }
00049     }
00050     else if(
00051         //(data.ping[L_PING]>data.ping[R_PING])||
00052          ((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]!=B_SPOT)&&(data.lnRawOrder[0]==LINE_EMPTY))
00053     ){
00054         cmps_set.AtkDeg = -150;
00055         if((sys.TurnDriBlind==0)&&(cmps_set.GoalDeg<0)){
00056             cmps_set.AtkDeg = cmps_set.GoalDeg*2;
00057         }
00058     }
00059     else{
00060         cmps_set.AtkDeg = 0;
00061     }
00062     if((sys.TurnDriBlind==0)&&(cmps_set.GoalDeg!=0)){
00063         Turn_timeout.attach(&TurnSignal, .25);
00064         Turn_stop.attach(&ValidTurn, .5);
00065     }
00066     else{
00067         Turn_timeout.attach(&TurnSignal, .25);
00068         Turn_stop.attach(&ValidTurn, .5);
00069     }
00070     sys.TurnStopFlag=1;
00071     
00072 }
00073 void TurnSignal(void){
00074     cmps_set.AtkDeg = 0;
00075 }
00076 void ValidTurn(void){
00077     sys.TurnStopFlag=0;
00078 }
00079 /*void ValidPidUpdate(void){
00080     if(sys.PidFlag==0){
00081         sys.PidFlag=1;
00082     }
00083 }*/
00084 void FaceToFront(void){
00085     cmps_set.AtkDeg=0;
00086 }
00087 //motor
00088 void ValidTx_motor(void){
00089     if(sys.MotorFlag==0){
00090         sys.MotorFlag=1;
00091     }
00092 }
00093 void tx_motor(){//モーターへの送信
00094     array2(MD_1CH*speed[1],MD_2CH*speed[0],MD_3CH*speed[2],MD_4CH*speed[3]);//右後左ド
00095     //array2((1)*speed[1],(1)*speed[0],(-1)*speed[2],(1)*speed[3]);//右後左ドgreen
00096     //array2((1)*speed[1],(1)*speed[0],(1)*speed[2],(-1)*speed[3]);//右後左ドred
00097     motor.printf("%s",StringFIN.c_str());
00098 }
00099 void move(int vx, int vy, int vs){//三輪オムニの移動（基本の形）
00100     uint8_t i;
00101     double pwm[4] = {0};
00102     
00103     pwm[0] = (double)((vx) + vs);
00104     pwm[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0)  * vy) + vs);
00105     pwm[2] = (double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy) + vs);
00106     pwm[3] = (sys.dri_pow)*(sys.DribbleFlag)*(sys.DriMotorBlind==0);
00107 
00108     for(i = 0; i < 4; i++){
00109         if(pwm[i] > 100){
00110             pwm[i] = 100;
00111         } else if(pwm[i] < -100){
00112             pwm[i] = -100;
00113         }
00114         speed[i] = pwm[i];
00115     }
00116 }
00117 //solenoid
00118 void DriveSolenoid(void){
00119     if(sys.KickBlind==1) return;
00120     if(sys.KickStopFlag==1) return;
00121     
00122     kicker=1;
00123     sys.KickStopFlag=1;
00124     Solenoid_timeout.attach(&SolenoidSignal, .5);
00125     Kick_stop.attach(&ValidSolenoid, 2.0);
00126 }
00127 void DriveSolenoidJudge(void){
00128     if(sys.DriBlind==0) return;
00129     if((data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
00130         DriveTurn();
00131         DriveSolenoid();
00132     }
00133 }
00134 void SolenoidSignal(void){
00135     kicker=0;
00136 }
00137 void ValidSolenoid(void){
00138     sys.KickStopFlag=0;
00139 }
00140 //ball
00141 void JudgeBallHold(void){
00142     if(data.ball==1){
00143         sys.BallHoldFlag=1;
00144     }
00145     else if(data.ball==0){
00146         sys.BallHoldFlag=0;
00147     }
00148     sys.BallHoldJudgeFlag=0;
00149 }
00150 //line
00151 void LineLiberate(void){
00152     LineKeeper = LINE_FREE;
00153     wait_us(10);
00154     LineKeeper = LINE_FIX;
00155 }
00156 //math
00157 uint8_t GetBit(uint8_t n, uint8_t bit){
00158     return (n>>(bit-1))%2;
00159 }