Dependencies: PID Serialservo TCS3200 XQ_servo encoder encoder2 mbed moter
Fork of nhk_2018_undercarry_test09 by
Diff: main.cpp
- Revision:
- 4:eabb7a738ff8
- Parent:
- 3:aaa2fde4fafd
- Child:
- 5:c5a2a5cf600d
diff -r aaa2fde4fafd -r eabb7a738ff8 main.cpp --- a/main.cpp Sat Mar 24 10:50:25 2018 +0000 +++ b/main.cpp Wed Mar 28 06:49:41 2018 +0000 @@ -2,22 +2,57 @@ #include "config.h" #include "moter.h" #include "PID.h" +#include "TCS3200.h" #include "encoder.h" +#include "encoder2.h" +#include "Serialservo.h" +#include "XQ_servo.h" Serial pc(USBTX, USBRX); Serial kbtpc(PC_12 ,PD_2);//serial5 I2C master(sda, scl); -DigitalOut led2(LED2); +DigitalOut led2(LED2);//チェック用 PID S_pid(SP, SI, SD, 0.0); PID R_pid(RP, RI, RD, 0.0); PID L_pid(LP, LI, LD, 0.0); + +//DigitalIn zpin(PA_1); + Encoder Enc_Carry_Y(PA_15,PA_14,PC_1); Encoder Enc_Carry_X(PA_4,PB_0,PB_7); +Encoder2 Enc_Arm(PF_1,PA_0,PA_1); + DigitalOut buzzer(PA_10); DigitalOut lamp(PC_3); Timer timer; +Serialservo servo1(PC_10, PC_11);//アームの肘 +Serialservo servo2(PC_10, PC_11);//回転軸 +XQ_servo servo_hand(PB_5); +//TCS3200 color(PB_5, PB_2, PC_4); +DigitalOut dpin(PC_9); +DigitalIn distance(PC_2); +DigitalIn redIn(PC_4); +DigitalIn blueIn(PB_2); +DigitalIn micro_switch(PC_0); +DigitalOut air(PB_4); + +DigitalOut motor_dir1(PA_8); //旧ピン番号 +DigitalOut motor_dir2(PA_9); +PwmOut motor_pwm(PB_3); + +/* +PwmOut motor_pwm(PA_9); +DigitalOut motor_dir1(PB_3); +DigitalOut motor_dir2(PA_8); +*/ +//DigitalOut motor_relay(PB_10); + void linecheck(char *buff ,int data[2]); void LineCheck(int dmode); +void syokikanomai(void); +void syokikanomai2(void); +void pre_touteki(void); +void touteki(float armpower, int pos, bool thro_mode); PwmOut pwm_pins[] = { PwmOut( PC_8 ), @@ -66,10 +101,32 @@ int state = START; int times = 0; bool throwing = false; +int receive = NOT_RECEIVE; +int redcount = 0; +int bluecount = 0; +int yellowcount = 0; +bool syokika_flag=false; +bool set_flag = false; +int encoder_count = 1000; + int main() { timer.reset(); timer.stop(); - //timer.start(); + servo1.init(0);//アーム + servo2.init(1);//回転軸 + //servo2.move(50); + //servo1.move(152); + dpin=0; + lamp=1; + air=1.0; + buzzer = 0.0; + //motor_relay = 0.0; + micro_switch.mode(PullUp); + + motor_pwm=0; + motor_dir1=1; //dir1,dir2はこの組み合わせじゃないと死ぬ + motor_dir2=1; + S_pid.init(); R_pid.init(); L_pid.init(); @@ -77,27 +134,22 @@ pwm_pins[1].period(0.00005); pwm_pins[2].period(0.00005); pwm_pins[3].period(0.00005); + motor_pwm.period(0.00005); pc.baud(115200); kbtpc.baud(9600); master.frequency(100000); - lamp = 1.0; + lamp = 1.0; +//-------初期化---------------------------------------- + syokikanomai(); + buzzer = 0; +//-----------メインループ----------------------------------------め while (true) { - //if(directmode == STRAIGHT){ - master.read(addr1,buff1,2); - linecheck(buff1,linedata_1); - //} - //if(directmode == RIGHT){ - master.read(addr2,buff2,2); - linecheck(buff2,linedata_2); - //} - //if(directmode == LEFT){ - master.read(addr3,buff3,2); - linecheck(buff3,linedata_3); - //} - //master.read(addr1,buff1,2); - //linecheck(buff1,linedata_1); - //linecheck(buff1,linedata_1); - //linecheck(buff1,linedata_1); + master.read(addr1,buff1,2); + linecheck(buff1,linedata_1); + master.read(addr2,buff2,2); + linecheck(buff2,linedata_2); + master.read(addr3,buff3,2); + linecheck(buff3,linedata_3); x_dist = Enc_Carry_X.read_rotate(); y_dist = Enc_Carry_Y.read_rotate(); kbtpc.putc(state); @@ -122,6 +174,8 @@ state = MANUAL; }else if(kbtread == START){ state = START; + }else if(kbtread == START_TEST){ + state = START_TEST; } //--------------手動-------------------------------------- if(mode == true){ @@ -140,13 +194,12 @@ //--------------自動--------------------------------- }else if(mode == false){ led2 = 0.0; - //timer.reset(); - //timer.start(); if(stopflag == true){ Stop(pwm_pins, dir_pins_1, dir_pins_2); }else{ LineCheck(directmode); switch(state){ +//------------------任意に進行方向を決めれる---------------------------------------- case MANUAL: if(crosscount == 1){ directmode = STOP; @@ -154,8 +207,66 @@ crosscount = 0; } break; - - //------------------------------------------ +//------------------テスト用-------------------------------------------------------- + case START_TEST: + if(crosscount == 0){ + directmode = RIGHT; + }else if(crosscount == 1){ + directmode = STOP; + state = STAND_BY_TEST; + crosscount = 0; + prelinedata = 0; + } + break; +//------------------テスト用待機位置----------------------------------------------- + case STAND_BY_TEST: + directmode = STOP; + if(distance == 1.0){ + led2 = 1.0; + if(redIn == 1.0){ + buzzer = 1.0; + redcount++; + }else if(blueIn == 1.0){ + buzzer = 1.0; + bluecount++; + }else{ + buzzer = 0.0; + } + if(redcount >= 50){ + servo_hand.move(hand_tsukami_degree); + //pre_touteki(); + }else if(bluecount >= 50){ + servo_hand.move(hand_tsukami_degree); + //pre_touteki(); + } + }else if(distance == 0.0 && redcount >= 50){ + servo2.move(koshi_nage_degree); + state = THROWING_TEST; + directmode = STOP; + led2 = 0.0; + redcount = 0; + crosscount = 0; + }else if(distance == 0.0 && bluecount >= 50){ + servo2.move(koshi_nage_degree); + state = THROWING_TEST; + directmode = STOP; + bluecount = 0; + led2 = 0.0; + crosscount = 0; + } + break; +//------------------------------------------------------------------------------ + case THROWING_TEST: + if(crosscount == 0){ + directmode = LEFT; + }else if(crosscount == 1){ + directmode = STOP; + touteki(0.8, 190, 0); + //syokikanomai(); + buzzer = 0.0; + } + break; +//------------------初期位置から待機位置1------------------------------------------ case START: if(crosscount == 0){ directmode = STRAIGHT; @@ -171,49 +282,71 @@ prelinedata = 0; } break; - //----------------------------------------------- +//------------------待機位置1---------------------------------------------------- case STAND_BY_1: crosscount = 0; directmode = STOP; - //buzzer = 1.0; - if(kbtread == RED_RECEIVE){ + if(distance == 1.0){ + if(redIn == 1.0){ + buzzer = 1.0; + redcount++; + }else if(blueIn == 1.0){ + buzzer = 1.0; + bluecount++; + }else{ + buzzer = 0.0; + } + if(redcount >= 100 ){ + servo_hand.move(hand_tsukami_degree); + servo1.move(hiji_tsukami_degree); + //pre_touteki(); + }else if(bluecount >= 100){ + servo_hand.move(hand_tsukami_degree); + servo1.move(hiji_tsukami_degree); + //pre_touteki(); + } + }else if(distance == 0.0 && redcount >= 100){ + servo2.move(koshi_nage_degree); state = RED_RECEIVE; - }else if(kbtread == BLUE_RECEIVE){ + directmode = STOP; + redcount = 0; + crosscount = 0; + }else if(distance == 0.0 && bluecount >= 100){ + state = BLUE_RECEIVE_1; + directmode = STOP; + bluecount = 0; + crosscount = 0; + } + /* + if(receive == RED_RECEIVE){ + state = RED_RECEIVE; + }else if(receive == BLUE_RECEIVE){ state = BLUE_RECEIVE_1; } + */ break; +//------------------待機位置1から投てき位置1---------------------------------------- case RED_RECEIVE: if(crosscount == 0){ directmode = LEFT; - }else if(crosscount == 1 && throwing == false){ + }else if(crosscount == 1){ directmode = STOP; prelinedata = 0; - if(kbtread == THROWING){ - throwing = true; - buzzer = 1.0; - timer.start(); - //lamp = 0.0; - //wait(3.0); - //buzzer = 0.0; - prelinedata = 0; - //directmode = RIGHT; - } - }else if(crosscount == 1 && throwing == true){ - times = timer.read(); - if(times > 2){ - lamp = 0.0; - buzzer = 0.0; + touteki(0.8,190,0); + if(set_flag == false){ directmode = RIGHT; - timer.reset(); - timer.stop(); } }else if(crosscount == 2){ directmode = STOP; - state = STAND_BY_1; - throwing = false; - crosscount = 0; + syokikanomai2(); + if(set_flag == true){ + state = STAND_BY_1; + //throwing = false; + crosscount = 0; + } } break; +//------------------待機位置1から投てき位置2---------------------------------------- case BLUE_RECEIVE_1: if(crosscount == 0){ directmode = RIGHT; @@ -224,18 +357,44 @@ }else if(crosscount == 3){ directmode = STOP; prelinedata = 0; - state = STAND_BY_2; + state = BLUE_RECEIVE_2; crosscount = 0; } break; +//------------------待機位置2---------------------------------------------------- case STAND_BY_2: crosscount = 0; + directmode = STOP; + if(distance == 1.0){ + if(blueIn == 1.0){ + buzzer = 1.0; + bluecount++; + }else if(micro_switch == 1.0){ + buzzer = 1.0; + yellowcount++; + }else{ + buzzer = 0.0; + } + if(bluecount >= 50){ + pre_touteki(); + }else if(yellowcount >= 500){ + air = 1.0; + } + }else if(distance == 0.0 && bluecount >= 50){ + state = BLUE_RECEIVE_2; + directmode = STOP; + bluecount = 0; + crosscount = 0; + } + /* if(kbtread == BLUE_RECEIVE){ state = BLUE_RECEIVE_2; }else if(kbtread == YELLOW_RECEIVE){ state = YELLOW_RECEIVE; } + */ break; +//------------------投てき位置2--------------------------------------------------- case BLUE_RECEIVE_2: if(crosscount == 0){ directmode = LEFT; @@ -246,10 +405,11 @@ if(kbtread == THROWING){ throwing = true; buzzer = 1.0; - timer.start(); + //timer.start(); prelinedata = 0; } }else{ + /* times = timer.read(); if(times > 2){ buzzer = 0.0; @@ -257,6 +417,7 @@ timer.reset(); timer.stop(); } + */ } }else if(crosscount == 2){ directmode = STOP; @@ -265,6 +426,7 @@ crosscount = 0; } break; +//------------------投てき位置3--------------------------------------------------- case YELLOW_RECEIVE: if(crosscount != 4){ directmode = LEFT; @@ -275,20 +437,12 @@ default: break; } - /* - if(crosscount == 1){ - directmode = STOP; - Stop(pwm_pins, dir_pins_1, dir_pins_2); - crosscount = 0; - } - */ +//--------------進行方向--------------------------------------------------------- switch(directmode){ case STOP: Stop(pwm_pins, dir_pins_1, dir_pins_2); break; case STRAIGHT: - //master.read(addr1,buff1,2); - //linecheck(buff1, linedata_1); output = S_pid.compute((double)linedata_1[0]); if(output >= 0){ rpower = power; @@ -301,8 +455,6 @@ } break; case RIGHT: - //master.read(addr2,buff2,2); - //linecheck(buff2, linedata_2); output = R_pid.compute((double)linedata_2[0]); if(output >= 0){ rpower = power; @@ -315,8 +467,6 @@ } break; case LEFT: - //master.read(addr3,buff3,2); - //linecheck(buff3, linedata_3); output = L_pid.compute((double)linedata_3[0]); if(output >= 0){ rpower = power; @@ -338,11 +488,11 @@ //pc.printf("buff2[0] = %d \r" ,buff2[0]); //pc.printf("line1 = %d \r" ,linedata_1[0]); //pc.printf("line2 = %d \r" ,linedata_3[1]); - //pc.printf("xdist = %f \r",x_dist); + pc.printf("xdist = %f \r",x_dist); //pc.printf("tar_xdist = %f \r",tar_x_dist); //pc.printf("ontheline = %d \r",ontheline); - //pc.printf("ydist = %f \r",y_dist); + pc.printf("ydist = %f \r",y_dist); //pc.printf("tary = %f \r",tar_y_dist); //pc.printf("stopflag = %d \r",stopflag); //pc.printf("cross = %d \r",crosscount); @@ -350,11 +500,156 @@ //pc.printf("line = %d" ,line); //pc.printf("rpower = %lf" ,rpower); //pc.printf("lpower = %lf" ,lpower); - pc.printf("dicmode = %d \r",directmode); + //pc.printf("dicmode = %d \r",directmode); pc.printf("output = %lf \n" ,output); } } +//---------------初期化--------------------------- + + +void syokikanomai(void){ + //timer.start(); + //int times = timer.read(); + syokika_flag = false; + while (set_flag==false) { + encoder_count= Enc_Arm.read_rotate(); + servo_hand.move(hand_uke_degree); + servo1.move(hiji_nage_degree);//退避 + servo2.move(koshi_uke_degree); + //motor_relay = 0; + motor_dir1=0; + motor_dir2=1; + motor_pwm=0.15; + + if(Enc_Arm.read_z() == 1) { + syokika_flag=true; + buzzer = 1.0; + } + + if (syokika_flag==true) { + if(encoder_count == 170) { + motor_pwm=0; + motor_dir1=1; + motor_dir2=1; + set_flag=true; //投てき後,falseにする. + } + } + } + //if(times >= 1.0){ + //wait(0.5); + //servo_hand.move(hand_uke_degree); + servo1.move(hiji_uke_degree); + servo1.move(hiji_uke_degree); + //servo2.move(koshi_uke_degree); + //} +} + +void syokikanomai2(void){ + //timer.start(); + //int times = timer.read(); + //syokika_flag = false; + if(set_flag==false) { + encoder_count= Enc_Arm.read_rotate(); + servo_hand.move(hand_uke_degree); + servo1.move(hiji_nage_degree);//退避 + servo2.move(koshi_uke_degree); + //motor_relay = 0; + motor_dir1=0; + motor_dir2=1; + motor_pwm=0.15; + + if(Enc_Arm.read_z() == 1) { + syokika_flag = true; + buzzer = 1.0; + } + + if (syokika_flag==true) { + if(encoder_count == 170) { + motor_pwm=0; + motor_dir1=1; + motor_dir2=1; + servo1.move(hiji_uke_degree); + servo1.move(hiji_uke_degree); + syokika_flag = false; + set_flag = true; //投てき後,falseにする. + } + } + } + //if(times >= 1.0){ + //wait(0.5); + //servo_hand.move(hand_uke_degree); + //servo2.move(koshi_uke_degree); + //} +} + +void pre_touteki(void){ + /* + while(Enc_Arm.read_rotate()<pos) { + motor_dir1 = 0; + motor_dir2 = 1; + motor_pwm = 0.15; + } + motor_dir1=1; + motor_dir2=1; + motor_pwm=0; + */ + //wait(1.0); + servo_hand.move(hand_tsukami_degree); + + //wait(1.0); + servo2.move(koshi_nage_degree); + //servo1.move(hiji_tsukami_degree); +} + +void touteki(float armpower ,int pos, bool thro_mode){ + if(set_flag == true){ + timer.start(); + } + int times = timer.read(); + //wait(1.0); + if(times >= 1.0 && times < 2.0){ + servo_hand.move(hand_uke_degree); + } + //wait(1.0); + //lamp = 0.0; + + if(times >= 2.0 && times < 3.0){ + servo1.move(hiji_nage_degree); + } + + //wait(1.0); + if(times >= 3.0 && times < 5.0){ + lamp = 0.0; + while(Enc_Arm.read_rotate() < pos) { + motor_dir1 = 0; + motor_dir2 = 1; + motor_pwm = 0.15; + } + } + motor_dir1=1; + motor_dir2=1; + motor_pwm=0; + //wait(2.0); +//----投てき------------------------- + if(times >= 5.0 && times < 6.0){ + motor_dir1 = 0; + motor_dir2 = 1; + motor_pwm = armpower; + } + //wait(1.0); + if(times >= 6.0 && times <7.0){ + motor_pwm = 0.0; + motor_dir1 = 0; + motor_dir2 = 0; + buzzer = 0; + lamp = 1.0; + set_flag = false; + timer.reset(); + timer.stop(); + } +} + void LineCheck(int dmode){ if(dmode == STRAIGHT){ if(linedata_1[1] == 1){