nakao kanaki
/
Eco_ziguzagu
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
main.cpp
- Committer:
- namikosaito
- Date:
- 2016-02-24
- Revision:
- 0:77188ca200ce
- Child:
- 1:f9c953ddc87a
File content as of revision 0:77188ca200ce:
#include "mbed.h"
#include "math.h"
#include "LF2.h"
#include "color.h"
#include "enc_gyro.h"
/*ここで切り替え（強制）*/
//#define SLOPE
//#define TURN
#define RIVER
//#define RIVER2
#define DOWNHILL
/*白との差*/
#define bw 550 //blueとwhite
#define yw 200 //yellow ちょっと厳しめにすること。
Serial pc(USBTX, USBRX);
PwmOut servo(p25);
PwmOut blue_led(p21);
void save_servo(void){ //サーボの振れすぎを防ぐ。
if(out>2250)out=2250;
else if(out<650)out=650;
}
/*関数*/
void field(void); //フィールドの色を見る
int wr_flag,wl_flag;
int lf_downhill(void); //ダウンヒルのライントレース用。他のところでも使ってるけど。cdsの最大値、最小値、その差を出す。
int sa_dh,max_dh,min_dh; //lf_downhillで使う変数。
void back(void); //白線の左右どちらに振れたのか見る。（エンコーダー＋ジャイロ）
void back_cds(void); //同上（ｃｄｓ）
int river_turn(void); //リバーの最初の曲がりをcdsで線を読みとる
int back_r_flag=0;
int back_l_flag=0;
float yoko; //上りラインに対しての左右の振れ
int kakudo;
int downhill; //ダウンヒルのとき１になる。
int start_flag=1; //fieldで使用
int rg_flag_1,rg_flag_2,rg_count=0;
int gr_flag_1,gr_flag_2,gr_count=0;
///////////////////////////////////////////////////////////////////////////////////////////////
int main(){
mcp.format(7,0);
mcp.frequency(1000000);
pc.baud(115200);
gyro.format(16,3);
Enc.attach(&distance, sp_t);
servo.period_ms(20); //サーボ周期
servo.pulsewidth_us(naka);
blue_led=1;
/*キャリブレーション*/
calibration();
/*スイッチ切り替え*/
/*
if(switch_1==0&&switch_2==0&&switch_3==0){ //最初から
#define SLOPE
#define TURN
#define RIVER
#define DOWNHILL
}
if(switch_1==1&&switch_2==0&&switch_3==0){ //リバーから
#define RIVER
#define DOWNHILL
}
if(switch_1==1&&switch_2==1&&switch_3==0){ //ダウンヒルから
#define DOWNHILL
}
*/
#ifdef SLOPE
while(1){
//pc.printf("data=%d red=%d blue=%d yellow=%d white_l=%d ml=%d m=%d mr=%d r=%d\r\n",data_1,red,blue,yellow,white_l,white_ml,white_m,white_mr,white_r);
pc.printf("out=%d,dir=%d,sa=%f,now=%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n",out,dir,sa,now,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12);
//pc.printf("s1=%d,s2=%d,s3=%d,c1=%d,d1=%d,e1=%d\r\n",s1,s2,s3,c1,d1,e1);
field();
LF();
//back();
//back_cds();
save_servo();
servo.pulsewidth_us(out);
if(rg_count==0&&gr_count==1)led1=1;
if(rg_count==1&&gr_count==1)led2=1;
if(rg_count==1&&gr_count==2)led3=1;
if(rg_count==2&&gr_count==2)led4=1;
if(rg_count==2&&gr_count==3){
led1=0;
led2=0;
led3=0;
led4=0;
break;
}
}
#endif
/////////////////////////////////////////////////////////////////////////////////////
#ifdef TURN
while(1){
//gyro_keisan();
out=1750;
servo.pulsewidth_us(out);
led4=1;
if(fixed_rot_data<-60)break;
}
while(1){
judge_color();
led3=1;
if(white_l==1||white_r==1||white_mr==1||white_ml==1||white_m==1)break;
}
#endif
////////////////////////////////////////////////////////////////////////////////////
#ifdef RIVER
/*0*/
while(1){ //ライントレース
LF();
out-=50;
judge_color();
//gyro_keisan();
servo.pulsewidth_us(out);
//pc.printf("kyori_m45=%f,theta=%d linetrace\r\n",kyori_m45,fixed_rot_data);
//pc.printf("out=%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n",out,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12);
if(blue==1){ //青になったらリバースタート
river_flag=1;
out=naka-250;
servo.pulsewidth_us(out);
break;
}
}
while(1){
static int magari_flag;
magari_flag++;
if(magari_flag>30)break;
}
/*1*/
while(1){ //初めに曲がる直前まで
//gyro_keisan();
LF(); //基本的にライントレース
out-=50;
if(fixed_rot_data<-60)out=gyro_pid(-45,5,0,0);//out=naka-150; //最初は角度も利用
//if(fixed_rot_data<-75)out=naka-250;
led1=1;
//pc.printf("kyori_m45=%f,theta=%d\r\n",kyori_m45,fixed_rot_data);
if(river_flag==1){
kyori_reset();
river_flag=0;
}
servo.pulsewidth_us(out);
if((river_turn()==1&&kyori_m45>Runder)||kyori>400||kyori_m45>R){ //Rｍｍ進んだらorｃｄｓで直角検知
river_theta=fixed_rot_data;
river_flag=1;
break;
}}
river_keisan(); //角度と距離の補正（いらないかも・・）
/*2*/
while(1){ //向きを９０度に合わせる。
gyro_keisan();
led2=1;
//pc.printf("2 theta=%d\r\n",fixed_rot_data);
out=gyro_pid(-90-phai,7,0,5);
save_servo();
servo.pulsewidth_us(out);
if(river_flag==1){
kyori_reset();
river_flag=0;
}
if(fixed_rot_data<-85-phai){ //ジャイロの角度が合ったら
out=naka;
break;
}
}
/*3*/
while(1){ //直進する
led3=1;
gyro_keisan();
pc.printf("%f,%f,theta=%d,phai=%f,out=%d\r\n",kyori_m90,Z,fixed_rot_data,phai,out);
out=gyro_pid(-90-phai,6,0.1,0);
save_servo();
servo.pulsewidth_us(out);
if(kyori_m90>Z){ //Zmm進んだら
out=naka-200; //前は100
white_r=0;
white_l=0;
break;
}
}
/*4*/
while(1){ //白線を見つけるまでそのまま
gyro_keisan();
led4=1;
judge_color();
LF();
lf_downhill();
pc.printf("%f,%d,%d\r\n",kyori_m90,Ec_count,out);
if(kyori_m90>T)break; //行き過ぎ
if(white_r==1||white_l==1||white_mr==1||white_ml==1||white_m==1||sa_dh>bw){ //白線検知
break;
}
}
if(kyori_m90>T){
servo.pulsewidth_us(1100);
wait(0.5);
}
/*5*/
while(1){ //ライントレースに戻る
gyro_keisan();
LF();
back_cds();
out-=50;
if(fixed_rot_data<-45)out-=50;
if(fixed_rot_data<-75)out-=100;
servo.pulsewidth_us(out);
judge_color();
led1=0;
led2=0;
led3=0;
led4=0;
if(red==1){kyori_reset();break;}
}
#endif
////////////////////////////////////////////////////////////////////////////
#ifdef DOWNHILL
while(1){
static int q;
q++;
pc.printf("a kyori_dh=%f, theta=%d, out=%d,dir=%d,sa=%f,now=%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n",kyori_dh,fixed_rot_data,out,dir,sa,now,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12);
judge_color();
LF();
back_cds();
servo.pulsewidth_us(out);
if(yellow==1&&q>30){ //黄色に入る
kyori_reset();
blue_led=1;
downhill=1;
break;
}
}
while(1){
LF();
lf_downhill();
judge_color();
back_cds();
pc.printf("kyori_dh=%f,sa_dh=%d, theta=%d, out=%d,dir=%d,sa=%f,now=%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n",kyori_dh,sa_dh,fixed_rot_data,out,dir,sa,now,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12);
//gyro_keisan();
if(kyori_dh>50){
static int f;
if(sa_dh>yw+50){
f=now;
led1=0;
led2=1;
led3=0;
led4=0;
}
else if(sa_dh<yw&&f>2){
out=1800;
led1=0;
led2=0;
led3=0;
led4=1;
}
else if(sa_dh<yw&&f<-2){
out=1200;
led1=1;
led2=0;
led3=0;
led4=0;
}
}
if(kyori_dh<600){
out-=100;
}
else if(kyori_dh>600&&kyori_dh<900){
led3=1;
out-=120;
if((white_r==0&&white_mr==0&&white_l==0&&white_ml==0&&white_m==0)||sa_dh<yw){
out=1300;
out-=(yw-sa_dh)*0.15;
}
}
else if(kyori_dh>1200&&kyori_dh<1700){
out+=50;
}
else if(kyori_dh>1700&&kyori_dh<2700){
led3=1;
out+=180;
if((white_r==0&&white_mr==0&&white_l==0&&white_ml==0&&white_m==0)||sa_dh<yw){
out=1900;
out+=(yw-sa_dh)*0.15;
}
}
/*
if((kyori_dh>2000&&kyori_dh<2700&&(white_r==0&&white_mr==0&&white_m==0))||((kyori_dh>2200&&kyori_dh<2700)&&fixed_rot_data>15)){
while(1){
LF();
lf_downhill();
out=2100;
if(fixed_rot_data<0)out=2000;
if(fixed_rot_data<-15)out=1900;
if(fixed_rot_data<-30)out=1700;
servo.pulsewidth_us(out);
if((white_r==1||white_l==1||white_mr==1||white_ml==1||white_m==1)||sa_dh>yw)break;
}
}
*/
save_servo();
servo.pulsewidth_us(out);
}
#endif
}
//////////////////////////////////////////////////////////
int river_turn(void){
static int ritu,ritu_flag;
ritu=0;
ritu_flag=0;
if(s1-s12>bw&&s1-s11>bw)ritu++;
if(s2-s12>bw&&s2-s11>bw)ritu++;
if(s3-s12>bw&&s3-s11>bw)ritu++;
if(s4-s12>bw&&s4-s11>bw)ritu++;
if(s5-s12>bw&&s5-s11>bw)ritu++;
if(ritu>3)ritu_flag=1;
//pc.printf("ritu=%d",ritu);
return ritu_flag;
}
int lf_downhill(void){
/*cdsの値の最大*/
max_dh=s1;
if(max_dh<s2)max_dh=s2;
if(max_dh<s3)max_dh=s3;
if(max_dh<s4)max_dh=s4;
if(max_dh<s5)max_dh=s5;
if(max_dh<s6)max_dh=s6;
if(max_dh<s7)max_dh=s7;
if(max_dh<s8)max_dh=s8;
if(max_dh<s9)max_dh=s9;
if(max_dh<s10)max_dh=s10;
if(max_dh<s11)max_dh=s11;
if(max_dh<s12)max_dh=s12;
/*cdsの値の最小*/
min_dh=s1;
if(min_dh>s2)min_dh=s2;
if(min_dh>s3)min_dh=s3;
if(min_dh>s4)min_dh=s4;
if(min_dh>s5)min_dh=s5;
if(min_dh>s6)min_dh=s6;
if(min_dh>s7)min_dh=s7;
if(min_dh>s8)min_dh=s8;
if(min_dh>s9)min_dh=s9;
if(min_dh>s10)min_dh=s10;
if(min_dh>s11)min_dh=s11;
if(min_dh>s12)min_dh=s12;
sa_dh=max_dh-min_dh;
return sa_dh;
}
void back_cds(void){ //ｃｄｓでラインのどちらにずれたのか検知する。
Find_White();
static int back_cds_l,back_cds_l2,back_cds_r,back_cds_r2,flag_loop;
if(now==-3||now==-4){
back_cds_l=0;
back_cds_r=1;
}
if(back_cds_r==1&&(now==-5||now==-6)){
back_cds_r2=1;
back_cds_r=0;
}
if((back_cds_r2==1&&now!=-6&&now!=-5)||sa_dh<yw){//||(white_r==0&&white_l==0&&white_mr==0&&white_ml==0&&white_m==0)||sa_dh<yw){ //変更
while(1){
LF();
gyro_keisan();
lf_downhill();
Find_White(); //いらないはず
// out=1100;
pc.printf("hidari now %d\r\n",now);
out=1100;
save_servo();
servo.pulsewidth_us(out);
judge_color();
field();
if(now==-6)flag_loop=1;
if((flag_loop==1&&(now==-4||now==-5))||white_mr==1||white_m==1){
back_cds_r2=0;
flag_loop=0;
break;
}
static int yw_flag;
if(sa_dh>yw)yw_flag++;
if(yw_flag>10){
yw_flag=0;
back_cds_r2=0;
flag_loop=0;
break;}
}
}
if(now==3||now==4){
back_cds_l=1;
back_cds_r=0;
}
if(back_cds_l==1&&(now==5||now==6)){
back_cds_l=0;
back_cds_l2=1;
}
if((back_cds_l2==1&&now!=6&&now!=5)||sa_dh<yw){//||(white_r==0&&white_l==0&&white_mr==0&&white_ml==0&&white_m==0)||sa_dh<yw){
while(1){
LF();
gyro_keisan();
Find_White();
lf_downhill();
pc.printf("migi now=%d\r\n",now);
out=2100;
save_servo();
servo.pulsewidth_us(out);//1900
judge_color();
field();
if(now==6)flag_loop=1;
if((flag_loop==1&&(now==4||now==5))||white_ml==1||white_m==1){ //調べて
back_cds_l2=0;
flag_loop=0;
break;
}
static int yw_flag;
if(sa_dh>yw)yw_flag++;
if(yw_flag>10){
back_cds_l2=0;
flag_loop=0;
yw_flag=0;
break;}
}
}
}
void back(void){ //エンコーダーとジャイロで左右を見分ける方法（使ってない）
if(white_mr==1){
back_l_flag=0;
back_r_flag=1;
}
if(back_r_flag==1&&white_r==1){
while(1){
if((rg_count==0&&gr_count==0)||(rg_count==2&&gr_count==2))yoko=kyori_15x;
if((rg_count==0&&gr_count==1)||(rg_count==1&&gr_count==2))yoko=kyori_30x;
if(rg_count==1&&gr_count==1)yoko=kyori_45x;
out=1300;
pc.printf("hidari %d\r\n",out);
servo.pulsewidth_us(out);
judge_color();
field();
if(white_ml==1||white_l==1||white_m==1){
back_r_flag=0;
break;
}
static int yoko_flag;
if(yoko<-5)yoko_flag=2;
if(yoko_flag==2&&yoko>0)break;
if(yoko>5)yoko_flag=3;
if(yoko_flag==3&&yoko<0)break;
}
}
if(white_ml==1){
back_r_flag=0;
back_l_flag=1;
}
if(back_l_flag==1&&white_l==1){
while(1){
if((rg_count==0&&gr_count==0)||(rg_count==2&&gr_count==2))yoko=kyori_15x;
if((rg_count==0&&gr_count==1)||(rg_count==1&&gr_count==2))yoko=kyori_30x;
if(rg_count==1&&gr_count==1)yoko=kyori_45x;
out=1700;
pc.printf("migi %d\r\n",out);
servo.pulsewidth_us(out);
judge_color();
field();
if(white_mr==1||white_r==1||white_m==1){
back_r_flag=0;
break;
}
static int yoko_flag;
if(yoko<-5)yoko_flag=2;
if(yoko_flag==2&&yoko>0)break;
if(yoko>5)yoko_flag=3;
if(yoko_flag==3&&yoko<0)break;
}
}
}
void field(void){ //上りにおいてフィールドの色を判断し、数えていく。
judge_color();
if((white_r==1||white_l==1||white_mr==1||white_ml==1||white_m==1)&&downhill==0)kyori_reset();
if((gr_flag_1==1&&red==1&&kyori_nobori>500)||(start_flag==1&&red==1)){
gr_flag_2=1;
}
if(gr_flag_2==1){
kyori_nobori_reset();
start_flag=0;
gr_flag_1=0;
gr_flag_2=0;
rg_flag_1=1;
gr_count++;
}
if(blue==1&&rg_flag_1==1&&kyori_nobori>500){
rg_flag_2=1;
}
if(rg_flag_2==1){
kyori_nobori_reset();
rg_flag_1=0;
rg_flag_2=0;
gr_flag_1=1;
rg_count++;
}
}