CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v10_T_Main

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

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy_parts/output.cpp@9:c966191926c5, 2016-03-08 (annotated)

Committer:
lilac0112_1
Date:
Tue Mar 08 09:52:22 2016 +0000
Revision:
9:c966191926c5
Parent:
2:635947de1583
Child:
10:6df631c39f9b
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 0:ea35c18c85fc 13 }
lilac0112_1 9:c966191926c5 14 void FrontHere(void){
lilac0112_1 9:c966191926c5 15 ReadCmps();
lilac0112_1 9:c966191926c5 16 cmps_set.CmpsInitialValue = cmps_set.cmps;
lilac0112_1 9:c966191926c5 17
lilac0112_1 9:c966191926c5 18 cmps_set.CmpsDiff = (REFERENCE-cmps_set.FrontDeg) - cmps_set.cmps;
lilac0112_1 9:c966191926c5 19 }
lilac0112_1 0:ea35c18c85fc 20 void ValidPidUpdate(void){
lilac0112_1 0:ea35c18c85fc 21 if(sys.PidFlag==0){
lilac0112_1 0:ea35c18c85fc 22 sys.PidFlag=1;
lilac0112_1 0:ea35c18c85fc 23 }
lilac0112_1 0:ea35c18c85fc 24 }
lilac0112_1 0:ea35c18c85fc 25 //motor
lilac0112_1 0:ea35c18c85fc 26
lilac0112_1 0:ea35c18c85fc 27 void ValidTx_motor(void){
lilac0112_1 0:ea35c18c85fc 28 if(sys.MotorFlag==0){
lilac0112_1 0:ea35c18c85fc 29 sys.MotorFlag=1;
lilac0112_1 0:ea35c18c85fc 30 }
lilac0112_1 0:ea35c18c85fc 31 }
lilac0112_1 0:ea35c18c85fc 32 void tx_motor(){//モーターへの送信
lilac0112_1 0:ea35c18c85fc 33 array2(speed[1],-speed[0],-speed[2],speed[3]);//右後左無
lilac0112_1 0:ea35c18c85fc 34 motor.printf("%s",StringFIN.c_str());
lilac0112_1 0:ea35c18c85fc 35 }
lilac0112_1 0:ea35c18c85fc 36 void move(int vx, int vy, int vs){//三輪オムニの移動（基本の形）
lilac0112_1 0:ea35c18c85fc 37 uint8_t i;
lilac0112_1 0:ea35c18c85fc 38 double pwm[4] = {0};
lilac0112_1 0:ea35c18c85fc 39
lilac0112_1 0:ea35c18c85fc 40 pwm[0] = (double)((vx) + vs);
lilac0112_1 0:ea35c18c85fc 41 pwm[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + vs);
lilac0112_1 0:ea35c18c85fc 42 pwm[2] = (double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy) + vs);
lilac0112_1 0:ea35c18c85fc 43 pwm[3] = 0;
lilac0112_1 0:ea35c18c85fc 44
lilac0112_1 0:ea35c18c85fc 45 for(i = 0; i < 4; i++){
lilac0112_1 0:ea35c18c85fc 46 if(pwm[i] > 100){
lilac0112_1 0:ea35c18c85fc 47 pwm[i] = 100;
lilac0112_1 0:ea35c18c85fc 48 } else if(pwm[i] < -100){
lilac0112_1 0:ea35c18c85fc 49 pwm[i] = -100;
lilac0112_1 0:ea35c18c85fc 50 }
lilac0112_1 0:ea35c18c85fc 51 speed[i] = pwm[i];
lilac0112_1 0:ea35c18c85fc 52 }
lilac0112_1 0:ea35c18c85fc 53 }
lilac0112_1 0:ea35c18c85fc 54 void move_rectan(int vx, int vy, int vs){//三輪オムニの移動（直交座標指定）
lilac0112_1 0:ea35c18c85fc 55 move(vx, vy, vs);
lilac0112_1 0:ea35c18c85fc 56 }
lilac0112_1 0:ea35c18c85fc 57 void move_polar(int degree, int power, int vs){//三輪オムニの移動（極座標指定）
lilac0112_1 0:ea35c18c85fc 58 int vx, vy, deg;
lilac0112_1 0:ea35c18c85fc 59 deg = (degree+5)/10;
lilac0112_1 0:ea35c18c85fc 60 vx = power*sin(DEG2RAD(deg));
lilac0112_1 0:ea35c18c85fc 61 vy = power*cos(DEG2RAD(deg));
lilac0112_1 0:ea35c18c85fc 62 move(vx, vy, vs);
lilac0112_1 0:ea35c18c85fc 63 }
lilac0112_1 0:ea35c18c85fc 64 //solenoid
lilac0112_1 0:ea35c18c85fc 65 void AvailableSolenoid(void){
lilac0112_1 0:ea35c18c85fc 66 if(sys.KickFlag==0){
lilac0112_1 0:ea35c18c85fc 67 sys.KickFlag = 1;
lilac0112_1 0:ea35c18c85fc 68 }
lilac0112_1 0:ea35c18c85fc 69 }
lilac0112_1 0:ea35c18c85fc 70 void DriveSolenoid(void){
lilac0112_1 0:ea35c18c85fc 71 sys.KickFlag = 0;
lilac0112_1 0:ea35c18c85fc 72 kicker=1;
lilac0112_1 0:ea35c18c85fc 73 Solenoid_timeout.attach(&SolenoidSignal, .5);
lilac0112_1 0:ea35c18c85fc 74 }
lilac0112_1 0:ea35c18c85fc 75 void SolenoidSignal(void){
lilac0112_1 0:ea35c18c85fc 76 kicker=0;
lilac0112_1 2:635947de1583 77 }
lilac0112_1 2:635947de1583 78 //math
lilac0112_1 2:635947de1583 79 uint8_t GetBit(uint8_t n, uint8_t bit){
lilac0112_1 2:635947de1583 80 return (n>>(bit-1))%2;
lilac0112_1 0:ea35c18c85fc 81 }