Dependencies: PID Serialservo TCS3200 XQ_servo encoder encoder2 mbed moter
Fork of nhk_2018_undercarry_test09 by
Diff: main.cpp
- Revision:
- 5:c5a2a5cf600d
- Parent:
- 4:eabb7a738ff8
- Child:
- 6:ff7fd6556a81
--- a/main.cpp Wed Mar 28 06:49:41 2018 +0000 +++ b/main.cpp Sun Apr 01 04:11:13 2018 +0000 @@ -2,7 +2,6 @@ #include "config.h" #include "moter.h" #include "PID.h" -#include "TCS3200.h" #include "encoder.h" #include "encoder2.h" #include "Serialservo.h" @@ -16,8 +15,6 @@ 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); @@ -28,7 +25,6 @@ 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); @@ -36,17 +32,10 @@ DigitalIn micro_switch(PC_0); DigitalOut air(PB_4); -DigitalOut motor_dir1(PA_8); //旧ピン番号 +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); @@ -114,8 +103,6 @@ timer.stop(); servo1.init(0);//アーム servo2.init(1);//回転軸 - //servo2.move(50); - //servo1.move(152); dpin=0; lamp=1; air=1.0; @@ -299,11 +286,9 @@ 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); @@ -317,13 +302,6 @@ 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: @@ -375,56 +353,39 @@ }else{ buzzer = 0.0; } - if(bluecount >= 50){ - pre_touteki(); + if(bluecount >= 100){ + servo_hand.move(hand_tsukami_degree); + servo1.move(hiji_tsukami_degree); }else if(yellowcount >= 500){ - air = 1.0; + air = 0.0; } - }else if(distance == 0.0 && bluecount >= 50){ + }else if(distance == 0.0 && bluecount >= 100){ + servo2.move(koshi_nage_degree); 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; }else if(crosscount == 1){ - if(throwing == false){ - directmode = STOP; - prelinedata = 0; - if(kbtread == THROWING){ - throwing = true; - buzzer = 1.0; - //timer.start(); - prelinedata = 0; - } - }else{ - /* - times = timer.read(); - if(times > 2){ - buzzer = 0.0; - directmode = RIGHT; - timer.reset(); - timer.stop(); - } - */ + directmode = STOP; + prelinedata = 0; + touteki(1.0,190,1); + if(set_flag == false){ + directmode = RIGHT; } }else if(crosscount == 2){ directmode = STOP; - state = STAND_BY_2; - throwing = false; - crosscount = 0; - } + syokikanomai2(); + if(set_flag == true){ + state = STAND_BY_2; + crosscount = 0; + } + } break; //------------------投てき位置3--------------------------------------------------- case YELLOW_RECEIVE: @@ -571,7 +532,7 @@ motor_dir2=1; servo1.move(hiji_uke_degree); servo1.move(hiji_uke_degree); - syokika_flag = false; + //syokika_flag = false; set_flag = true; //投てき後,falseにする. } } @@ -632,10 +593,18 @@ motor_pwm=0; //wait(2.0); //----投てき------------------------- - if(times >= 5.0 && times < 6.0){ - motor_dir1 = 0; - motor_dir2 = 1; - motor_pwm = armpower; + if(thro_mode == 0.0){ + if(times >= 5.0 && times < 6.0){ + motor_dir1 = 0; + motor_dir2 = 1; + motor_pwm = armpower; + } + }else if(thro_mode == 1.0){ + if(times >= 5.0 && times < 5.5){ + dpin = 1.0; + }else{ + dpin = 0.0; + } } //wait(1.0); if(times >= 6.0 && times <7.0){