Hajime Muraki
main
Dependencies: TextLCD mbed PID
Diff: main.cpp
- Revision:
- 3:440e774cc24b
- Parent:
- 2:59edff92b599
- Child:
- 4:536cd493a337
--- a/main.cpp Mon Mar 03 00:24:44 2014 +0000
+++ b/main.cpp Wed Mar 12 04:51:30 2014 +0000
@@ -5,21 +5,27 @@
#include <math.h>
#include <sstream>
-#define MOTOR_P 30
-#define NO_IR 45
+#define MOTOR_P 26
+#define HOME_P 25
+#define NO_BALL 45
#define LINE_LP 30
-#define LINE_FP 30
+#define LINE_FP 35
#define LINE_ON 0xFFF0
-#define LINE_TIME 0.5
+#define LINE_TIME 0.6
+#define LINE_PING 40
#define R 1.0
#define S_MAX 5
#define S1 15
#define S2 10
#define S3 5
#define RATE 0.05
-#define P_GAIN 0.8
+#define P_GAIN 1.2
#define I_GAIN 0.0
-#define D_GAIN 0.02
+#define D_GAIN 0.015
+#define HOME_BACK 30
+#define HOME 1
+#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))) )
@@ -45,29 +51,59 @@
//uint16_t analogHex[4] = {0};
int speed[4] = {0};
-uint8_t value_ir = 0, ir_num = 0;
+uint8_t value_ir = 0, ir_num = 0, ir_dis = 0, ball_on = 0;
uint8_t ping[4] = {0};
-uint8_t line[4] = {0}, line_stop[4] = {0};
-uint8_t back = 0;
+uint8_t line[4] = {0}, line_stop[4] = {0}, line_ping[4] = {0};
+uint8_t kick = 0;
+//uint8_t robot_state = 0;
int compass = 0;
int x = 0, y = 0, s = 0, i = 0, line_on = 0;
int compassdef = 0, data = 0;
uint8_t pingdef[4] = {0};
double pidinput = 180.0;
-double compasspid = 0;
+double compasspid = 0.0;
+double goal = 0.0;
-double way[8][2] = {
- { 0 , 1 },
- {-0.707, 0.707},
- {-1 , 0 },
- {-0.707,-0.707},
- { 0 ,-1 },
- { 0.707,-0.707},
- { 1 , 0 },
- { 0.707, 0.707}
+double way[10][2] = {
+ { 0 , 1 }, //FF
+ {-0.707, 0.707}, //FL
+ {-1 , 0 }, //LL
+ {-0.707,-0.707}, //BL
+ { 0 ,-1 }, //BB
+ { 0.707,-0.707}, //BR
+ { 1 , 0 }, //RR
+ { 0.707, 0.707}, //FR
+ {-0.500, 0.866}, //FFL
+ { 0.500, 0.866} //FFR
+};
+double ball_way[12][2] = {
+ { 0.0 , 1.0 }, //FF
+ {-0.985,-0.173}, //FL
+ {-0.342,-0.940}, //LL
+ { 0.342,-0.940}, //BL
+ { 0.966, 0.259}, //BB
+ {-0.342,-0.940}, //BR
+ { 0.342,-0.940}, //RR
+ { 0.985,-0.173}, //FR
+ {-0.985, 0.174}, //FFL
+ { 0.985, 0.174}, //FFR
+ {-0.173, 0.985}, //FFFL
+ { 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};
@@ -75,7 +111,7 @@
pwm[0] = (double)((vx) + vs);
pwm[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + vs);
pwm[2] = (double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy) + vs);
- pwm[3] = 0;
+ pwm[3] = kick;
for(i = 0; i < 4; i++){
if(pwm[i] > 100){
@@ -94,27 +130,59 @@
}
//ライン判断
-void line_state(){
- if(line[0]){
- y = -LINE_FP;
+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;
+ }
}
- if(line[1]){
- x = LINE_LP;
- }
- if(line[2]){
+
+ if(line[BACK]){
y = LINE_FP;
+ } else if(line_ping[BACK]){
+ if(ping[BACK] < LINE_PING){
+ if(y < 10){
+ y = 10;
+ }
+ } else {
+ line_ping[BACK] = 0;
+ myled[2] = 0;
+ }
}
- if(line[3]){
+
+ if(line[RIGHT]){
x = -LINE_LP;
+ } else if(line_ping[RIGHT]){
+ if(ping[RIGHT] < LINE_PING){
+ if(x > 10){
+ x = -10;
+ }
+ } else {
+ line_ping[RIGHT] = 0;
+ myled[3] = 0;
+ }
}
-}
-
-void lcds(int lcd1){
- lcd.cls();
- lcd.locate(0, 0);
- lcd.printf("%d", lcd1);
- //lcd.locate(2, 1);
- //lcd.printf("YATTIYATTI");
+
+ 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(){
@@ -129,79 +197,69 @@
lcd.printf("%03d %03d\n%03d %03d", line[FRONT], line[LEFT], line[BACK], line[RIGHT]);
}
-
-void home(){
- if(ping[LEFT] > 80){
- x = -20;
- } else if(ping[RIGHT] > 80){
- x = 20;
- }
- back = 30;
-/* if(EER_EQ(ping[LEFT]+ping[RIGHT],pingdef[LEFT]+pingdef[RIGHT], 5)){
- back = 10;
- }*/
- if(ping[BACK] > back){
- y = -20;
- }
-}
-
void line_stop0(){
- line_stop[0] = 0;
+ line_stop[FRONT] = 0;
}
void line_stop1(){
- line_stop[1] = 0;
+ line_stop[LEFT] = 0;
}
void line_stop2(){
- line_stop[2] = 0;
+ line_stop[BACK] = 0;
}
void line_stop3(){
- line_stop[3] = 0;
+ line_stop[RIGHT] = 0;
}
void line_check()
{
- if(!line_stop[0]){
- if(adcline[0].read_u16() > LINE_ON){
- line[0] = 1;
- line_stop[0] = 1;
+ if(!line_stop[FRONT]){
+ if(adcline[FRONT].read_u16() > LINE_ON){
+ line[FRONT] = 1;
+ line_stop[FRONT] = 1;
+ line_ping[FRONT] = 1;
myled[0] = 1;
- liner0.attach(&line_stop0,LINE_TIME);
+ liner0.attach(&line_stop0,1);
} else {
- line[0] = 0;
- myled[0] = 0;
+ line[FRONT] = 0;
+ ////myled[0] = 0;
}
}
- if(!line_stop[1]){
- if(adcline[1].read_u16() > LINE_ON){
- line[1] = 1;
- line_stop[1] = 1;
+ if(!line_stop[LEFT]){
+ if(adcline[LEFT].read_u16() > LINE_ON){
+ line[LEFT] = 1;
+ line_stop[LEFT] = 1;
+ line_ping[LEFT] = 1;
myled[1] = 1;
liner1.attach(&line_stop1,LINE_TIME);
} else {
- line[1] = 0;
- myled[1] = 0;
+ line[LEFT] = 0;
+ ////myled[1] = 0;
}
}
- if(!line_stop[2]){
- if(adcline[2].read_u16() > LINE_ON){
- line[2] = 1;
- line_stop[2] = 1;
+ if(!line_stop[BACK]){
+ if(adcline[BACK].read_u16() > LINE_ON){
+ line[BACK] = 1;
+ line_stop[BACK] = 1;
+ line_stop[FRONT] = 1;
+ line_ping[BACK] = 1;
myled[2] = 1;
+ liner0.attach(&line_stop0,LINE_TIME);
liner2.attach(&line_stop2,LINE_TIME);
} else {
- line[2] = 0;
- myled[2] = 0;
+ line[BACK] = 0;
+ ////myled[2] = 0;
}
}
- if(!line_stop[3]){
- if(adcline[3].read_u16() > LINE_ON){
- line[3] = 1;
- line_stop[3] = 1;
+ if(!line_stop[RIGHT]){
+ if(adcline[RIGHT].read_u16() > LINE_ON){
+ line[RIGHT] = 1;
+ line_stop[RIGHT] = 1;
+ line_ping[RIGHT] = 1;
myled[3] = 1;
liner3.attach(&line_stop3,LINE_TIME);
} else {
- line[3] = 0;
- myled[3] = 0;
+ line[RIGHT] = 0;
+ ////myled[3] = 0;
}
}
}
@@ -209,16 +267,42 @@
void pidupdate()
{
+ pid.setSetPoint(180.0 + goal);
pidinput = compass;
pid.setProcessValue(pidinput);
compasspid = -pid.compute();
}
+void home()
+{
+ x = 0;
+ y = 0;
+ kick = 0;
+ goal = 0.0;
+
+ // if((145 < ping[LEFT]+ping[RIGHT]) && (ping[LEFT]+ping[RIGHT] < 155)){
+ if(ping[LEFT] > PING_LR){
+ x = -HOME_P;
+ y = -5;
+ } else if(ping[RIGHT] > PING_LR){
+ x = HOME_P;
+ y = -5;
+ }
+ // }
+
+ if(ping[BACK] > HOME_BACK){
+ y = -HOME_P;
+ } else if(ping[BACK] < 5){
+ y = HOME_P;
+ }
+}
+
int main() {
//dcompass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
- uint8_t num = 0;
+ //uint8_t num = 0;
+ uint8_t dir = 0, power = 0;
wait(1);
@@ -238,193 +322,85 @@
sw.mode(PullUp);
start.mode(PullUp);
- myled[0] = 1;
- while(start){}
- myled[0] = 0;
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("Chronograph");
- /*
- while(1){
- s = compasspid;
- pc.printf("%d\n", s);
+
+ myled[0] = 1;
+ while(!start){
+ lcd.locate(6,1);
+ lcd.printf("%d", compass);
wait(0.1);
}
- */
-
+ myled[0] = 0;
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("Chronograph");
+ lcd.locate(7,1);
+ lcd.printf("ABURASOBA");
+
+
while(1){
+
+ x = 0;
+ y = 0;
+ s = compasspid;
+ power = MOTOR_P;
+
+ if(/*(ir_dis < 60) && */( (ir_num == FF) || (ir_num == FFL) || (ir_num == FFR) )){
+ kick = 25;
+ //if(130 < ping[LEFT]+ping[RIGHT]){
+ //if(goal == 0){
+ /* if(ping[LEFT] > 100){
+ goal = -GOAL;
+ }*//* else if(ping[RIGHT] > 100){
+ //goal = GOAL;
+ }*/
+ //}
+ //}
+
+ } else {
+ kick = 0;
+ 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)){
+ 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;
+ } else if((ir_num == FFL) || (ir_num == FFR)){
+ power = MOTOR_P;
+ } else {
+ power = MOTOR_P + 5;
+ }
+ 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;
+ }
+ }
+ }
+
line_check();
- //x = MOTOR_P;
- //y = 0;
- if(ir_num > 7){
- ir_num = 0;
- }
- x = MOTOR_P * way[ir_num][0];
- y = MOTOR_P * way[ir_num][1];
- s = compasspid;
-
- if(value_ir > NO_IR){
+ if((ir_num == NO_BALL)/* || (ball_on < 40)*/){
home();
}
+
+ line_state();
- line_state();
+ if((x == 0) && (y == 0)){
+ if(ping[BACK] > 25){
+ y = -HOME_P;
+ }
+ }
+
+ //x = 0; y = 0;
move(x,y,s);
}
- while(1){
- line_check();
- //pc.printf("%d\n", adcline[1].read_u16());
- //pc.printf("%d %d %d %d\n", line[0], line[1], line[2], line[3]);
- if(ir_num > 7){
- ir_num = 0;
- }
- x = MOTOR_P;//MOTOR_P * way[ir_num][0];
- y = 0;//MOTOR_P * way[ir_num][1];
- s = (compass - 180) / 3;
- if(s > S1){
- s = S1;
- } else if(s < -S1){
- s = -S1;
- } else if(s > S2){
- s = S2;
- } else if(s < -S2){
- s = -S2;
- } else if(s > 0){
- s = S3;
- } else if(s < 0){
- s = -S3;
- }
-
- line_state();
-
- move(x,y,s);
- }
-
-/*
- while(1){
- x = MOTOR_P * way[num][0];
- y = MOTOR_P * way[num][1];
- s = (compass - 180) / 3;
- if(s > S1){
- s = S1;
- } else if(s < -S1){
- s = -S1;
- } else if(s > S2){
- s = S2;
- } else if(s < -S2){
- s = -S2;
- } else if(s > S3){
- s = S3;
- } else if(s < -S3){
- s = -S3;
- }
- if(!sw){
- num++;
- wait(0.2);
- if(num > 7){
- num = 0;
- }
- lcds(num);
- }
- //pc.printf("%d\n", s);
-
- move(x,y,s);
- //wait(0.1);
- }
-*/
-/*
- while(1){
- x = 30;
- y = 0;
- move(x,y,s);
- wait(0.5);
- x = -30;
- y = 0;
- move(x,y,s);
- wait(0.5);
- //pc.printf("%d\n", s);
- //pc.printf("%d\n", adcline[3].read_u16());
- }
-*/
-
-/* while(1){
- if(ir_num > 7){
- ir_num = 0;
- }
- x = MOTOR_P * way[ir_num][0];
- y = MOTOR_P * way[ir_num][1];
- s = (compass - 180) / 3;
- if(s > S_MAX){
- s = S_MAX;
- } else if(s < -S_MAX){
- s = -S_MAX;
- }
-
- move(x,y,s);
- }
- */
-/*
- while(1){
- i = 3;
- analogHex[i] = adcIn[i].read_u16();
- if(analogHex[i] > 10000){
- myled[i] = 1;
- line[i] = 1;
- line_on = 1;
- } else {
- myled[i] = 0;
- line[i] = 0;
- line_on = 0;
- }
-
- x = 30;
- y = 0;
-
- if(line[1]){
- x = LINE_P;
- } else if(line[3]){
- x = -LINE_P;
- }
-
- if(line[0]){
- y = -LINE_P;
- } else if(line[2]){
- y = LINE_P;
- }
- move(x,y,s);
-
- if(line_on)wait(0.3);
- }
- */
- /*
- while(1){
- for(i = 0; i < 4; i++){
- analogHex[i] = adcIn[i].read_u16();
- if(analogHex[i] > 10000){
- myled[i] = 1;
- line[i] = 1;
- } else {
- myled[i] = 0;
- line[i] = 0;
- }
- //wait(0.1);
- }
-
- if(line[0]){
- x = -30;
- } else if(line[2]){
- x = 30;
- }
-
- if(line[1]){
- y = 30;
- } else if(line[3]){
- y = -30;
- }
-
- move(x,y);
- x = 0;
- y = 0;
- //pc.printf("%05d %05d %05d %05d\n", analogHex[0], analogHex[1], analogHex[2], analogHex[3]);
- }
- */
}