CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v10_T_Main

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

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy_parts/output.cpp@0:ea35c18c85fc, 2016-02-27 (annotated)

Committer:
lilac0112_1
Date:
Sat Feb 27 09:14:37 2016 +0000
Revision:
0:ea35c18c85fc
Child:
2:635947de1583
maincode for japan open.

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