CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_T_2v00

Main Program

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/setup_command_active/setup.cpp@27:769cb5a7ea37, 2016-01-31 (annotated)

Committer:
lilac0112_1
Date:
Sun Jan 31 15:46:19 2016 +0000
Revision:
27:769cb5a7ea37
Parent:
25:f98a7c2a5970
Child:
28:8ac6c3c1e643
plan; solenoid; ir_turn; line stop

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 19:342da3a5a474 7
lilac0112_1 19:342da3a5a474 8 //sw&lcd
lilac0112_1 19:342da3a5a474 9 Sw[0].mode(PullUp);
lilac0112_1 19:342da3a5a474 10 Sw[1].mode(PullUp);
lilac0112_1 19:342da3a5a474 11 Sw[2].mode(PullUp);
lilac0112_1 19:342da3a5a474 12 Sw[3].mode(PullUp);
lilac0112_1 24:9fb74ea3c25d 13 Sw_ticker.attach(Sw_sample, 0.050);
lilac0112_1 22:a95f7c63af3c 14
lilac0112_1 19:342da3a5a474 15 Lcd.cls();
lilac0112_1 19:342da3a5a474 16 Lcd.print(lcdstr[0][0]);
lilac0112_1 19:342da3a5a474 17
lilac0112_1 19:342da3a5a474 18 //ball
lilac0112_1 19:342da3a5a474 19 BallChecker.mode(PullUp);
lilac0112_1 19:342da3a5a474 20
lilac0112_1 19:342da3a5a474 21 //value
lilac0112_1 22:a95f7c63af3c 22 data.strategy = 0;
lilac0112_1 24:9fb74ea3c25d 23 data.motorlog[X_AXIS] = 0;
lilac0112_1 24:9fb74ea3c25d 24 data.motorlog[Y_AXIS] = 0;
lilac0112_1 19:342da3a5a474 25
lilac0112_1 19:342da3a5a474 26 //spi
lilac0112_1 21:d69a8f3c76e1 27 spi.format(16, 3);
lilac0112_1 21:d69a8f3c76e1 28 spi.frequency(1000000);
lilac0112_1 21:d69a8f3c76e1 29 spi_ss[0]=spi_ss[1]=spi_ss[2]=spi_ss[3]=1;
lilac0112_1 19:342da3a5a474 30
lilac0112_1 19:342da3a5a474 31 //motor
lilac0112_1 22:a95f7c63af3c 32 motor.baud(115200); //ボーレート設定
lilac0112_1 22:a95f7c63af3c 33 motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
lilac0112_1 19:342da3a5a474 34 //motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
lilac0112_1 19:342da3a5a474 35
lilac0112_1 19:342da3a5a474 36 for(int i=0; i<5; i++){
lilac0112_1 22:a95f7c63af3c 37 ReadCmps();
lilac0112_1 21:d69a8f3c76e1 38 data.CmpsInitialValue = data.cmps;
lilac0112_1 21:d69a8f3c76e1 39 wait_ms(10);
lilac0112_1 19:342da3a5a474 40 }
lilac0112_1 23:d95d8d3e89f3 41 data.CmpsDiff = REFERENCE - data.cmps;
lilac0112_1 21:d69a8f3c76e1 42 data.FrontDeg=0;
lilac0112_1 19:342da3a5a474 43
lilac0112_1 24:9fb74ea3c25d 44 pid.setInputLimits(MINIMUM,MAXIMUM); //pid sed def
lilac0112_1 24:9fb74ea3c25d 45 pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT); //pid sed def
lilac0112_1 24:9fb74ea3c25d 46 pid.setBias(PID_BIAS); //pid sed def
lilac0112_1 24:9fb74ea3c25d 47 pid.setMode(AUTO_MODE); //pid sed def
lilac0112_1 24:9fb74ea3c25d 48 pid.setSetPoint(REFERENCE); //pid sed def
lilac0112_1 19:342da3a5a474 49 //pidupdate.attach(&PidUpdate, PID_CYCLE);
lilac0112_1 19:342da3a5a474 50 }
lilac0112_1 19:342da3a5a474 51 void SetUp2(void){//スタートした時の処理
lilac0112_1 19:342da3a5a474 52 data.strategy=0;
lilac0112_1 24:9fb74ea3c25d 53
lilac0112_1 24:9fb74ea3c25d 54 data.motorlog[X_AXIS] = 0;
lilac0112_1 24:9fb74ea3c25d 55 data.motorlog[Y_AXIS] = 0;
lilac0112_1 24:9fb74ea3c25d 56 for(int i=0; i<5; i++){
lilac0112_1 24:9fb74ea3c25d 57 ReadCmps();
lilac0112_1 24:9fb74ea3c25d 58 data.CmpsInitialValue = data.cmps;
lilac0112_1 24:9fb74ea3c25d 59 wait_ms(10);
lilac0112_1 24:9fb74ea3c25d 60 }
lilac0112_1 24:9fb74ea3c25d 61 data.CmpsDiff = REFERENCE - data.cmps;
lilac0112_1 24:9fb74ea3c25d 62 data.FrontDeg=0;
lilac0112_1 24:9fb74ea3c25d 63
lilac0112_1 27:769cb5a7ea37 64 Line_ticker.attach(&ReadLine, 0.005);
lilac0112_1 24:9fb74ea3c25d 65 Solenoid_ticker.attach(&AvailableSolenoid, 6.0);
lilac0112_1 24:9fb74ea3c25d 66 Ir_ticker.attach(&ValidIr, 0.050);
lilac0112_1 24:9fb74ea3c25d 67 //motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
lilac0112_1 27:769cb5a7ea37 68 Motor_ticker.attach(&ValidTx_motor, 0.020);
lilac0112_1 27:769cb5a7ea37 69 pidupdate.attach(&ValidPidUpdate, PID_CYCLE);
lilac0112_1 27:769cb5a7ea37 70
lilac0112_1 27:769cb5a7ea37 71 data.motorlog[X_AXIS]=0;
lilac0112_1 27:769cb5a7ea37 72 data.motorlog[Y_AXIS]=0;
lilac0112_1 19:342da3a5a474 73 }
lilac0112_1 19:342da3a5a474 74 void StopProcess(void){//コマンドに戻るときの処理
lilac0112_1 24:9fb74ea3c25d 75 pidupdate.detach();
lilac0112_1 24:9fb74ea3c25d 76 Motor_ticker.detach();
lilac0112_1 24:9fb74ea3c25d 77 Line_ticker.detach();
lilac0112_1 24:9fb74ea3c25d 78 Solenoid_ticker.detach();
lilac0112_1 24:9fb74ea3c25d 79 Ir_ticker.detach();
lilac0112_1 24:9fb74ea3c25d 80 DriveSolenoid();
lilac0112_1 24:9fb74ea3c25d 81 motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
lilac0112_1 24:9fb74ea3c25d 82 move(0,0,0);
lilac0112_1 27:769cb5a7ea37 83 data.OutputPID=0;
lilac0112_1 27:769cb5a7ea37 84 data.InputPID=REFERENCE;
lilac0112_1 19:342da3a5a474 85 }