CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v10_T_Main

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

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy_parts/output.cpp@18:3a42a931c95a, 2016-03-15 (annotated)

Committer:
lilac0112_1
Date:
Tue Mar 15 12:03:51 2016 +0000
Revision:
18:3a42a931c95a
Parent:
13:b20921316f3c
Child:
19:967207de919d
LINE NAOSITA

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 18:3a42a931c95a 7
lilac0112_1 9:c966191926c5 8 //pid.setSetPoint(REFERENCE + cmps_set.FrontDeg);
lilac0112_1 0:ea35c18c85fc 9 cmps_set.cmps = hmc.sample()/10.0;
lilac0112_1 18:3a42a931c95a 10 cmps_set.InputPID = fmod((cmps_set.cmps+cmps_set.CmpsDiff+7200.0), 360.0);//0<cmps_set.cmps<359.0
lilac0112_1 0:ea35c18c85fc 11
lilac0112_1 0:ea35c18c85fc 12 pid.setProcessValue(cmps_set.InputPID);
lilac0112_1 0:ea35c18c85fc 13 cmps_set.OutputPID = -pid.compute();
lilac0112_1 18:3a42a931c95a 14 /*if((abs(cmps_set.OutputPID)<PID_OUT_MIN)&&(cmps_set.OutputPID!=0)){
lilac0112_1 10:6df631c39f9b 15 if(cmps_set.OutputPID>0) cmps_set.OutputPID=PID_OUT_MIN;
lilac0112_1 10:6df631c39f9b 16 else cmps_set.OutputPID=-PID_OUT_MIN;
lilac0112_1 18:3a42a931c95a 17 }*/
lilac0112_1 0:ea35c18c85fc 18 }
lilac0112_1 9:c966191926c5 19 void FrontHere(void){
lilac0112_1 10:6df631c39f9b 20 //ReadCmps();
lilac0112_1 10:6df631c39f9b 21 //cmps_set.CmpsInitialValue = cmps_set.cmps;
lilac0112_1 9:c966191926c5 22
lilac0112_1 9:c966191926c5 23 cmps_set.CmpsDiff = (REFERENCE-cmps_set.FrontDeg) - cmps_set.cmps;
lilac0112_1 9:c966191926c5 24 }
lilac0112_1 18:3a42a931c95a 25 void HmcReset(void){
lilac0112_1 18:3a42a931c95a 26 hmc_reset=HMC_RST;
lilac0112_1 18:3a42a931c95a 27 wait_us(100);
lilac0112_1 18:3a42a931c95a 28 hmc_reset=HMC_RUN;
lilac0112_1 18:3a42a931c95a 29 }
lilac0112_1 13:b20921316f3c 30 void TurnAttack(void){
lilac0112_1 13:b20921316f3c 31 cmps_set.FrontDeg += cmps_set.AtkDeg;
lilac0112_1 13:b20921316f3c 32 FrontHere();
lilac0112_1 13:b20921316f3c 33 //ResetDegree();
lilac0112_1 13:b20921316f3c 34 }
lilac0112_1 0:ea35c18c85fc 35 void ValidPidUpdate(void){
lilac0112_1 0:ea35c18c85fc 36 if(sys.PidFlag==0){
lilac0112_1 0:ea35c18c85fc 37 sys.PidFlag=1;
lilac0112_1 0:ea35c18c85fc 38 }
lilac0112_1 0:ea35c18c85fc 39 }
lilac0112_1 0:ea35c18c85fc 40 //motor
lilac0112_1 0:ea35c18c85fc 41
lilac0112_1 0:ea35c18c85fc 42 void ValidTx_motor(void){
lilac0112_1 0:ea35c18c85fc 43 if(sys.MotorFlag==0){
lilac0112_1 0:ea35c18c85fc 44 sys.MotorFlag=1;
lilac0112_1 0:ea35c18c85fc 45 }
lilac0112_1 0:ea35c18c85fc 46 }
lilac0112_1 0:ea35c18c85fc 47 void tx_motor(){//モーターへの送信
lilac0112_1 10:6df631c39f9b 48 array2(speed[1],-speed[0],-speed[2],-speed[3]);//右後左無
lilac0112_1 0:ea35c18c85fc 49 motor.printf("%s",StringFIN.c_str());
lilac0112_1 0:ea35c18c85fc 50 }
lilac0112_1 0:ea35c18c85fc 51 void move(int vx, int vy, int vs){//三輪オムニの移動（基本の形）
lilac0112_1 0:ea35c18c85fc 52 uint8_t i;
lilac0112_1 0:ea35c18c85fc 53 double pwm[4] = {0};
lilac0112_1 0:ea35c18c85fc 54
lilac0112_1 0:ea35c18c85fc 55 pwm[0] = (double)((vx) + vs);
lilac0112_1 0:ea35c18c85fc 56 pwm[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + vs);
lilac0112_1 0:ea35c18c85fc 57 pwm[2] = (double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy) + vs);
lilac0112_1 10:6df631c39f9b 58 pwm[3] = (sys.dri_pow)*(sys.DribbleFlag);
lilac0112_1 0:ea35c18c85fc 59
lilac0112_1 0:ea35c18c85fc 60 for(i = 0; i < 4; i++){
lilac0112_1 0:ea35c18c85fc 61 if(pwm[i] > 100){
lilac0112_1 0:ea35c18c85fc 62 pwm[i] = 100;
lilac0112_1 0:ea35c18c85fc 63 } else if(pwm[i] < -100){
lilac0112_1 0:ea35c18c85fc 64 pwm[i] = -100;
lilac0112_1 0:ea35c18c85fc 65 }
lilac0112_1 0:ea35c18c85fc 66 speed[i] = pwm[i];
lilac0112_1 0:ea35c18c85fc 67 }
lilac0112_1 0:ea35c18c85fc 68 }
lilac0112_1 0:ea35c18c85fc 69 //solenoid
lilac0112_1 0:ea35c18c85fc 70 void AvailableSolenoid(void){
lilac0112_1 0:ea35c18c85fc 71 if(sys.KickFlag==0){
lilac0112_1 0:ea35c18c85fc 72 sys.KickFlag = 1;
lilac0112_1 0:ea35c18c85fc 73 }
lilac0112_1 0:ea35c18c85fc 74 }
lilac0112_1 0:ea35c18c85fc 75 void DriveSolenoid(void){
lilac0112_1 0:ea35c18c85fc 76 sys.KickFlag = 0;
lilac0112_1 0:ea35c18c85fc 77 kicker=1;
lilac0112_1 0:ea35c18c85fc 78 Solenoid_timeout.attach(&SolenoidSignal, .5);
lilac0112_1 0:ea35c18c85fc 79 }
lilac0112_1 0:ea35c18c85fc 80 void SolenoidSignal(void){
lilac0112_1 0:ea35c18c85fc 81 kicker=0;
lilac0112_1 2:635947de1583 82 }
lilac0112_1 11:3efae754e6ef 83 //line
lilac0112_1 11:3efae754e6ef 84 void LineLiberate(void){
lilac0112_1 11:3efae754e6ef 85 LineKeeper = LINE_FREE;
lilac0112_1 11:3efae754e6ef 86 wait_us(10);
lilac0112_1 11:3efae754e6ef 87 LineKeeper = LINE_FIX;
lilac0112_1 11:3efae754e6ef 88 }
lilac0112_1 2:635947de1583 89 //math
lilac0112_1 2:635947de1583 90 uint8_t GetBit(uint8_t n, uint8_t bit){
lilac0112_1 2:635947de1583 91 return (n>>(bit-1))%2;
lilac0112_1 0:ea35c18c85fc 92 }