CatPot for defence on RoboCup in 2015 winter
Dependencies: AQM0802A HMC6352 MultiSerial PID Servo mbed
main.cpp
- Committer:
- lilac0112_1
- Date:
- 2015-01-27
- Revision:
- 1:e3248f278663
- Parent:
- 0:d35efbf4d62e
- Child:
- 2:39135c67083d
File content as of revision 1:e3248f278663:
/*********************************** *RoboCupJunior Soccer B Open 2014 *Koshinestu progrum * *このプログラムでは以下の処理をする. * LPC1114FN28/102からI2Cを用いて各種センサデータを収集 * * データからロボットの移動やキッカー等のモータの動作を決定する処理を行う * * MotorDriverにmaxonに命令 * * ServoMotorでステアリング * * LCDでデバック * * スイッチ4つとスタートスイッチで処理を実行 * * キッカー用のDigitalOutがどこかに入る * ************************* * *Pin Map * * p5,p6,p7,p29,p30 >> SPI >>Stepping * * p9,p10 >> I2C orSerial >> LPC1114FN28/102 read * * p13,p14 >> Serial >> Motor * * p22~p26 >> DebugSw and StartSw * * p27,p28 >> I2C >> DebugLCD * * p21 >> Kick * * p22 >> ServoMotor * ******************************/ /* ****以下IRはNighは距離が近いものとする. */ #include "mbed.h" #include "PID.h" #include "AQM0802A.h" #include "HMC6352.h" #include "Servo.h" #include "def.h" #include <math.h> #include <sstream> BusIn Sw(p22,p23,p24,p25,p26); DigitalOut Led[4] = {LED1,LED2,LED3,LED4}; I2C Sensor(p9,p10);//sda,scl HMC6352 hmc6352(p9,p10); AQM0802A Lcd(p28,p27);//sda,scl Servo Servo(p21); Serial Motor(p13,p14);//tx,rx Serial pc(USBTX,USBRX); DigitalOut Kick(p22); extern string StringFIN; extern void array(int,int,int,int); int speed[4] = {0}; /*Ir*/ uint8_t IrNum;//場所によるirの数を表したもの0~11まではボールがある状態12はボールがない状態 typedef enum {FRONT = 1, RIGHT, BACK, LEFT, NA} Direction; /*SensorData回収↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓*/ unsigned int IrReceiveR(){ /*順位解析版 Slave側はIRを要求した場合IRのみを返してくるとする. irは値が大きいほうが近いと仮定する */ char data_r[3],data_l[3]; bool val; Sensor.write(READ_IR);//一斉送信のつもり//コンパスへの影響はどのくらいですか val = Sensor.read(ADDRESS_R|1, data_r, 3);// IRデータを受信 Led[0] = val; wait_ms(5); val = Sensor.read(ADDRESS_L|1, data_l, 3); Led[0] = !val; if((data_r[0] == 0)||(data_l[0] == 0)){/*ボールを検知しているかチェック*/ if((data_r[0] == 0)&&(data_l[0] == 0)){ return 12; } if(data_r[0] == 0){ return (data_l[0]/6+6)*12*12 + (data_l[0]%6+6)*12 + data_r[0]/6; } return data_r[0]/6*12*12 + (data_r[0]%6)*12 + data_l[0]/6+6; } if(data_r[2]>data_l[2]){ if(data_r[2]>data_l[1]){ return data_r[0]/6*12*12 + (data_r[0]%6)*12 + data_l[0]/6+6; } return data_r[0]/6*12*12 +(data_l[0]/6+6)*12+ data_r[0]%6; }else{ if(data_l[2]>data_r[1]){ return (data_l[0]/6+6)*12*12 + (data_l[0]%6+6)*12 + data_r[0]/6; } return (data_l[0]/6+6)*12*12 +(data_r[0]/6)*12+ (data_l[0]%6+6); } } uint8_t IrReceiveS(){ /*単純解析版 一番大きい位置だけ返す Slave側はIRを要求した場合IRのみを返してくるとする. irは値が大きいほうが近いと仮定する */ char data_r[3],data_l[3]; bool val; Sensor.write(READ_IR);//一斉送信のつもり//コンパスへの影響はどのくらいですか val = Sensor.read(ADDRESS_R|1, data_r, 3);// IRデータを受信 Led[0] = val; wait_ms(5); val = Sensor.read(ADDRESS_L|1, data_l, 3); Led[0] = !val; if((data_r[0] == 0)||(data_l[0] == 0)){/*ボールを検知しているかチェック*/ if((data_r[0] == 0)&&(data_l[0] == 0)){ return 12; } if(data_r[0] == 0){ return data_l[0]/6+6; } return data_r[0]/6; } if(data_r[2]>data_l[2]){ return data_r[0]/6; } return data_l[0]/6+6; } uint8_t IrReceiveM(){ /*値解析版 一番近い場所の値を返す(位置はとりあえずない) Slave側はIRを要求した場合IRのみを返してくるとする. irは値が大きいほうが近いと仮定する */ char data_r[3],data_l[3]; bool val; Sensor.write(READ_IR);//一斉送信のつもり//コンパスへの影響はどのくらいですか val = Sensor.read(ADDRESS_R|1, data_r, 3);// IRデータを受信 Led[0] = val; wait_ms(5); val = Sensor.read(ADDRESS_L|1, data_l, 3); Led[0] = !val; if((data_r[0] == 0)||(data_l[0] == 0)){/*ボールを検知しているかチェック*/ if((data_r[0] == 0)&&(data_l[0] == 0)){ return 255; } if(data_r[0] == 0){ return data_l[1]; } return data_r[1]; } if(data_r[2]>data_l[2]){ return data_r[1]; } return data_l[1]; } unsigned int IrReceiveSM(){//16bit() /*値解析版 一番大きい値とその位置を返す Slave側はIRを要求した場合IRのみを返してくるとする. irは値が大きいほうが近いと仮定する */ char data_r[3],data_l[3]; bool val; Sensor.write(READ_IR);//一斉送信のつもり//コンパスへの影響はどのくらいですか val = Sensor.read(ADDRESS_R|1, data_r, 3);// IRデータを受信 Led[0] = val; wait_ms(5); val = Sensor.read(ADDRESS_L|1, data_l, 3); Led[0] = !val; if((data_r[0] == 0)||(data_l[0] == 0)){/*ボールを検知しているかチェック*/ if((data_r[0] == 0)&&(data_l[0] == 0)){ return (12<<8)+255; } if(data_r[0] == 0){ return (data_l[0]/6+6)<<8 + data_l[1]; } return data_r[0]/6<<8 + data_r[1]; } if(data_r[2]>data_l[2]){ return data_r[0]/6<<8 + data_r[1]; } return ((data_l[0]/6)+6) <<8 + data_l[1]; } uint8_t LineReceive(){ //先に要求しない場合はLineの状況を送信すること.//4bit //前ー右ー後ろー左 char data_r[2],data_l[2]; bool val; //example val = slave.read(address,data,length,repeat); val = Sensor.read(ADDRESS_R|1, data_r, 1); Led[1] = val; wait_ms(5); val = Sensor.read(ADDRESS_L|1, data_l, 1); Led[1] = !val; return data_r[0]<<2 + data_l[0]; } void PingReceiveRL(char ping[]){//ping[0]==Right,ping[1]==Left char ReadSelect[1] = {READ_PING}; bool val; val = Sensor.write(ADDRESS_R|0, ReadSelect , 1); Led[2] = val; val = Sensor.read(ADDRESS_R|1, ping, 2); Led[2] = !val; } void PingReceiveFB(char ping[]){//ping[0]==FRONT,ping[1]==BACK char ReadSelect[1] = {READ_PING}; bool val; val = Sensor.write(ADDRESS_L|0, ReadSelect , 1); Led[2] = val; val = Sensor.read(ADDRESS_L|1, ping, 2); Led[2] = !val; } /*SensorData回収↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑*/ /*モーター駆動処理↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓*/ void move(int vr, int vl){ double pwm[4] = {0}; uint8_t i = 0; pwm[0] = -vr; pwm[1] = 0; pwm[2] = 0; pwm[3] = vl; for(i = 0; i < 4; i++){ if(pwm[i] > 100){ pwm[i] = 100; }else if(pwm[i] < -100){ pwm[i] = -100; } speed[i] = pwm[i]; } } void ControlRobo0(int *CompassDef)//LeftFront 11時 { } void ControlRobo1(int *CompassDef)//Front 12時 { /*前にボールがある場合の動作*/ int Compass; char Ping[2]; uint8_t Line,LineIr; unsigned int IrNumMax; Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; if(!((Compass>=150)&&(Compass<=210))){ while(!((Compass>=150)&&(Compass<=210))){ move(20,-20);//適当な回転。 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; } return; } if(IrReceiveM()<=150){//適当な150 /*ステッピングを正面に設定*/ /*busy_wait()*/ /*モーターを前進*/ return; } PingReceiveRL(Ping); if((Ping[0]>90)&&(Ping[1]>40)){//数値は適当 /*ステッピングを少しだけ傾ける*/ /*モーターを左右差つけて回す(ロボットは少し傾く)*/ /*busy_wait()*/ /*モーターを前進*/ while(1){ Line = LineReceive(); if(Line){ //ラインを検知していないか LineIr = Line & (IrReceiveS()/3 + 1);//計算により方位を合わせる。 while(LineIr){ move(0,0);// Led[1] = Led[2] = Led[3] = 1; LineIr = LineReceive() & (IrReceiveS()/3 + 1); } Led[1] = Led[2] = Led[3] = 0; Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; while(!((Compass>=150)&&(Compass<=210))){ move(20,-20);//適当な回転。 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; } return; } Kick = 1; wait_ms(200); Kick = 0; IrNumMax = IrReceiveSM();//位置と大きさ if(!((IrNumMax&0x00)>>8) == 1){//この1はボールの関数の1を表す. Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; while(!((Compass>=150)&&(Compass<=210))){ move(20,-20);//適当な回転。 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; } return; } if((IrNumMax&0x00FF)<150){ Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; while(!((Compass>=150)&&(Compass<=210))){ move(20,-20);//適当な回転。 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; } return; } } }else if((Ping[0]>40)&&(Ping[1]>90)){ }else{ } } void ControlRobo2(int *CompassDef)//RightFront 1時 { int Compass; char Ir[2] ={0};//1, 1,2位, 1位の大きさ char ir_num,line;//わかりやすい名前 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; if(!((Compass>=150)&&(Compass<=210))){ while(!((Compass>=150)&&(Compass<=210))){ move(20,-20);//適当な回転。 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; } move(0,0); return; } /*irデータ取得 1,2位の場所、一位の距離*/ if(!(Ir[0]%13 ==2)){ return; } if(Ir[1]>=100){//近い場合 //2位を含んだ計算 /*motorset*/ /*stepset*/ return; } /*2位を含んだ計算*/ /*計算された分stepmove*/ move(20,20); /*line,ir get*/ do{ if(line){ move(0,0); return; } /*line,ir get*/ }while(ir_num == 2); move(0,0); } void ControlRobo3(int *CompassDef)//RightFront 2時 { int Compass; char Ir[2] ={0};//1, 1,2位, 1位の大きさ char ir_num,line;//わかりやすい名前 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; if(!((Compass>=150)&&(Compass<=210))){ while(!((Compass>=150)&&(Compass<=210))){ move(20,-20);//適当な回転。 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; } move(0,0); return; } /*irデータ取得 1,2位の場所、一位の距離*/ if(!(Ir[0]%13 ==3)){ return; } if(Ir[1] >= 100){ /*2位を含めた計算*/ /*move()*/ /*Stepset*/ return; } /*2位を含めた計算*/ /*Stepsetting 距離等により角度が変わる*/ move(-20,-20); /*ir,line check*/ do{ if(line){ move(0,0); return; } /*line,ir check*/ }while(ir_num == 3); move(0,0); } void ControlRobo4(int *CompassDef)//Right 3時 { int a = 50; unsigned int ir_sm; double value = 0; double v = 50;/* cm/s */ double x = 20;/*モーターを回す割合,今は適当*/ ir_sm = IrReceiveSM(); if(ir_sm&0xF00>>8 == 4){ return; } a = 300 - ir_sm&0x00FF ;//適当な計算//300は勘 value = 10 * a * SHORT_LEN + 3*(a * a + SHORT_LEN * SHORT_LEN); value = PI *(3 * ( a + SHORT_LEN) - sqrt(value))/4/v; if(300 - ir_sm&0x00FF >200){ //Step.Step(-10);//適当 } /*SetPerm() *valueに応じた何かを計算する *ACCまたはMAX_SPEEDをいじればいい気がする * */ //Step.Step(kakudo+10); /*x = v*何か*/ move(-x,-x); } void ControlRobo5(int *CompassDef)//RightBack 4時 { int Compass; char Ir[2] ={0};//1, 1,2位, 1位の大きさ char ir_num,line;//わかりやすい名前 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; if(!((Compass>=150)&&(Compass<=210))){ while(!((Compass>=150)&&(Compass<=210))){ move(20,-20);//適当な回転。 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; } move(0,0); return; } Servo.position(HOME); /*irデータ取得 1,2位の場所、一位の距離*/ if(!(Ir[0]%13 == 5)){ return; } if(Ir[1] >=100){//近い /*計算*/ Servo.position(ONE); move(-20,-20); /*Step.setpaaa Step.move();-180+初期位置 */ /*ir,linecheck*/ do{ if(line){ move(0,0); return; } if(!(ir_num >1 && ir_num <=5)){ move(0,0); return; } /*ir,line check*/ }while(1); } //遠い /*計算*/ Servo.position(FOUR); move(-20,-20); /*ir,linecheck*/ do{ if(line){ move(0,0); return; } if(!(ir_num >1 && ir_num <=5)){ move(0,0); } /*ir,line check*/ }while(1); } void ControlRobo6(int *CompassDef)//BackRight 5時 { } void ControlRobo7(int *CompassDef)//Back 6時 { /***** * this function is drawing a semicircle. * semicircle manipulated ir_data&ping_data. * * 変数名は後から変えるためにわかりやすい名前にしておく **/ uint8_t ir_num; uint8_t pingl,pingr; /* ir(1位,2位,必要なら距離も)と超音波のデータを取得 */ if(ir_num==7){//一致しているかどうか念のため return ; } if(pingr<pingl){//本当ならば壁と垂直かどうか、確認すべき。コンパスや超音波の合計を見れば可能 /*軌道の計算、楕円の半分見たいな感じの軌道がベスト*/ /*半円なのであらかじめステッピングを回転させる必要がある*/ /*モーター設定*/ /*ステッピング設定*/ return; } /*軌道の計算、楕円の半分見たいな感じの軌道がベスト*/ /*半円なのであらかじめステッピングを回転させる必要がある*/ /*モーター設定*/ /*ステッピング設定*/ } void ControlRobo8(int *CompassDef)//BackLeft 7時 { Servo.position(FOUR); } void ControlRobo9(int *CompassDef)//LeftBack 8時 { Servo.position(FIVE); } void ControlRobo10(int *CompassDef)//Left 9時 { Servo.position(SIX); } void ControlRobo11(int *CompassDef)//LeftFront 10時 { Servo.position(NINE); } void GoHome(int *CompassDef)//Ball is not found. { /*line検知をしないバージョン*/ int Compass; char ping[2] = {0}; Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; if(!((Compass>=150)&&(Compass<=210))){ while(!((Compass>=150)&&(Compass<=210))){ move(20,-20);//適当な回転。 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; } /*return;してもいいかもしれない*/ } PingReceiveFB(ping); if(ping[1] >=60){//後ろからの距離60cm while(ping[1] >=60){ move(-20,-20); PingReceiveFB(ping); } } move(0,0);//stop } /*モーター駆動処理↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑*/ uint8_t SwRead(){ /****** *返却値はスイッチの状況 *参照元::aaaaa * *Calibration = 0x01; *Kicker = 0x02; *Debug1 = 0x04; *Debug2 = 0x08; *StartS = 0x10; * *****/ uint8_t i,temp,temp2; temp = Sw; if(!(temp == Calibration ||temp == Kicker ||temp == Debug1 ||temp == Debug2 ||temp == StartS)) return 0;/*スイッチは押されていない状況*/ if(temp !=0x00){ for(i = 0; i < 50; i++); temp2 = Sw; if(temp == temp2){ return temp; } } return 0; } //通信(モータ用) void tx_motor(){ array(speed[0],speed[1],speed[3],speed[2]); Motor.printf("%s",StringFIN.c_str()); } void SetUp(int *compass_def){ /*初期化*/ Kick = 0; Sw.mode(PullUp); Motor.baud(115200); //ボーレート設定 Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止 Motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み(モータ用) //move(0,0);//停止 //Step.Resets(); //Step.ReleseSW(0,1); Servo.calibrate(0.0006, 120.0); Servo.position(HOME);//タイヤを前に向ける /* hmc6352.setOpMode(HMC6352_CONTINUOUS, 1, 20); *compass_def = (hmc6352.sample() / 10); Lcd.cls(); */ /*初期化終了*/ } void StartLoop(){ /* *スイッチが押されるまでロボットはスタートしない. * *待っている間にほかのスイッチが押された場合は押されている間その動作をする等. * *switch割り当て *1.コンパスのキャリブレーション実行スイッチ *2.キッカーのキック(check用) *3,4.自由 *5.StartSw */ uint8_t State = 0; while(1){ State = SwRead(); if(State == 0) continue; if(State == Calibration){ /*calibration command enter... * * */ continue; } if(State == Kicker){ /* *kicker check * * */ continue; } if(State == Debug1){ /* debug command free * *display out to selected 3 menus. compass, ir, ping, line,etc... * **/ } if(State == Debug2){ /* debug command free * * decide movement of the beginning. * * **/ } if(State == StartS){ /*game start...*/ return; } } } void ClockPointing(int to_st, int power){ /* to_st 時計において何時の方向にサーボを動かしたいか power タイヤのモータのパワー.絶対値. from_stで現在のサーボ状態保存.(初期がHOMEであること前提) これを利用して,3時⇔9時の移動のみサーボを動かさずにタイヤの駆動方向の反転のみで状態遷移可能. 改善の余地としてはワンショットタイマーを加えて 割り込み関数が作動するまでサーボ駆動時のタイヤの高出力を控え, 割り込み関数で引数の通りの出力でタイヤを駆動するというものがある. しかし場合によってはサーボの移動距離が短いときネックになることがある. 使うときはサーボの移動距離に応じて処理を変えるなどの工夫が必要になる. */ static int from_st = TWELFTH; static int qb=0; int TireDir[] = {1, 1, 1, -1, -1, -1, -1, -1, 1, 1, 1, 1};//1時、2時、3時、4時…12時//タイヤの駆動方向 float ClockDir[]={ONE, TWO, THREE, FOUR, FIVE, SIX, SEVEN, EIGHT, NINE, TEN, ELEVEN, TWELVE};//サーボの方向 //想定外の引数が来た時の対処 if( (to_st > TWELFTH) || (to_st < FIRST) || (from_st > TWELFTH) || (from_st < FIRST) ) return; if( (from_st == THIRD || from_st == NINTH) && (to_st == THIRD || to_st == NINTH) && (from_st != to_st) ){ qb = !qb; if(qb) for(int i=0; i<12; i++) TireDir[i] = TireDir[i]*(-1);//タイヤの駆動方向反転 } else{ qb=0; Servo.position(ClockDir[to_st]);//サーボの方向転換 } move(abs(power)*TireDir[to_st], abs(power)*TireDir[to_st]);//タイヤの出力 from_st = to_st;//状態(サーボの)のプログラム上の遷移 } int main() { /*Ir*/ //uint8_t IrNum;//場所によるirの数を表したもの0~11まではボールがある状態12はボールがない状態 //→global変数 /*Line*/ uint8_t Line; /*Compass*/ int CompassDef = 0, Compass = 0; /*State */ //Direction LineIr = NA;//方位設定奴...エラーでてめんどくさいので後でまたやることにする。 uint8_t LineIr = NA; /*関数ポインタ*/ void (*ControlRobo[13])(int *);//irの位置による分岐 ControlRobo[0] = ControlRobo0; ControlRobo[1] = ControlRobo1; ControlRobo[2] = ControlRobo2; ControlRobo[3] = ControlRobo3; ControlRobo[4] = ControlRobo4; ControlRobo[5] = ControlRobo5; ControlRobo[6] = ControlRobo6; ControlRobo[7] = ControlRobo7; ControlRobo[8] = ControlRobo8; ControlRobo[9] = ControlRobo9; ControlRobo[10] = ControlRobo10; ControlRobo[11] = ControlRobo11; ControlRobo[12] = GoHome; SetUp(&CompassDef);/*set up routine*/ //StartLoop(); /*loop stat, switch push end*/ /* 前を向くように設定をするべき アウトオブバウンズからの復帰時に反対を向けてスタートなので必要。 */ while(0){//前を向かせるwhile Compass = ((hmc6352.sample() / 10) + 540 - CompassDef) % 360; if(Compass == 3) break;//前を向いたら抜ける。 } while(0) { Line = LineReceive(); if(Line){ //ラインを検知していないか LineIr = Line & (IrReceiveS()/3 + 1);//計(lineとirの方位を合わせる。 while(LineIr){ move(0,0);// Led[1] = Led[2] = Led[3] = 1; LineIr = LineReceive() & (IrReceiveS()/3 + 1); } Led[1] = Led[2] = Led[3] = 0; continue; } IrNum = IrReceiveS(); (*ControlRobo[IrNum])(&CompassDef);//irの位置によったループ等の処理に移る。 //wait_ms(0); } move(0,0); Servo.position(HOME); uint8_t clock[]={TWELFTH, THIRD, SIXTH, NINTH}; uint8_t clock2[]={FIRST, SECOND, THIRD, FOURTH, FIFTH, SIXTH, SEVENTH, EIGHTH, NINTH, TENTH, ELEVENTH, TWELFTH}; Servo.position(HOME); while(1){ /*デモプログラム * *正方形の軌道 * *円軌道 * *反復横跳び * *関数ClockPointingを活用することで少しだけ楽に以上の動きを実現できる. */ for(int i=0; i < 4; i++){ ClockPointing(clock[i], 20); wait(2); } for(int i=0; i < 12; i++){ ClockPointing(clock2[i], 20); wait(1); } for(int i=0; i < 4; i++){ ClockPointing(THIRD, 20); wait(2); ClockPointing(NINTH, 20); wait(2); } } }