Dependencies: PID Serialservo TCS3200 XQ_servo encoder encoder2 mbed moter
Fork of nhk_2018_undercarry_test09 by
main.cpp
- Committer:
- kenken0721
- Date:
- 2018-04-07
- Revision:
- 8:91a72648f312
- Parent:
- 7:f1a924244b76
- Child:
- 9:0c1fce6d8094
File content as of revision 8:91a72648f312:
#include "mbed.h" #include "config.h" #include "moter.h" #include "PID.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);//チェック用 PID S_pid(SP, SI, SD, 0.0); PID R_pid(RP, RI, RD, 0.0); PID L_pid(LP, LI, LD, 0.0); 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); DigitalOut throwing_relay(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 pwm_pins[] = { PwmOut( PC_8 ), PwmOut( PC_6 ), PwmOut( PB_14 ), PwmOut( PB_13 ) }; DigitalOut dir_pins_1[] = { DigitalOut( PC_5 ), DigitalOut( PA_7 ), DigitalOut( PB_6 ), DigitalOut( PB_1) }; DigitalOut dir_pins_2[] = { DigitalOut( PB_12 ), DigitalOut( PA_6 ), DigitalOut( PC_7 ), DigitalOut( PB_15) }; void linecheck(char *buff ,int data[2]); void LineCheck(int dmode); void Linecount(void); void syokikanomai(void); void syokikanomai2(void); void touteki(float armpower, int pos, bool thro_mode); void yellow_throwing(); bool set_pos_flag = false; bool redthrowing = false; bool yellow_throw_flag = false; bool taiki_flag = false; int crosscount = 0; bool mode = true;//trueでラジコン、falseでオート bool stopflag = true;//trueで機体停止 double power = POWER; int kbtread = 0; int count = 0; int counter = 0; int linedata_1[2] = {0}; int linedata_2[2] = {0}; int linedata_3[2] = {0}; int prelinedata = 0; double output = 0.0; char buff1[2]; char buff2[2]; char buff3[2]; float x_dist = 0.0; float y_dist = 0.0; float tar_x_dist = 0.0; float tar_x_dist_2 = 0.0; float tar_y_dist = 0.0; bool ontheline = false; int directmode = STRAIGHT; int state = START; int times = 0; int redcount = 0; int bluecount = 0; int yellowcount = 0; bool syokika_flag=false; bool set_flag = false; int encoder_count = 1000; int linecountcheck = false; int main() { timer.reset(); timer.stop(); throwing_relay=0; lamp=1; air=1.0; buzzer = 0.0; micro_switch.mode(PullUp); motor_pwm=0; motor_dir1=1; motor_dir2=1; servo1.init(0);//アーム servo2.init(1);//回転軸 S_pid.init(); R_pid.init(); L_pid.init(); pwm_pins[0].period(0.00005); 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; //-------初期化------------------------------------------------- syokikanomai(); buzzer = 0; //-----------メインループ---------------------------------------- while (true) { if(directmode == STRAIGHT){ master.read(addr1,buff1,2); linecheck(buff1,linedata_1); }else if(directmode == RIGHT){ master.read(addr2,buff2,2); linecheck(buff2,linedata_2); }else if(directmode == LEFT){ 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); if(kbtpc.readable()){ kbtread = kbtpc.getc(); } if(kbtread == 255){ mode = true; }else if(kbtread == 150){ mode = false; }else if(kbtread == 100){ stopflag = true; }else if(kbtread == 200){ stopflag = false; }else if(kbtread == STRAIGHT){ directmode = STRAIGHT; }else if(kbtread == RIGHT){ directmode = RIGHT; }else if(kbtread == LEFT){ directmode = LEFT; }else if(kbtread == START){ state = START; }else if(kbtread == STAND_BY_1){ state = STAND_BY_1; }else if(kbtread == YELLOW_RECEIVE){ state = YELLOW_RECEIVE; crosscount = 5; air = 0.0; } //--------------手動-------------------------------------- if(mode == true){ led2 = 1.0; if(kbtread == STRAIGHT){ Straight(power, power, pwm_pins, dir_pins_1, dir_pins_2); }else if(kbtread == BACK){ Back(power, power, pwm_pins, dir_pins_1, dir_pins_2); }else if(kbtread == RIGHT){ Right(power, power, pwm_pins, dir_pins_1, dir_pins_2); }else if(kbtread == LEFT){ Left(power, power, pwm_pins, dir_pins_1, dir_pins_2); }else{ Stop(pwm_pins, dir_pins_1, dir_pins_2); } //--------------自動--------------------------------- }else if(mode == false){ led2 = 0.0; if(stopflag == true){ Stop(pwm_pins, dir_pins_1, dir_pins_2); }else{ LineCheck(directmode); switch(state){ //------------------初期位置から待機位置1------------------------------------------ case START: if(crosscount == 0){ directmode = STRAIGHT; }else if(crosscount == 1){ if(directmode == STRAIGHT){ directmode = LEFT; prelinedata = 0; }else if(directmode == LEFT){ directmode = LEFT; Linecount(); } }else if(crosscount == 2){ directmode = STOP; state = STAND_BY_1; crosscount = 0; prelinedata = 0; } break; //------------------待機位置1---------------------------------------------------- case STAND_BY_1: crosscount = 0; directmode = STOP; if(distance == 1.0){ if(redthrowing == false){ if(redIn == 1.0){ buzzer = 1.0; redcount++; }else { buzzer = 0.0; } }else if(redthrowing == true){ if(redIn == 1.0){ buzzer = 1.0; bluecount++; }else { buzzer = 0.0; } } if(redcount >= 100 ){ buzzer = 0; servo_hand.move(hand_tsukami_degree); servo1.move(hiji_tsukami_degree); servo1.move(hiji_tsukami_degree); }else if(bluecount >= 100){ servo_hand.move(hand_tsukami_degree); servo1.move(hiji_tsukami_degree); servo1.move(hiji_tsukami_degree); } }else if(distance == 0.0 && redcount >= 100){ servo2.move(koshi_red_nage_degree); servo2.move(koshi_red_nage_degree); state = RED_RECEIVE; directmode = STOP; redcount = 0; crosscount = 0; }else if(distance == 0.0 && bluecount >= 100){ servo2.move(koshi_blue_nage_degree); servo2.move(koshi_blue_nage_degree); state = BLUE_RECEIVE_1; directmode = STOP; bluecount = 0; crosscount = 0; } break; //------------------待機位置1から投てき位置1---------------------------------------- case RED_RECEIVE: if(crosscount == 0){ directmode = LEFT; }else if(crosscount == 1){ directmode = STOP; prelinedata = 0; touteki(1.0,195,0); //syokikanomai2(); if(set_flag == false){ directmode = RIGHT; Linecount(); } }else if(crosscount == 2){ directmode = STOP; syokikanomai2(); if(set_flag == true){ state = STAND_BY_1; //taiki_flag = false; crosscount = 0; redthrowing = true; } } break; //------------------待機位置1から投てき位置2---------------------------------------- case BLUE_RECEIVE_1: if(crosscount == 0){ directmode = RIGHT; }else if(crosscount == 1){ directmode = STRAIGHT; }else if(crosscount == 2){ directmode = LEFT; Linecount(); }else if(crosscount == 3){ directmode = STOP; prelinedata = 0; state = BLUE_RECEIVE_2; crosscount = 0; } break; //------------------待機位置2---------------------------------------------------- case STAND_BY_2: crosscount = 0; directmode = STOP; if(distance == 1.0){ if(redIn == 1.0){ buzzer = 1.0; bluecount++; }else if(micro_switch == 0.0){ buzzer = 1.0; yellowcount++; }else{ buzzer = 0.0; } if(bluecount >= 100){ servo_hand.move(hand_tsukami_degree); servo1.move(hiji_tsukami_degree); servo1.move(hiji_tsukami_degree); }else if(yellowcount >= 500){ air = 0.0; } }else if(distance == 0.0 && bluecount >= 100){ servo2.move(koshi_blue_nage_degree); servo2.move(koshi_blue_nage_degree); state = BLUE_RECEIVE_2; directmode = STOP; bluecount = 0; crosscount = 0; }else if(distance == 0.0 && yellowcount >= 500){ state = YELLOW_RECEIVE; buzzer = 0.0; directmode = STOP; yellowcount = 0; crosscount = 0; } break; //------------------投てき位置2--------------------------------------------------- case BLUE_RECEIVE_2: if(crosscount == 0){ directmode = LEFT; }else if(crosscount == 1){ directmode = STOP; prelinedata = 0; touteki(1.0,200,1); //syokikanomai2(); if(set_flag == false){ directmode = RIGHT; Linecount(); } }else if(crosscount == 2){ directmode = STOP; syokikanomai2(); if(set_flag == true){ state = STAND_BY_2; //taiki_flag = false; crosscount = 0; } } break; //------------------投てき位置3--------------------------------------------------- case YELLOW_RECEIVE: if(crosscount != 5){ directmode = LEFT; }else{ directmode = STOP; if(yellow_throw_flag == false){ yellow_throwing(); }else{ touteki(1.0,190,1.0); syokikanomai2(); } } break; default: break; } //--------------進行方向--------------------------------------------------------- switch(directmode){ case STOP: Stop(pwm_pins, dir_pins_1, dir_pins_2); break; case STRAIGHT: output = S_pid.compute((double)linedata_1[0]); if(output >= 0){ Straight(power, power + output, pwm_pins, dir_pins_1, dir_pins_2); }else { Straight(power + (-1 * output), power, pwm_pins, dir_pins_1, dir_pins_2); } break; case RIGHT: output = R_pid.compute((double)linedata_2[0]); if(output >= 0){ Right(power, power + output, pwm_pins, dir_pins_1, dir_pins_2); }else { Right(power + (-1 * output), power, pwm_pins, dir_pins_1, dir_pins_2); } break; case LEFT: output = L_pid.compute((double)linedata_3[0]); if(output >= 0){ Left(power, power + output, pwm_pins, dir_pins_1, dir_pins_2); }else { Left(power + (-1 * output), power, pwm_pins, dir_pins_1, dir_pins_2); } break; default: break; } } } pc.printf("xdist = %f \r",x_dist); pc.printf("ydist = %f \r",y_dist); pc.printf("output = %lf \n" ,output); } } //---------------初期化---------------------------------------------------------- void yellow_throwing(){ if(yellow_throw_flag == false){ timer.start(); } int times = timer.read(); if(times >= 2.0 && times < 3.0){ servo_hand.move(hand_uke_degree); }else if(times >= 3.0 && times < 4.0){ servo1.move(hiji_uke_degree); }else if(times >= 4.0 && times < 5.0){ servo_hand.move(hand_tsukami_degree); }else if(times >=5.0 && times <5.5){ servo2.move(koshi_nage_degree); }else if(times >= 5.5){ yellow_throw_flag = true; timer.stop(); timer.reset(); } } void syokikanomai(void){ 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); servo1.move(hiji_nage_degree);//退避 servo2.move(koshi_uke_degree); motor_dir1=0; motor_dir2=1; motor_pwm=0.12; 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にする. } } } servo1.move(hiji_uke_degree); servo1.move(hiji_uke_degree); } void syokikanomai2(void){ if(set_flag==false) { encoder_count= Enc_Arm.read_rotate(); servo_hand.move(hand_uke_degree); servo1.move(hiji_nage_degree);//退避 servo1.move(hiji_nage_degree);//退避 if(yellow_throw_flag == false){ servo2.move(koshi_uke_degree); servo2.move(koshi_uke_degree); } motor_dir1=0; motor_dir2=1; motor_pwm=0.12; if(Enc_Arm.read_z() == 1) { syokika_flag = true; buzzer = 1.0; } if (syokika_flag==true) { int set_pos; if(yellow_throw_flag == true) set_pos = 164; else set_pos = 170; if(encoder_count == set_pos){ motor_pwm=0; motor_dir1=1; motor_dir2=1; if(yellow_throw_flag == false){ servo_hand.move(hand_uke_degree); servo1.move(hiji_uke_degree); servo1.move(hiji_uke_degree); }else{ servo_hand.move(hand_tsukami_degree); servo2.move(koshi_uke_degree); servo2.move(koshi_uke_degree); } set_flag = true; //投てき後,falseにする. taiki_flag = true; yellow_throw_flag = false; } } } } void touteki(float armpower ,int pos, bool throwing_mode){ led2 = 1.0; if(set_flag == true){ timer.start(); } int times = timer.read(); if(times >= 2.0 && times < 3.0){ servo_hand.move(hand_uke_degree); led2 = 0.0; } if(times >= 3.0 && times < 4.0){ servo1.move(hiji_nage_degree); servo1.move(hiji_nage_degree); } if(times >= 4.0 && times < 6.0){ lamp = 0.0; if(set_pos_flag == false){ if(Enc_Arm.read_rotate() < pos) { motor_dir1 = 0; motor_dir2 = 1; motor_pwm = 0.15; }else{ motor_dir1=1; motor_dir2=1; motor_pwm=0; set_pos_flag = true; } } } //----投てき------------------------- if(throwing_mode == 0.0){ if(times >= 6.0 && times < 7.0){ motor_dir1 = 0; motor_dir2 = 1; motor_pwm = armpower; } }else if(throwing_mode == 1.0){ if(times >= 6.0 && times < 6.1){ throwing_relay = 1.0; }else{ throwing_relay = 0.0; } } if(times >= 7.0 && times <8.0){ motor_pwm = 0.0; motor_dir1 = 1; motor_dir2 = 1; buzzer = 0; lamp = 1.0; }else if(times >= 8.0 && yellow_throwing == false){ set_flag = false; set_pos_flag = false; timer.reset(); timer.stop(); }else if(times >= 10.0 && yellow_throw_flag == true){ set_flag = false; set_pos_flag = false; timer.reset(); timer.stop(); } } void Linecount(void){ if(directmode == RIGHT){ if(linecountcheck == false){ tar_x_dist_2 = x_dist; linecountcheck = true; }else{ if((x_dist - tar_x_dist_2) > X_STOP_DIST_3){ crosscount++; linecountcheck = false; } } }else if(directmode == LEFT){ if(linecountcheck == false){ tar_x_dist_2 = x_dist; linecountcheck = true; }else{ if((tar_x_dist_2 - x_dist) >= X_STOP_DIST_2){ crosscount++; linecountcheck = false; } } } } void LineCheck(int dmode){ if(dmode == STRAIGHT){ if(linedata_1[1] == 1){ ontheline = true; tar_y_dist = y_dist; } if(ontheline == true){ if((y_dist - tar_y_dist) >= Y_STOP_DIST){ ontheline = false; crosscount++; } } }else if(dmode == RIGHT){ if(linedata_2[1] == 1){ ontheline = true; tar_x_dist = x_dist; } if(ontheline == true){ if((x_dist - tar_x_dist) >= X_STOP_DIST){ ontheline = false; crosscount++; } } }else if(dmode == LEFT){ if(linedata_3[1] == 1){ ontheline = true; tar_x_dist = x_dist; } if(ontheline == true){ if((tar_x_dist - x_dist) >= X_STOP_DIST){ ontheline = false; crosscount++; } } } } void linecheck(char *buff, int data[2]){ if(buff[0] == 24){ data[0] = 0; data[1] = 0; }else if(buff[0] == 56){ data[0] = 1; data[1] = 0; }else if(buff[0] == 48){ data[0] = 2; data[1] = 0; }else if(buff[0] == 112){ data[0] = 3; data[1] = 0; }else if(buff[0] == 96){ data[0] = 4; data[1] = 0; }else if(buff[0] == 224){ data[0] = 5; data[1] = 0; }else if(buff[0] == 192){ data[0] = 6; data[1] = 0; }else if(buff[0] == 128){ data[0] = 7; data[1] = 0; }else if(buff[0] == 28){ data[0] = -1; data[1] = 0; }else if(buff[0] == 12){ data[0] = -2; data[1] = 0; }else if(buff[0] == 14){ data[0] = -3; data[1] = 0; }else if(buff[0] == 6){ data[0] = -4; data[1] = 0; }else if(buff[0] == 7){ data[0] = -5; data[1] = 0; }else if(buff[0] == 3){ data[0] = -6; data[1] = 0; }else if(buff[0] == 1){ data[0] = -7; data[1] = 0; }else if(buff[0] == 255){ data[0] = prelinedata; data[1] = 1; }else if(buff[0] == 0){ data[0] = prelinedata; data[1] = 0; }else{ data[0] = prelinedata; data[1] = 0; } prelinedata = data[0]; }