ケンタ ミヤザキ
/
nhk_2018_undercarry_test04
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: PID mbed moter encoder
Revision 3:aaa2fde4fafd, committed 2018-03-24
- Comitter:
- kenken0721
- Date:
- Sat Mar 24 10:50:25 2018 +0000
- Parent:
- 2:c5996dd62e9c
- Commit message:
Changed in this revision
--- a/PID.lib Fri Mar 23 12:34:57 2018 +0000 +++ b/PID.lib Sat Mar 24 10:50:25 2018 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/kenken0721/code/PID/#09a00a9407f8 +https://os.mbed.com/users/kenken0721/code/PID/#3bc8034f569e
--- a/config.h Fri Mar 23 12:34:57 2018 +0000 +++ b/config.h Sat Mar 24 10:50:25 2018 +0000 @@ -3,17 +3,18 @@ #define scl PB_8 #define SP 0.02 #define SI 0.00 -#define SD 0.00 -#define RP 0.03 +#define SD 0.00005 +#define RP 0.02 #define RI 0.0 -#define RD 0.0 -#define LP 0.03 +#define RD 0.00 +#define LP 0.024 #define LI 0.0 -#define LD 0.0 -#define STOP_DIST 1.2 +#define LD 0.00000 +#define X_STOP_DIST 1.2 +#define Y_STOP_DIST 1.2 #define addr1 0x24 #define addr2 0x10 -#define addr3 0x01 +#define addr3 0x64 #define STRAIGHT 1 @@ -30,6 +31,4 @@ #define STAND_BY_1 12 #define STAND_BY_2 13 #define THROWING 14 - - - +#define MANUAL 15 \ No newline at end of file
--- 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
+}