Dependencies: PID Serialservo TCS3200 XQ_servo encoder encoder2 mbed moter
Fork of nhk_2018_undercarry_test09 by
main.cpp
- Committer:
- kenken0721
- Date:
- 2018-04-01
- Revision:
- 6:ff7fd6556a81
- Parent:
- 5:c5a2a5cf600d
- Child:
- 7:f1a924244b76
File content as of revision 6:ff7fd6556a81:
#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 syokikanomai(void); void syokikanomai2(void); void touteki(float armpower, int pos, bool thro_mode); int crosscount = 0; bool mode = true;//trueでラジコン、falseでオート bool stopflag = true;//trueで機体停止 double power = POWER; double rpower = POWER; double lpower = 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_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 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) { 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); 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; } //--------------手動-------------------------------------- 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; } }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(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); 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_nage_degree); servo2.move(koshi_nage_degree); state = RED_RECEIVE; directmode = STOP; redcount = 0; crosscount = 0; }else if(distance == 0.0 && bluecount >= 100){ 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(0.8,190,0); if(set_flag == false){ directmode = RIGHT; } }else if(crosscount == 2){ directmode = STOP; syokikanomai2(); if(set_flag == true){ state = STAND_BY_1; crosscount = 0; } } break; //------------------待機位置1から投てき位置2---------------------------------------- case BLUE_RECEIVE_1: if(crosscount == 0){ directmode = RIGHT; }else if(crosscount == 1){ directmode = STRAIGHT; }else if(crosscount == 2){ directmode = LEFT; }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(blueIn == 1.0){ buzzer = 1.0; bluecount++; }else if(micro_switch == 1.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_nage_degree); servo2.move(koshi_nage_degree); state = BLUE_RECEIVE_2; directmode = STOP; bluecount = 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,190,1); if(set_flag == false){ directmode = RIGHT; } }else if(crosscount == 2){ directmode = STOP; syokikanomai2(); if(set_flag == true){ state = STAND_BY_2; crosscount = 0; } } break; //------------------投てき位置3--------------------------------------------------- case YELLOW_RECEIVE: if(crosscount != 4){ directmode = LEFT; }else{ directmode = STOP; } 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){ rpower = power; lpower = power + output; Straight(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2); }else { rpower = power + (-1 * output); lpower = power; Straight(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2); } break; case RIGHT: output = R_pid.compute((double)linedata_2[0]); if(output >= 0){ rpower = power; lpower = power + output; Right(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2); }else { rpower = power + (-1 * output); lpower = power; Right(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2); } break; case LEFT: output = L_pid.compute((double)linedata_3[0]); if(output >= 0){ rpower = power; lpower = power + output; Left(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2); }else { rpower = power + (-1 * output); lpower = power; Left(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2); } break; default: break; } } } //pc.printf("kbtread = %d \r" ,kbtread); //pc.printf("buff1[0] = %d \r" ,buff1[0]); //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("tar_xdist = %f \r",tar_x_dist); //pc.printf("ontheline = %d \r",ontheline); 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); //pc.printf("buff[1] = %d" ,buff1[1]); //pc.printf("line = %d" ,line); //pc.printf("rpower = %lf" ,rpower); //pc.printf("lpower = %lf" ,lpower); //pc.printf("dicmode = %d \r",directmode); pc.printf("output = %lf \n" ,output); } } //---------------初期化--------------------------- 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.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にする. } } } 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);//退避 servo2.move(koshi_uke_degree); servo1.move(hiji_nage_degree);//退避 servo2.move(koshi_uke_degree); 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); set_flag = true; //投てき後,falseにする. } } } } void touteki(float armpower ,int pos, bool throwing_mode){ if(set_flag == true){ timer.start(); } int times = timer.read(); if(times >= 1.0 && times < 2.0){ servo_hand.move(hand_uke_degree); } if(times >= 2.0 && times < 3.0){ servo1.move(hiji_nage_degree); } 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; //----投てき------------------------- if(throwing_mode == 0.0){ if(times >= 5.0 && times < 6.0){ motor_dir1 = 0; motor_dir2 = 1; motor_pwm = armpower; } }else if(throwing_mode == 1.0){ if(times >= 5.0 && times < 5.5){ throwing_relay = 1.0; }else{ throwing_relay = 0.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){ 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]; }