CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v10_T_Main

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

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy_parts/output.cpp@13:b20921316f3c, 2016-03-11 (annotated)

Committer:
lilac0112_1
Date:
Fri Mar 11 00:39:21 2016 +0000
Revision:
13:b20921316f3c
Parent:
11:3efae754e6ef
Child:
18:3a42a931c95a
somehow

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