CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_T_2v00

Main Program

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/setup_command_active/setup.cpp@45:c23f25c00d0d, 2016-02-25 (annotated)

Committer:
lilac0112_1
Date:
Thu Feb 25 06:12:47 2016 +0000
Revision:
45:c23f25c00d0d
Parent:
43:f11f68918299 
Hokushinetsu(20:1)

Who changed what in which revision?

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