CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v10_T_Main

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

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/setup_command_active/setup.cpp@2:635947de1583, 2016-03-03 (annotated)

Committer:
lilac0112_1
Date:
Thu Mar 03 05:58:43 2016 +0000
Revision:
2:635947de1583
Parent:
0:ea35c18c85fc
Child:
4:52da8da146d4
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
lilac0112_1 0:ea35c18c85fc 5 void SetUp(void){//電源を入れた時の処理
lilac0112_1 0:ea35c18c85fc 6 //int i=1;
lilac0112_1 0:ea35c18c85fc 7 //solenoid
lilac0112_1 0:ea35c18c85fc 8 kicker=0;
lilac0112_1 0:ea35c18c85fc 9 //sw&lcd
lilac0112_1 0:ea35c18c85fc 10 Sw[0].mode(PullUp);
lilac0112_1 0:ea35c18c85fc 11 Sw[1].mode(PullUp);
lilac0112_1 0:ea35c18c85fc 12 Sw[2].mode(PullUp);
lilac0112_1 0:ea35c18c85fc 13 Sw[3].mode(PullUp);
lilac0112_1 0:ea35c18c85fc 14 Sw_ticker.attach(Sw_sample, 0.050);
lilac0112_1 0:ea35c18c85fc 15
lilac0112_1 0:ea35c18c85fc 16 Lcd.cls();
lilac0112_1 0:ea35c18c85fc 17 Lcd.print(lcdstr[0][0]);
lilac0112_1 0:ea35c18c85fc 18
lilac0112_1 0:ea35c18c85fc 19 //ball
lilac0112_1 0:ea35c18c85fc 20 BallChecker.mode(PullUp);
lilac0112_1 0:ea35c18c85fc 21
lilac0112_1 0:ea35c18c85fc 22 //value
lilac0112_1 0:ea35c18c85fc 23 sys.strategy = 0;
lilac0112_1 2:635947de1583 24 sys.ir_pow_table = 0;
lilac0112_1 0:ea35c18c85fc 25 sys.s_pow=30;
lilac0112_1 0:ea35c18c85fc 26 sys.l_pow=30;
lilac0112_1 2:635947de1583 27 sys.dri_pow=30;
lilac0112_1 2:635947de1583 28
lilac0112_1 2:635947de1583 29 //ir
lilac0112_1 2:635947de1583 30 data.irNotice=IR_NONE;
lilac0112_1 0:ea35c18c85fc 31
lilac0112_1 0:ea35c18c85fc 32 //spi
lilac0112_1 0:ea35c18c85fc 33 spi.format(16, 3);
lilac0112_1 0:ea35c18c85fc 34 spi.frequency(1000000);
lilac0112_1 0:ea35c18c85fc 35 spi_ss[0]=spi_ss[1]=spi_ss[2]=spi_ss[3]=1;
lilac0112_1 0:ea35c18c85fc 36
lilac0112_1 0:ea35c18c85fc 37 //motor
lilac0112_1 0:ea35c18c85fc 38 motor.baud(115200); //ボーレート設定
lilac0112_1 0:ea35c18c85fc 39 motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
lilac0112_1 0:ea35c18c85fc 40 //motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
lilac0112_1 0:ea35c18c85fc 41 //compass
lilac0112_1 0:ea35c18c85fc 42 hmc_reset=0;
lilac0112_1 0:ea35c18c85fc 43 for(int i=0; i<5; i++){
lilac0112_1 0:ea35c18c85fc 44 ReadCmps();
lilac0112_1 0:ea35c18c85fc 45 cmps_set.CmpsInitialValue = cmps_set.cmps;
lilac0112_1 0:ea35c18c85fc 46 wait_ms(10);
lilac0112_1 0:ea35c18c85fc 47 }
lilac0112_1 0:ea35c18c85fc 48 cmps_set.CmpsDiff = REFERENCE - cmps_set.cmps;
lilac0112_1 0:ea35c18c85fc 49 cmps_set.FrontDeg=0;
lilac0112_1 0:ea35c18c85fc 50
lilac0112_1 0:ea35c18c85fc 51 pid.setInputLimits(MINIMUM,MAXIMUM); //pid sed def
lilac0112_1 0:ea35c18c85fc 52 pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT); //pid sed def
lilac0112_1 0:ea35c18c85fc 53 pid.setBias(PID_BIAS); //pid sed def
lilac0112_1 0:ea35c18c85fc 54 pid.setMode(AUTO_MODE); //pid sed def
lilac0112_1 0:ea35c18c85fc 55 pid.setSetPoint(REFERENCE); //pid sed def
lilac0112_1 0:ea35c18c85fc 56 //pidupdate.attach(&PidUpdate, PID_CYCLE);
lilac0112_1 0:ea35c18c85fc 57 }
lilac0112_1 0:ea35c18c85fc 58 void SetUp2(void){//スタートした時の処理
lilac0112_1 0:ea35c18c85fc 59
lilac0112_1 0:ea35c18c85fc 60 __enable_irq(); // 許可
lilac0112_1 0:ea35c18c85fc 61
lilac0112_1 0:ea35c18c85fc 62 sys.KickOffFlag=1;
lilac0112_1 0:ea35c18c85fc 63 sys.KickFlag=0;
lilac0112_1 0:ea35c18c85fc 64 //solenoid
lilac0112_1 0:ea35c18c85fc 65 kicker=0;
lilac0112_1 0:ea35c18c85fc 66 //compass
lilac0112_1 0:ea35c18c85fc 67 hmc_reset=0;
lilac0112_1 0:ea35c18c85fc 68 for(int i=0; i<5; i++){
lilac0112_1 0:ea35c18c85fc 69 ReadCmps();
lilac0112_1 0:ea35c18c85fc 70 cmps_set.CmpsInitialValue = cmps_set.cmps;
lilac0112_1 0:ea35c18c85fc 71 wait_ms(10);
lilac0112_1 0:ea35c18c85fc 72 }
lilac0112_1 0:ea35c18c85fc 73 cmps_set.CmpsDiff = REFERENCE - cmps_set.cmps;
lilac0112_1 0:ea35c18c85fc 74 cmps_set.FrontDeg=0;
lilac0112_1 0:ea35c18c85fc 75
lilac0112_1 0:ea35c18c85fc 76 //Line_ticker.attach(&ReadLine, 0.005);
lilac0112_1 0:ea35c18c85fc 77 Line[A_SPOT].rise(&LineCall_A);
lilac0112_1 0:ea35c18c85fc 78 Line[B_SPOT].rise(&LineCall_B);
lilac0112_1 0:ea35c18c85fc 79 Line[C_SPOT].rise(&LineCall_C);
lilac0112_1 0:ea35c18c85fc 80 Solenoid_ticker.attach(&AvailableSolenoid, 6.0);
lilac0112_1 0:ea35c18c85fc 81 Ir_ticker.attach(&ValidIr, 0.050);
lilac0112_1 0:ea35c18c85fc 82 Ping_ticker.attach(&ValidPing, 0.050);
lilac0112_1 0:ea35c18c85fc 83 //Hmc_ticker.attach(&HmcReset, 1.0);
lilac0112_1 0:ea35c18c85fc 84 //motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
lilac0112_1 0:ea35c18c85fc 85 Motor_ticker.attach(&ValidTx_motor, 0.020);
lilac0112_1 0:ea35c18c85fc 86 pidupdate.attach(&ValidPidUpdate, PID_CYCLE);
lilac0112_1 0:ea35c18c85fc 87 }
lilac0112_1 0:ea35c18c85fc 88 void StopProcess(void){//コマンドに戻るときの処理
lilac0112_1 0:ea35c18c85fc 89 pidupdate.detach();
lilac0112_1 0:ea35c18c85fc 90 Motor_ticker.detach();
lilac0112_1 0:ea35c18c85fc 91 ////Line_ticker.detach();
lilac0112_1 0:ea35c18c85fc 92 Solenoid_ticker.detach();
lilac0112_1 0:ea35c18c85fc 93 Ir_ticker.detach();
lilac0112_1 0:ea35c18c85fc 94 Ping_ticker.detach();
lilac0112_1 0:ea35c18c85fc 95 ////Hmc_ticker.detach();
lilac0112_1 0:ea35c18c85fc 96 motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
lilac0112_1 0:ea35c18c85fc 97 move(0,0,0);
lilac0112_1 0:ea35c18c85fc 98 cmps_set.OutputPID=0;
lilac0112_1 0:ea35c18c85fc 99 cmps_set.InputPID=REFERENCE;
lilac0112_1 0:ea35c18c85fc 100 //solenoid
lilac0112_1 0:ea35c18c85fc 101 kicker=1;
lilac0112_1 0:ea35c18c85fc 102 wait(0.25);
lilac0112_1 0:ea35c18c85fc 103 kicker=0;
lilac0112_1 0:ea35c18c85fc 104
lilac0112_1 0:ea35c18c85fc 105 __disable_irq(); // 禁止
lilac0112_1 0:ea35c18c85fc 106 __enable_irq(); // 許可
lilac0112_1 0:ea35c18c85fc 107 Sw_ticker.attach(Sw_sample, 0.050);
lilac0112_1 0:ea35c18c85fc 108
lilac0112_1 0:ea35c18c85fc 109 }