Hajime Muraki
main
Dependencies: TextLCD mbed PID
Diff: main.cpp
- Revision:
- 1:fb4277ce4d93
- Parent:
- 0:e6d14fec4954
- Child:
- 2:59edff92b599
--- a/main.cpp Tue Feb 25 03:06:52 2014 +0000
+++ b/main.cpp Fri Feb 28 10:50:11 2014 +0000
@@ -1,19 +1,34 @@
#include "mbed.h"
-#include "HMC6352.h"
#include "TextLCD.h"
#include "common.h"
#include <math.h>
#include <sstream>
-#define LINE_P 70
+#define MOTOR_P 30
+#define LINE_LP 30
+#define LINE_FP 40
+#define LINE_ON 0xFFF0
+#define LINE_TIME 0.5
#define R 1.0
+#define S_MAX 5
+#define S1 15
+#define S2 10
+#define S3 5
+//誤差errの範囲でvalue1がvalue2と等しければ0以外を、等しくなれば0を返す
+#define ERR_EQ(value1,value2,err) ( ((value1) <= ((value2)+(err)))&&((value1) >= ((value2)-(err))) )
+DigitalIn sw(p5);
+DigitalIn start(p29);
DigitalOut myled[4] = {LED1, LED2, LED3, LED4};
Serial motor(p9,p10);
Serial sensor(p13,p14);
Serial pc(USBTX, USBRX);
TextLCD lcd(p26, p25, p24, p23, p22, p21);
-//AnalogIn adcIn[4] = {p17, p18, p19, p20};
+AnalogIn adcline[4] = {p16, p17, p19, p20};
+Timeout liner0;
+Timeout liner1;
+Timeout liner2;
+Timeout liner3;
//HMC6352 dcompass(p9,p10);
extern string StringFIN;
@@ -22,12 +37,25 @@
//uint16_t analogHex[4] = {0};
int speed[4] = {0};
+uint8_t value_ir = 0, ir_num = 0;
uint8_t ping[4] = {0};
-uint8_t line[4] = {0};
-uint8_t ir_max = 0, ir_num = 0;
+uint8_t line[4] = {0}, line_stop[4] = {0};
+uint8_t back = 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 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}
+};
void move(int vx, int vy, int vs){
@@ -57,22 +85,72 @@
//ライン判断
void line_state(){
if(line[0]){
- y = -LINE_P;
+ y = -LINE_FP;
}
if(line[1]){
- x = LINE_P;
+ x = LINE_LP;
}
if(line[2]){
- y = LINE_P;
+ y = LINE_FP;
}
if(line[3]){
- x = -LINE_P;
+ x = -LINE_LP;
}
}
+void lcds(int lcd1){
+ lcd.cls();
+ lcd.locate(0, 0);
+ lcd.printf("%d", lcd1);
+ //lcd.locate(2, 1);
+ //lcd.printf("YATTIYATTI");
+}
+
+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 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;
+}
+void line_stop1(){
+ line_stop[1] = 0;
+}
+void line_stop2(){
+ line_stop[2] = 0;
+}
+void line_stop3(){
+ line_stop[3] = 0;
+}
+
int main() {
//dcompass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+ uint8_t num = 0;
wait(1);
@@ -83,15 +161,141 @@
//compassdef = (compass / 10); //コンパス初期値保存
//compassdef = (dcompass.sample() / 10);
-
- wait(1);
-
- //while(1);
+ sw.mode(PullUp);
+ start.mode(PullUp);
- y = 20;
+ myled[0] = 1;
+ while(start){}
+ myled[0] = 0;
+/*
+ while(1){
+ //x = 30;
+ //y = 35;
+ 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;
+ }
+ move(x,y,s);
+ //pc.printf("%d\n", s);
+ //pc.printf("%d\n", adcline[3].read_u16());
+ }
+*/
+/* while(1){
+ //lcd_ping();
+ lcd_line();
+ wait(0.2);
+ }
+*/
+/* 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){
+ if(!line_stop[0]){
+ if(adcline[0].read_u16() > LINE_ON){
+ line[0] = 1;
+ line_stop[0] = 1;
+ myled[0] = 1;
+ liner0.attach(&line_stop0,LINE_TIME);
+ } else {
+ line[0] = 0;
+ myled[0] = 0;
+ }
+ }
+ if(!line_stop[1]){
+ if(adcline[1].read_u16() > LINE_ON){
+ line[1] = 1;
+ line_stop[1] = 1;
+ myled[1] = 1;
+ liner1.attach(&line_stop1,LINE_TIME);
+ } else {
+ line[1] = 0;
+ myled[1] = 0;
+ }
+ }
+ if(!line_stop[2]){
+ if(adcline[2].read_u16() > LINE_ON){
+ line[2] = 1;
+ line_stop[2] = 1;
+ myled[2] = 1;
+ liner2.attach(&line_stop2,LINE_TIME);
+ } else {
+ line[2] = 0;
+ myled[2] = 0;
+ }
+ }
+ if(!line_stop[3]){
+ if(adcline[3].read_u16() > LINE_ON){
+ line[3] = 1;
+ line_stop[3] = 1;
+ myled[3] = 1;
+ liner3.attach(&line_stop3,LINE_TIME);
+ } else {
+ line[3] = 0;
+ myled[3] = 0;
+ }
+ }
+ //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 = 30;//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 > S3){
+ s = S3;
+ } else if(s < -S3){
+ s = -S3;
+ }
+ line_state();
+ move(x,y,s);
+ }
+ //y = 20;
while(1){
- s = (compass - 180) / 5;
+ x = 20 * way[num][0];
+ y = 20 * way[num][1];
+ s = (compass - 180) / 3;
+
+ if(!sw){
+ num++;
+ wait(0.2);
+ if(num > 7){
+ num = 0;
+ }
+ lcds(num);
+ }
//pc.printf("%d\n", s);
move(x,y,s);