ryo seki
a
Diff: IR.cpp
- Revision:
- 0:62ac378df546
- Child:
- 1:ed00e9c4ba6e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IR.cpp Fri Apr 19 09:14:08 2013 +0000
@@ -0,0 +1,221 @@
+
+#include "mbed.h"
+#include "IR.h"
+
+extern Timer timer_ir;
+
+int direction = 0;
+int Distance = 0;
+int IR_found;
+
+void IR_Position(){
+
+ int ir_value[ALL_IR+100] = {0};
+
+ int active_ir = 0; /* 今回更新する赤外線の番号 */
+ int memory_ir = 0; /*赤外線時間カウンタ*/
+ int flag_ir = 0;
+ int value = 0;
+
+ static int direc = 99;
+ static int past_direc = 99;
+
+ int liftball;
+
+ for(int i=0; i<ALL_IR; i++) {
+ flag_ir = 1;
+
+ DigitalIn sensor_ir(ir_num[active_ir]); /* 今回更新する赤外線の個体を呼び出す */
+
+ timer_ir.start(); /* タイマー起動 */
+
+ if(sensor_ir) { /* もし立ち上がっていたら */
+ while(sensor_ir) { /* 立ち下がるまで待つ */
+ if(timer_ir.read_us() >= IR_TIME_NOTFOUND) {
+ flag_ir = 0;
+ break; /* 立ち上がっている時間が指定時間越えたらブレイク */
+ }
+ }
+ }
+
+ timer_ir.stop(); /* タイマー停止 */
+ timer_ir.reset(); /* タイマーリセット */
+
+ if(flag_ir) {
+ timer_ir.start(); /* タイマー起動 */
+
+ while(!(sensor_ir)) { /* 立ち上がるまで待つ */
+ if(timer_ir.read_us() >= IR_TIME_NOTFOUND) {
+ flag_ir = 0;
+ break; /* 立ち上がっている時間が指定時間越えたらブレイク */
+ }
+ }
+ }
+
+ /*ボールが指定時間内に見つかっていたら*/
+ if(flag_ir) {
+ memory_ir = timer_ir.read_us();
+
+ while(1) {
+ if((timer_ir.read_us()-memory_ir)>=IR_TIME_NOTFOUND)break;
+
+ if(!(sensor_ir)) {
+ //value = moving_ave( (timer_ir.read_us()-memory_ir)/10 , active_ir );
+ value = (timer_ir.read_us()-memory_ir)/10;
+
+ break;
+ }
+ }
+ } else {
+ /*ボールが見つかっていない場合*/
+ value = 0;
+ }
+ timer_ir.stop(); /* タイマー停止 */
+ timer_ir.reset(); /* タイマーリセット */
+
+ memory_ir = 0;
+
+ ir_value[active_ir] = value; //direction array
+
+ active_ir++;
+
+ if( active_ir >= ALL_IR) {
+ active_ir = 0;
+
+ /***********direction***********/
+
+ int min = 100,youso_min = 100;
+
+ for(int i = 0; i<DIREC_IR; i++) {
+ if((ir_value[i]<min)&&(ir_value[i])) {
+ min = ir_value[i];
+ youso_min = i;
+ }
+ }
+
+ if(youso_min == 8){
+ direc = 1;
+ }else if(youso_min == 9){
+ direc = 15;
+ }else{
+ direc = youso_min * 2;
+ }
+ /*
+ if(ir_value[youso_min] > 50){
+ liftball = 1;
+ }else{
+ liftball = 0;
+ }
+
+ if((past_direc != 99)&&(liftball == 0)){
+ if(past_direc == 0){
+ if((direc >= 4)&&(direc <= 12)){
+ direc = past_direc;
+ }
+ }else if(past_direc == 1){
+ if((direc >= 4)&&(direc <= 14)){
+ direc = past_direc;
+ }
+ }else if(past_direc == 15){
+ if((direc >= 2)&&(direc <= 12)){
+ direc = past_direc;
+ }
+ }else{
+ if(abs(past_direc - direc) >= 4){
+ direc = past_direc;
+ }
+ }
+ }*/
+
+ //direc = youso_min * 2;
+
+ /******* direction end *******/
+
+ /******* distance *******/
+
+ int dista;
+
+ if((ir_value[youso_min]<=28 + TERM)) {
+ dista = 30;
+ } else if((ir_value[youso_min]>28 + TERM)&&(ir_value[youso_min]<=35 + TERM)) {
+ dista = 90;
+ } else if((ir_value[youso_min]>35 + TERM)&&(ir_value[youso_min]<=40 + TERM)) {
+ dista = 120;
+ } else if( ir_value[youso_min]>40 + TERM) {
+ dista = 180;
+ } else {
+ dista = 0;
+ }
+
+ int count_ir = 0,total_ir = 0;
+
+ for(int i=0; i<DIREC_IR; i++){
+ if(ir_value[i]){
+ total_ir += ir_value[i];
+ count_ir++;
+ }
+ }
+
+ double hihhihi = 0;
+
+ if(!ir_value[10]) ir_value[10] = 1000000;
+
+ hihhihi = (double)ir_value[youso_min]/(double)ir_value[10];
+
+ if(ir_value[10] <= 38){
+ if((direc == 0)&&(hihhihi >= 1.0)){
+ dista = 10;
+ }else if((direc == 1)&&(hihhihi >= 1.0)){
+ dista = 10;
+ }else if((direc == 2)&&(hihhihi >= 0.75)){
+ dista = 10;
+ }else if((direc == 3)&&(hihhihi >= 0.65)){
+ dista = 10;
+ }else if((direc == 4)&&(hihhihi >= 0.60)){
+ dista = 10;
+ }else if((direc == 5)&&(hihhihi >= 0.60)){
+ dista = 10;
+ }else if((direc == 6)&&(hihhihi >= 0.70)){
+ dista = 10;
+ }else if((direc == 7)&&(hihhihi >= 0.70)){
+ dista = 10;
+ }else if((direc == 8)&&(hihhihi >= 0.70)){
+ dista = 10;
+ }else if((direc == 9)&&(hihhihi >= 0.70)){
+ dista = 10;
+ }else if((direc == 10)&&(hihhihi >= 0.60)){
+ dista = 10;
+ }else if((direc == 11)&&(hihhihi >= 0.60)){
+ dista = 10;
+ }else if((direc == 12)&&(hihhihi >= 0.60)){
+ dista = 10;
+ }else if((direc == 13)&&(hihhihi >= 0.65)){
+ dista = 10;
+ }else if((direc == 14)&&(hihhihi >= 0.75)){
+ dista = 10;
+ }else if((direc == 15)&&(hihhihi >= 1.0)){
+ dista = 10;
+ }
+ }
+
+ int count = 0;
+
+ for(int i=0;i<DIREC_IR;i++){
+ if(ir_value[i])count++;
+ }
+
+ if(count) IR_found = 1;
+ else IR_found = 0;
+
+
+ /******** distance end *******/
+
+ past_direc = direc;
+
+ direction = direc;
+ Distance = dista;
+
+ //printf("derection:%d distance:%d\n",direction,Distance);
+ }
+ }
+}
\ No newline at end of file