ケンタ ミヤザキ
Dependencies: PID Serialservo TCS3200 XQ_servo encoder encoder2 mbed moter
Fork of
nhk_2018_undercarry_test09
by 
ケンタ ミヤザキ
Diff: main.cpp
- Revision:
- 3:aaa2fde4fafd
- Parent:
- 2:c5996dd62e9c
- Child:
- 4:eabb7a738ff8
--- a/main.cpp Fri Mar 23 12:34:57 2018 +0000
+++ b/main.cpp Sat Mar 24 10:50:25 2018 +0000
@@ -13,6 +13,9 @@
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);
+DigitalOut buzzer(PA_10);
+DigitalOut lamp(PC_3);
+Timer timer;
void linecheck(char *buff ,int data[2]);
void LineCheck(int dmode);
@@ -60,25 +63,44 @@
float tar_y_dist = 0.0;
bool ontheline = false;
int directmode = STRAIGHT;
-
-int main() {
+int state = START;
+int times = 0;
+bool throwing = false;
+int main() {
+ timer.reset();
+ timer.stop();
+ //timer.start();
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);
+ pc.baud(115200);
kbtpc.baud(9600);
master.frequency(100000);
+ lamp = 1.0;
while (true) {
- if(directmode == STRAIGHT) master.read(addr1,buff1,2);
- if(directmode == RIGHT) master.read(addr2,buff2,2);
- if(directmode == LEFT) master.read(addr3,buff3,2);
-
+ //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);
+ //linecheck(buff1,linedata_1);
x_dist = Enc_Carry_X.read_rotate();
y_dist = Enc_Carry_Y.read_rotate();
- kbtpc.putc(1);
+ kbtpc.putc(state);
if(kbtpc.readable()){
kbtread = kbtpc.getc();
}
@@ -96,6 +118,10 @@
directmode = RIGHT;
}else if(kbtread == LEFT){
directmode = LEFT;
+ }else if(kbtread == MANUAL){
+ state = MANUAL;
+ }else if(kbtread == START){
+ state = START;
}
//--------------手動--------------------------------------
if(mode == true){
@@ -114,23 +140,155 @@
//--------------自動---------------------------------
}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;
+ Stop(pwm_pins, dir_pins_1, dir_pins_2);
+ crosscount = 0;
+ }
+ break;
+
+ //------------------------------------------
+ case START:
+ if(crosscount == 0){
+ directmode = STRAIGHT;
+ }else if(crosscount == 1 && directmode == STRAIGHT){
+ directmode = LEFT;
+ prelinedata = 0;
+ }else if(crosscount == 1 && directmode == LEFT){
+ directmode = LEFT;
+ }else if(crosscount == 2){
+ directmode = STOP;
+ state = STAND_BY_1;
+ crosscount = 0;
+ prelinedata = 0;
+ }
+ break;
+ //-----------------------------------------------
+ case STAND_BY_1:
+ crosscount = 0;
+ directmode = STOP;
+ //buzzer = 1.0;
+ if(kbtread == RED_RECEIVE){
+ state = RED_RECEIVE;
+ }else if(kbtread == BLUE_RECEIVE){
+ state = BLUE_RECEIVE_1;
+ }
+ break;
+ case RED_RECEIVE:
+ if(crosscount == 0){
+ directmode = LEFT;
+ }else if(crosscount == 1 && throwing == false){
+ 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;
+ directmode = RIGHT;
+ timer.reset();
+ timer.stop();
+ }
+ }else if(crosscount == 2){
+ directmode = STOP;
+ state = STAND_BY_1;
+ throwing = false;
+ crosscount = 0;
+ }
+ break;
+ 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 = STAND_BY_2;
+ crosscount = 0;
+ }
+ break;
+ case STAND_BY_2:
+ crosscount = 0;
+ if(kbtread == BLUE_RECEIVE){
+ state = BLUE_RECEIVE_2;
+ }else if(kbtread == YELLOW_RECEIVE){
+ state = YELLOW_RECEIVE;
+ }
+ break;
+ 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();
+ }
+ }
+ }else if(crosscount == 2){
+ directmode = STOP;
+ state = STAND_BY_2;
+ throwing = false;
+ crosscount = 0;
+ }
+ break;
+ case YELLOW_RECEIVE:
+ if(crosscount != 4){
+ directmode = LEFT;
+ }else{
+ directmode = STOP;
+ }
+ break;
+ default:
+ break;
+ }
+ /*
if(crosscount == 1){
directmode = STOP;
Stop(pwm_pins, dir_pins_1, dir_pins_2);
crosscount = 0;
- }else{
- switch(directmode){
+ }
+ */
+ 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);
+ //linecheck(buff1, linedata_1);
output = S_pid.compute((double)linedata_1[0]);
if(output >= 0){
rpower = power;
@@ -144,7 +302,7 @@
break;
case RIGHT:
//master.read(addr2,buff2,2);
- linecheck(buff2, linedata_2);
+ //linecheck(buff2, linedata_2);
output = R_pid.compute((double)linedata_2[0]);
if(output >= 0){
rpower = power;
@@ -158,12 +316,12 @@
break;
case LEFT:
//master.read(addr3,buff3,2);
- linecheck(buff3, linedata_3);
+ //linecheck(buff3, linedata_3);
output = L_pid.compute((double)linedata_3[0]);
if(output >= 0){
rpower = power;
lpower = power + output;
- Left(lpower, rpower, pwm_pins, dir_pins_1, dir_pins_2);
+ Left(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
}else {
rpower = power + (-1 * output);
lpower = power;
@@ -172,16 +330,18 @@
break;
default:
break;
- }
}
}
}
- //pc.printf("kbtread = %d \r" ,kbtread);
+ //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_1[1]);
+ //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);
@@ -189,8 +349,8 @@
//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("lpower = %lf" ,lpower);
+ pc.printf("dicmode = %d \r",directmode);
pc.printf("output = %lf \n" ,output);
}
}
@@ -202,25 +362,29 @@
tar_y_dist = y_dist;
}
if(ontheline == true){
- if((y_dist - tar_y_dist) >= STOP_DIST){
+ if((y_dist - tar_y_dist) >= Y_STOP_DIST){
ontheline = false;
crosscount++;
}
}
}else if(dmode == RIGHT){
- ontheline = true;
- tar_x_dist = x_dist;
+ if(linedata_2[1] == 1){
+ ontheline = true;
+ tar_x_dist = x_dist;
+ }
if(ontheline == true){
- if((x_dist - tar_x_dist) >= STOP_DIST){
+ if((x_dist - tar_x_dist) >= X_STOP_DIST){
ontheline = false;
crosscount++;
}
}
}else if(dmode == LEFT){
- ontheline = true;
- tar_x_dist = x_dist;
+ if(linedata_3[1] == 1){
+ ontheline = true;
+ tar_x_dist = x_dist;
+ }
if(ontheline == true){
- if((x_dist - tar_x_dist) <= STOP_DIST){
+ if((tar_x_dist - x_dist) >= X_STOP_DIST){
ontheline = false;
crosscount++;
}
@@ -285,5 +449,4 @@
data[1] = 0;
}
prelinedata = data[0];
-
-}
\ No newline at end of file
+}