ケンタ ミヤザキ
Dependencies: PID Serialservo TCS3200 XQ_servo encoder encoder2 mbed moter
Fork of
nhk_2018_undercarry_test08
by 
ケンタ ミヤザキ
Diff: main.cpp
- Revision:
- 2:c5996dd62e9c
- Parent:
- 1:f4dbd6b9cc27
- Child:
- 3:aaa2fde4fafd
diff -r f4dbd6b9cc27 -r c5996dd62e9c main.cpp
--- a/main.cpp Thu Mar 22 06:16:03 2018 +0000
+++ b/main.cpp Fri Mar 23 12:34:57 2018 +0000
@@ -9,9 +9,12 @@
I2C master(sda, scl);
DigitalOut led2(LED2);
PID S_pid(SP, SI, SD, 0.0);
-Encoder Enc_Carry_X(PA_15,PA_14,PC_1);
-Encoder Enc_Carry_Y(PF_1,PA_1,PA_0);
-int linecheck(char *buff);
+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);
+void linecheck(char *buff ,int data[2]);
+void LineCheck(int dmode);
PwmOut pwm_pins[] = {
PwmOut( PC_8 ),
@@ -23,26 +26,29 @@
DigitalOut dir_pins_1[] = {
DigitalOut( PC_5 ),
DigitalOut( PA_7 ),
- DigitalOut( PC_7 ),
- DigitalOut( PB_15)
+ DigitalOut( PB_6 ),
+ DigitalOut( PB_1)
};
DigitalOut dir_pins_2[] = {
DigitalOut( PB_12 ),
DigitalOut( PA_6 ),
- DigitalOut( PB_6 ),
- DigitalOut( PB_1)
+ DigitalOut( PC_7 ),
+ DigitalOut( PB_15)
};
+int crosscount = 0;
bool mode = true;//trueでラジコン、falseでオート
bool stopflag = true;//trueで機体停止
-int posit[2] = {0};//機体の現在位置
-int nextposit[2] = {0};//機体の位置指定
double power = POWER;
+double rpower = POWER;
+double lpower = POWER;
int kbtread = 0;
int count = 0;
int counter = 0;
-int linedata = 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];
@@ -50,23 +56,28 @@
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 main() {
-
+int main() {
S_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);
- kbtpc.baud(2400);
-
- char buff1[2];
-
+ kbtpc.baud(9600);
+ master.frequency(100000);
while (true) {
-
- master.read(addr1,buff1,2);
- int line = linecheck(buff1);
+ if(directmode == STRAIGHT) master.read(addr1,buff1,2);
+ if(directmode == RIGHT) master.read(addr2,buff2,2);
+ if(directmode == LEFT) master.read(addr3,buff3,2);
+
+ //master.read(addr1,buff1,2);
+ linecheck(buff1,linedata_1);
+ x_dist = Enc_Carry_X.read_rotate();
+ y_dist = Enc_Carry_Y.read_rotate();
kbtpc.putc(1);
if(kbtpc.readable()){
kbtread = kbtpc.getc();
@@ -79,17 +90,23 @@
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;
}
//--------------手動--------------------------------------
if(mode == true){
led2 = 1.0;
- if(kbtread == 1){
+ if(kbtread == STRAIGHT){
Straight(power, power, pwm_pins, dir_pins_1, dir_pins_2);
- }else if(kbtread == 2){
+ }else if(kbtread == BACK){
Back(power, power, pwm_pins, dir_pins_1, dir_pins_2);
- }else if(kbtread == 3){
+ }else if(kbtread == RIGHT){
Right(power, power, pwm_pins, dir_pins_1, dir_pins_2);
- }else if(kbtread == 4){
+ }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);
@@ -97,72 +114,176 @@
//--------------自動---------------------------------
}else if(mode == false){
led2 = 0.0;
+
if(stopflag == true){
Stop(pwm_pins, dir_pins_1, dir_pins_2);
}else{
- if(line == 255){
- stopflag = true;
+ LineCheck(directmode);
+ if(crosscount == 1){
+ directmode = STOP;
Stop(pwm_pins, dir_pins_1, dir_pins_2);
+ crosscount = 0;
}else{
- output = S_pid.compute((double)line);
+ 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);
+ 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:
+ //master.read(addr2,buff2,2);
+ linecheck(buff2, linedata_2);
+ output = R_pid.compute((double)linedata_2[0]);
if(output >= 0){
- Straight(power, power + output, pwm_pins, dir_pins_1, dir_pins_2);
+ rpower = power;
+ lpower = power + output;
+ Right(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
}else {
- Straight(power + (-1 * output), power, pwm_pins, dir_pins_1, dir_pins_2);
+ rpower = power + (-1 * output);
+ lpower = power;
+ Right(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
+ }
+ break;
+ case LEFT:
+ //master.read(addr3,buff3,2);
+ 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);
+ }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" ,kbtread);
- ///pc.printf("buff[0] = %d" ,buff1[0]);
+ //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("xdist = %f \r",x_dist);
+ //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 = %d" ,rpower);
- pc.printf("lpower = %d" ,lpower);
+ //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 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) >= STOP_DIST){
+ ontheline = false;
+ crosscount++;
+ }
+ }
+ }else if(dmode == RIGHT){
+ ontheline = true;
+ tar_x_dist = x_dist;
+ if(ontheline == true){
+ if((x_dist - tar_x_dist) >= STOP_DIST){
+ ontheline = false;
+ crosscount++;
+ }
+ }
+ }else if(dmode == LEFT){
+ ontheline = true;
+ tar_x_dist = x_dist;
+ if(ontheline == true){
+ if((x_dist - tar_x_dist) <= STOP_DIST){
+ ontheline = false;
+ crosscount++;
+ }
+ }
+ }
+}
-int linecheck(char *buff){
+void linecheck(char *buff, int data[2]){
if(buff[0] == 24){
- linedata = 0;
+ data[0] = 0;
+ data[1] = 0;
}else if(buff[0] == 56){
- linedata = 1;
+ data[0] = 1;
+ data[1] = 0;
}else if(buff[0] == 48){
- linedata = 2;
+ data[0] = 2;
+ data[1] = 0;
}else if(buff[0] == 112){
- linedata = 3;
+ data[0] = 3;
+ data[1] = 0;
}else if(buff[0] == 96){
- linedata = 4;
+ data[0] = 4;
+ data[1] = 0;
}else if(buff[0] == 224){
- linedata = 5;
+ data[0] = 5;
+ data[1] = 0;
}else if(buff[0] == 192){
- linedata = 6;
+ data[0] = 6;
+ data[1] = 0;
}else if(buff[0] == 128){
- linedata = 7;
+ data[0] = 7;
+ data[1] = 0;
}else if(buff[0] == 28){
- linedata = -1;
+ data[0] = -1;
+ data[1] = 0;
}else if(buff[0] == 12){
- linedata = -2;
+ data[0] = -2;
+ data[1] = 0;
}else if(buff[0] == 14){
- linedata = -3;
+ data[0] = -3;
+ data[1] = 0;
}else if(buff[0] == 6){
- linedata = -4;
+ data[0] = -4;
+ data[1] = 0;
}else if(buff[0] == 7){
- linedata = -5;
+ data[0] = -5;
+ data[1] = 0;
}else if(buff[0] == 3){
- linedata = -6;
+ data[0] = -6;
+ data[1] = 0;
}else if(buff[0] == 1){
- linedata = -7;
- }else if(buff[0] == 15 && buff[1] == 15){
- linedata = 255;
- }else if(buff[0] == 0 && buff[1] == 0){
- linedata = prelinedata;
+ 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{
- linedata = prelinedata;
+ data[0] = prelinedata;
+ data[1] = 0;
}
- prelinedata = linedata;
+ prelinedata = data[0];
- return linedata;
}
\ No newline at end of file