Hajime Muraki
main
Dependencies: TextLCD mbed PID
Diff: main.cpp
- Revision:
- 4:536cd493a337
- Parent:
- 3:440e774cc24b
- Child:
- 5:6604ec9044a0
--- a/main.cpp Wed Mar 12 04:51:30 2014 +0000
+++ b/main.cpp Mon Mar 17 05:41:07 2014 +0000
@@ -5,12 +5,14 @@
#include <math.h>
#include <sstream>
-#define MOTOR_P 26
-#define HOME_P 25
+#define MOTOR_P 35
+#define HOME_P 30
+#define TURN_P 20
+#define BALL_P 25
#define NO_BALL 45
-#define LINE_LP 30
-#define LINE_FP 35
-#define LINE_ON 0xFFF0
+#define LINE_P 30
+#define LINE_PLR 17
+#define LINE_ON 0x2710 //10000
#define LINE_TIME 0.6
#define LINE_PING 40
#define R 1.0
@@ -19,34 +21,39 @@
#define S2 10
#define S3 5
#define RATE 0.05
-#define P_GAIN 1.2
+#define P_GAIN 1.7
#define I_GAIN 0.0
-#define D_GAIN 0.015
+#define D_GAIN 0.013
#define HOME_BACK 30
#define HOME 1
+#define KICK 25
#define GOAL 30.0
#define PING_LR 100
-//誤差errの範囲でvalue1がvalue2と等しければ0以外を、等しくなれば0を返す
-#define ERR_EQ(value1,value2,err) ( ((value1) <= ((value2)+(err)))&&((value1) >= ((value2)-(err))) )
+#define BALL_FAR 43
+#define BALL_NEAR 40
-DigitalIn sw(p5);
+DigitalIn sw_wh(p5);
DigitalIn start(p7);
DigitalOut myled[4] = {LED1, LED2, LED3, LED4};
Serial motor(p9,p10);
Serial sensor(p13,p14);
+Serial xbee(p28,p27);
Serial pc(USBTX, USBRX);
TextLCD lcd(p26, p25, p24, p23, p22, p21);
AnalogIn adcline[4] = {p16, p17, p19, p20};
PID pid(P_GAIN,I_GAIN,D_GAIN, RATE);
-Timeout liner0;
-Timeout liner1;
-Timeout liner2;
-Timeout liner3;
+Timeout liner_F;
+Timeout liner_L;
+Timeout liner_B;
+Timeout liner_R;
Ticker pidupdata;
+Ticker xbeetx;
//HMC6352 dcompass(p9,p10);
extern string StringFIN;
extern void array(int,int,int,int);
+extern void xbee_tx(void);
+extern void xbee_rx(void);
extern void micon_rx(void);
//uint16_t analogHex[4] = {0};
@@ -79,7 +86,7 @@
};
double ball_way[12][2] = {
{ 0.0 , 1.0 }, //FF
- {-0.985,-0.173}, //FL
+ {-0.866,-0.500}, //FL
{-0.342,-0.940}, //LL
{ 0.342,-0.940}, //BL
{ 0.966, 0.259}, //BB
@@ -92,19 +99,6 @@
{ 0.173, 0.985} //FFFR
};
-double all_way[10][2] = {
- { 0 , 1 }, //FF
- {-0.707, 0.707}, //FL
- {-0.939,-0.342}, //LL {-1 , 0 },
- {-0.707,-0.707}, //BL
- { 0 ,-1 }, //BB
- { 0.707,-0.707}, //BR
- { 0.939,-0.342}, //RR { 1 , 0 },
- { 0.707, 0.707}, //FR
- {-0.500, 0.866}, //FFL
- { 0.500, 0.866} //FFR
-};
-
void move(int vx, int vy, int vs){
double pwm[4] = {0};
@@ -132,81 +126,36 @@
//ライン判断
void line_state(){
if(line[LEFT]){
- x = LINE_LP;
- } else if(line_ping[LEFT]){
- if(ping[LEFT] < LINE_PING){
- if(x < 10){
- x = 10;
- }
- } else {
- line_ping[LEFT] = 0;
- myled[1] = 0;
- }
+ x = LINE_P;
+ } else if(line[RIGHT]){
+ x = -LINE_P;
}
-
- if(line[BACK]){
- y = LINE_FP;
- } else if(line_ping[BACK]){
- if(ping[BACK] < LINE_PING){
- if(y < 10){
- y = 10;
- }
+
+ if(line[FRONT]){
+ if(line[LEFT] | line[RIGHT]){
+ y = -LINE_PLR;
} else {
- line_ping[BACK] = 0;
- myled[2] = 0;
+ y = -LINE_P;
}
- }
-
- if(line[RIGHT]){
- x = -LINE_LP;
- } else if(line_ping[RIGHT]){
- if(ping[RIGHT] < LINE_PING){
- if(x > 10){
- x = -10;
- }
+ } else if(line[BACK]){
+ if(line[LEFT] | line[RIGHT]){
+ y = LINE_PLR;
} else {
- line_ping[RIGHT] = 0;
- myled[3] = 0;
+ y = LINE_P;
}
}
-
- if(line[FRONT]){
- y = -LINE_FP;
- } else if(line_ping[FRONT]){
- if(ping[FRONT] < LINE_PING){
- if(y > 10){
- y = -10;
- }
- } else {
- line_ping[FRONT] = 0;
- myled[0] = 0;
- }
- }
-
}
-void lcd_ping(){
- lcd.cls();
- lcd.locate(0,0);
- lcd.printf("%03d %03d\n%03d %03d", ping[FRONT], ping[LEFT], ping[BACK], ping[RIGHT]);
-}
-
-void lcd_line(){
- lcd.cls();
- lcd.locate(0,0);
- lcd.printf("%03d %03d\n%03d %03d", line[FRONT], line[LEFT], line[BACK], line[RIGHT]);
-}
-
-void line_stop0(){
+void line_stop_F(){
line_stop[FRONT] = 0;
}
-void line_stop1(){
+void line_stop_L(){
line_stop[LEFT] = 0;
}
-void line_stop2(){
+void line_stop_B(){
line_stop[BACK] = 0;
}
-void line_stop3(){
+void line_stop_R(){
line_stop[RIGHT] = 0;
}
@@ -216,24 +165,28 @@
if(adcline[FRONT].read_u16() > LINE_ON){
line[FRONT] = 1;
line_stop[FRONT] = 1;
- line_ping[FRONT] = 1;
+ line_stop[BACK] = 1;
+ //line_ping[FRONT] = 1;
myled[0] = 1;
- liner0.attach(&line_stop0,1);
+ liner_F.attach(&line_stop_F,LINE_TIME);
+ liner_B.attach(&line_stop_B,LINE_TIME);
} else {
line[FRONT] = 0;
- ////myled[0] = 0;
+ myled[0] = 0;
}
}
if(!line_stop[LEFT]){
if(adcline[LEFT].read_u16() > LINE_ON){
line[LEFT] = 1;
line_stop[LEFT] = 1;
- line_ping[LEFT] = 1;
+ line_stop[RIGHT] = 1;
+ //line_ping[LEFT] = 1;
myled[1] = 1;
- liner1.attach(&line_stop1,LINE_TIME);
+ liner_L.attach(&line_stop_L,LINE_TIME);
+ liner_R.attach(&line_stop_R,LINE_TIME);
} else {
line[LEFT] = 0;
- ////myled[1] = 0;
+ myled[1] = 0;
}
}
if(!line_stop[BACK]){
@@ -241,25 +194,27 @@
line[BACK] = 1;
line_stop[BACK] = 1;
line_stop[FRONT] = 1;
- line_ping[BACK] = 1;
+ //line_ping[BACK] = 1;
myled[2] = 1;
- liner0.attach(&line_stop0,LINE_TIME);
- liner2.attach(&line_stop2,LINE_TIME);
+ liner_B.attach(&line_stop_B,LINE_TIME);
+ liner_F.attach(&line_stop_F,LINE_TIME);
} else {
line[BACK] = 0;
- ////myled[2] = 0;
+ myled[2] = 0;
}
}
if(!line_stop[RIGHT]){
if(adcline[RIGHT].read_u16() > LINE_ON){
line[RIGHT] = 1;
line_stop[RIGHT] = 1;
- line_ping[RIGHT] = 1;
+ line_stop[LEFT] = 1;
+ //line_ping[RIGHT] = 1;
myled[3] = 1;
- liner3.attach(&line_stop3,LINE_TIME);
+ liner_R.attach(&line_stop_R,LINE_TIME);
+ liner_L.attach(&line_stop_L,LINE_TIME);
} else {
line[RIGHT] = 0;
- ////myled[3] = 0;
+ myled[3] = 0;
}
}
}
@@ -267,7 +222,7 @@
void pidupdate()
{
- pid.setSetPoint(180.0 + goal);
+ //pid.setSetPoint(180.0 + goal);
pidinput = compass;
pid.setProcessValue(pidinput);
@@ -302,7 +257,8 @@
//dcompass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
//uint8_t num = 0;
- uint8_t dir = 0, power = 0;
+ uint8_t dir = 0, power = 0, lcd_count = 0;
+ int x_ball = 0, x_turn = 0, y_ball = 0, y_turn = 0;
wait(1);
@@ -310,6 +266,8 @@
motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
sensor.attach(&micon_rx,Serial::RxIrq); //送信空き割り込み（センサ用）
+ xbee.attach(&xbee_rx,Serial::RxIrq);
+ xbeetx.attach(&xbee_tx,1);
//compassdef = (compass / 10); //コンパス初期値保存
//compassdef = (dcompass.sample() / 10);
pid.setInputLimits(0.0,360.0);
@@ -319,20 +277,52 @@
pid.setSetPoint(180.0);
pidupdata.attach(&pidupdate, 0.05);
- sw.mode(PullUp);
- start.mode(PullUp);
+ sw_wh.mode(PullUp);
+ start.mode(PullDown);
lcd.cls();
lcd.locate(0,0);
lcd.printf("Chronograph");
-
+
+ //move(x,y,s);
+
myled[0] = 1;
+
+ //スタート前チェック
while(!start){
- lcd.locate(6,1);
- lcd.printf("%d", compass);
+ if(lcd_count == 0){
+ lcd.locate(6,1);
+ lcd.printf("%3d", compass);
+ } else if(lcd_count == 1){
+ lcd.locate(0,1);
+ lcd.printf("ir_dis = %3d", ir_dis);
+ } else if(lcd_count == 2){
+ lcd.locate(0,0);
+ lcd.printf("ir_num = %3d\nir_val = %3d", ir_num, value_ir);
+ } else if(lcd_count == 3){
+ lcd.locate(0,0);
+ lcd.printf("F:%3d B:%3d\nL:%3d R:%3d", ping[FRONT], ping[BACK], ping[LEFT], ping[RIGHT]);
+ } else if(lcd_count == 4){
+ lcd.locate(0,0);
+ lcd.printf("F:%5d B:%5d\nL:%5d R:%5d", adcline[FRONT].read_u16(), adcline[BACK].read_u16(), adcline[LEFT].read_u16(), adcline[RIGHT].read_u16());
+ }
+ if(!sw_wh){
+ lcd_count++;
+ if(lcd_count == 5){
+ lcd_count = 0;
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("Chronograph");
+ }
+ wait(0.5);
+ }
wait(0.1);
}
+
+
+
+
myled[0] = 0;
lcd.cls();
lcd.locate(0,0);
@@ -341,6 +331,113 @@
lcd.printf("ABURASOBA");
+
+ while(1){
+ x = 0;
+ y = 0;
+ s = compasspid;
+ power = MOTOR_P;
+
+ if((ir_dis < 35)/* && ( (ir_num == FF) || (ir_num == FFL) || (ir_num == FFR) )*/){
+ kick = KICK;
+ } else {
+ kick = 0;
+ //goal = 0.0;
+ }
+
+ if((ir_num != FF) && (ir_num != FFL) && (ir_num != FFR)){
+ if(ir_dis > BALL_FAR){
+ x_turn = TURN_P * way[ir_num][0];
+ y_turn = TURN_P * way[ir_num][1];
+
+ x_ball = BALL_P * ball_way[ir_num][0];
+ y_ball = BALL_P * ball_way[ir_num][1];
+
+ if(ir_num == BB){
+ if(ping[LEFT] > ping[RIGHT]){
+ x_ball = -x_ball;
+ }
+ }
+
+ x = x_turn + x_ball;
+ y = y_turn + y_ball;
+
+ } else if(ir_dis < BALL_NEAR){
+ x_turn = -(TURN_P - 5) * way[ir_num][0];
+ y_turn = -(TURN_P - 5) * way[ir_num][1];
+
+ x_ball = BALL_P * ball_way[ir_num][0];
+ y_ball = BALL_P * ball_way[ir_num][1];
+
+ if(ir_num == BB){
+ if(ping[LEFT] > ping[RIGHT]){
+ x_ball = -x_ball;
+ }
+ }
+
+ x = x_turn + x_ball;
+ y = y_turn + y_ball;
+
+ } else {
+ if((ir_num == FL) || (ir_num == FR)){
+ power = 20;
+ }
+ x = power * ball_way[ir_num][0];
+ y = power * ball_way[ir_num][1];
+ if(ir_num == BB){
+ if(ping[LEFT] > ping[RIGHT]){
+ x = -x;
+ }
+ }
+ }
+
+ } else {
+ if(ir_num == FF){
+ power = MOTOR_P + 10;
+ }
+ x = power * ball_way[ir_num][0];
+ y = power * ball_way[ir_num][1];
+
+ }
+
+ line_check();
+
+ if((ir_num == NO_BALL)/* || (ball_on < 40)*/){
+ home();
+ }
+
+ //x = 30; y = 10;
+ line_state();
+
+ if((x == 0) && (y == 0)){
+ if(ping[BACK] > 25){
+ y = -HOME_P;
+ }
+ }
+
+ //x = 0; y = 0;
+
+ move(x,y,s);
+ }
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
while(1){
x = 0;
@@ -349,7 +446,7 @@
power = MOTOR_P;
if(/*(ir_dis < 60) && */( (ir_num == FF) || (ir_num == FFL) || (ir_num == FFR) )){
- kick = 25;
+ //kick = 25;
//if(130 < ping[LEFT]+ping[RIGHT]){
//if(goal == 0){
/* if(ping[LEFT] > 100){
@@ -365,12 +462,12 @@
goal = 0.0;
}
- if((ir_dis > 50) && (ir_dis < 150) && (ir_num != BB) && (ir_num != FL) && (ir_num != FR) && (ir_num != BL) && (ir_num != BR)){
+ if((ir_dis > 130) && (ir_dis < 150) && (ir_num != BB) && (ir_num != FL) && (ir_num != FR) && (ir_num != BL) && (ir_num != BR)){
x = power * way[ir_num][0];
y = power * way[ir_num][1];
} else {
if((ir_num == FF) || (ir_num == FFFL) || (ir_num == FFFR)){
- power = MOTOR_P + 5;
+ power = MOTOR_P + 10;
} else if((ir_num == FFL) || (ir_num == FFR)){
power = MOTOR_P;
} else {
@@ -391,6 +488,7 @@
home();
}
+ //x = 30; y = 10;
line_state();
if((x == 0) && (y == 0)){