ケンタ ミヤザキ
Dependencies: PID Serialservo TCS3200 XQ_servo encoder encoder2 mbed moter
Fork of
nhk_2018_undercarry_test09
by 
ケンタ ミヤザキ
Diff: main.cpp
- Revision:
- 7:f1a924244b76
- Parent:
- 6:ff7fd6556a81
- Child:
- 8:91a72648f312
--- a/main.cpp Sun Apr 01 11:58:23 2018 +0000
+++ b/main.cpp Fri Apr 06 08:22:03 2018 +0000
@@ -32,7 +32,7 @@
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);
@@ -60,16 +60,18 @@
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 yellow_throw_flag = false;
+bool taiki_flag = false;
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;
@@ -84,6 +86,7 @@
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;
@@ -95,6 +98,7 @@
bool syokika_flag=false;
bool set_flag = false;
int encoder_count = 1000;
+int linecountcheck = false;
int main() {
timer.reset();
@@ -127,12 +131,17 @@
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);
+ 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);
@@ -188,6 +197,7 @@
prelinedata = 0;
}else if(directmode == LEFT){
directmode = LEFT;
+ Linecount();
}
}else if(crosscount == 2){
directmode = STOP;
@@ -227,6 +237,8 @@
redcount = 0;
crosscount = 0;
}else if(distance == 0.0 && bluecount >= 100){
+ servo2.move(koshi_nage_degree);
+ servo2.move(koshi_nage_degree);
state = BLUE_RECEIVE_1;
directmode = STOP;
bluecount = 0;
@@ -241,14 +253,17 @@
directmode = STOP;
prelinedata = 0;
touteki(0.8,190,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;
}
}
@@ -261,6 +276,7 @@
directmode = STRAIGHT;
}else if(crosscount == 2){
directmode = LEFT;
+ Linecount();
}else if(crosscount == 3){
directmode = STOP;
prelinedata = 0;
@@ -276,7 +292,7 @@
if(blueIn == 1.0){
buzzer = 1.0;
bluecount++;
- }else if(micro_switch == 1.0){
+ }else if(micro_switch == 0.0){
buzzer = 1.0;
yellowcount++;
}else{
@@ -296,6 +312,11 @@
directmode = STOP;
bluecount = 0;
crosscount = 0;
+ }else if(distance == 0.0 && yellowcount >= 500){
+ state = YELLOW_RECEIVE;
+ directmode = STOP;
+ yellowcount = 0;
+ crosscount = 0;
}
break;
//------------------投てき位置2---------------------------------------------------
@@ -306,24 +327,33 @@
directmode = STOP;
prelinedata = 0;
touteki(1.0,190,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 != 4){
+ 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:
@@ -337,37 +367,25 @@
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);
+ Straight(power, power + output, 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);
+ 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){
- rpower = power;
- lpower = power + output;
- Right(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
+ Right(power, power + output, 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);
+ 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){
- rpower = power;
- lpower = power + output;
- Left(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
+ Left(power, power + output, 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);
+ Left(power + (-1 * output), power, pwm_pins, dir_pins_1, dir_pins_2);
}
break;
default:
@@ -375,29 +393,30 @@
}
}
}
- //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 yellow_throwing(){
+ if(yellow_throw_flag == false){
+ timer.start();
+ }
+ int times = timer.read();
+ if(times >= 0.5 && times < 1.5){
+ servo_hand.move(hand_tsukami_degree);
+ }else if(times >=1.5 && times <2.5){
+ servo2.move(koshi_nage_degree);
+ }else if(times >= 2.5){
+ yellow_throw_flag = true;
+ timer.stop();
+ timer.reset();
+ }
+}
+
void syokikanomai(void){
syokika_flag = false;
while (set_flag==false) {
@@ -410,13 +429,13 @@
motor_dir1=0;
motor_dir2=1;
motor_pwm=0.15;
- if(Enc_Arm.read_z() == 1) {
+ if(Enc_Arm.read_z() == 1){
syokika_flag=true;
buzzer = 1.0;
}
if (syokika_flag==true) {
- if(encoder_count == 170) {
+ if(encoder_count == 170){
motor_pwm=0;
motor_dir1=1;
motor_dir2=1;
@@ -430,8 +449,12 @@
void syokikanomai2(void){
if(set_flag==false) {
- encoder_count= Enc_Arm.read_rotate();
- servo_hand.move(hand_uke_degree);
+ encoder_count= Enc_Arm.read_rotate();
+ if(yellow_throw_flag == true){
+ servo_hand.move(hand_tsukami_degree);
+ }else{
+ servo_hand.move(hand_uke_degree);
+ }
servo1.move(hiji_nage_degree);//退避
servo2.move(koshi_uke_degree);
servo1.move(hiji_nage_degree);//退避
@@ -444,30 +467,36 @@
buzzer = 1.0;
}
if (syokika_flag==true) {
- if(encoder_count == 170) {
+ if(encoder_count == 170){
motor_pwm=0;
motor_dir1=1;
motor_dir2=1;
+ servo_hand.move(hand_uke_degree);
servo1.move(hiji_uke_degree);
servo1.move(hiji_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 >= 1.0 && times < 2.0){
+ if(times >= 2.0 && times < 3.0){
servo_hand.move(hand_uke_degree);
+ led2 = 0.0;
}
- if(times >= 2.0 && times < 3.0){
+ if(times >= 3.0 && times < 4.0){
+ servo1.move(hiji_nage_degree);
servo1.move(hiji_nage_degree);
}
- if(times >= 3.0 && times < 5.0){
+ if(times >= 4.0 && times < 6.0){
lamp = 0.0;
while(Enc_Arm.read_rotate() < pos) {
motor_dir1 = 0;
@@ -478,21 +507,22 @@
motor_dir1=1;
motor_dir2=1;
motor_pwm=0;
+
//----投てき-------------------------
if(throwing_mode == 0.0){
- if(times >= 5.0 && times < 6.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 >= 5.0 && times < 5.5){
+ if(times >= 6.0 && times < 6.5){
throwing_relay = 1.0;
}else{
throwing_relay = 0.0;
}
}
- if(times >= 6.0 && times <7.0){
+ if(times >= 7.0 && times <8.0){
motor_pwm = 0.0;
motor_dir1 = 0;
motor_dir2 = 0;
@@ -504,6 +534,30 @@
}
}
+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){