Kang hokyoung / Mbed 2 deprecated 2014_Ensoul_Capstone

2014_Ensoul_Capstone

Dependencies:   TextLCD Ultrasonic mbed BufferedSoftSerial

main.cpp@0:d429f13fe4be, 2014-07-03 (annotated)

Committer:
MR_Kang
Date:
Thu Jul 03 05:13:08 2014 +0000
Revision:
0:d429f13fe4be
Child:
1:2859bfed20b4
Publish

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MR_Kang 0:d429f13fe4be 1 #include "mbed.h"
MR_Kang 0:d429f13fe4be 2 #include "Ultrasonic.h"
MR_Kang 0:d429f13fe4be 3
MR_Kang 0:d429f13fe4be 4 AnalogIn Sharp(p20);
MR_Kang 0:d429f13fe4be 5 Ultrasonic F_sonic_R(p11,p11);
MR_Kang 0:d429f13fe4be 6 Ultrasonic F_sonic_L(p12,p12);
MR_Kang 0:d429f13fe4be 7 Ultrasonic F_sonic_F(p8,p8);
MR_Kang 0:d429f13fe4be 8
MR_Kang 0:d429f13fe4be 9 PwmOut RMotor_Front(p21);
MR_Kang 0:d429f13fe4be 10 PwmOut LMotor_Front(p23);
MR_Kang 0:d429f13fe4be 11 PwmOut RMotor_Back(p22);
MR_Kang 0:d429f13fe4be 12 PwmOut LMotor_Back(p24);
MR_Kang 0:d429f13fe4be 13 DigitalOut RMotor_EN(p5);
MR_Kang 0:d429f13fe4be 14 DigitalOut LMotor_EN(p6);
MR_Kang 0:d429f13fe4be 15 Timeout motortime;
MR_Kang 0:d429f13fe4be 16 Timeout backmotortime;
MR_Kang 0:d429f13fe4be 17 Timeout Rightmotortime;
MR_Kang 0:d429f13fe4be 18 Timeout Leftmotortime;
MR_Kang 0:d429f13fe4be 19 float a,b =0;
MR_Kang 0:d429f13fe4be 20 float length;
MR_Kang 0:d429f13fe4be 21 void Front()
MR_Kang 0:d429f13fe4be 22 {
MR_Kang 0:d429f13fe4be 23 RMotor_EN = 1;
MR_Kang 0:d429f13fe4be 24 LMotor_EN = 1;
MR_Kang 0:d429f13fe4be 25 RMotor_Back.pulsewidth(0);
MR_Kang 0:d429f13fe4be 26 LMotor_Back.pulsewidth(0);
MR_Kang 0:d429f13fe4be 27 RMotor_Front.pulsewidth(0.05);
MR_Kang 0:d429f13fe4be 28 LMotor_Front.pulsewidth(0.05);
MR_Kang 0:d429f13fe4be 29 }
MR_Kang 0:d429f13fe4be 30
MR_Kang 0:d429f13fe4be 31 void Turn_R()
MR_Kang 0:d429f13fe4be 32 {
MR_Kang 0:d429f13fe4be 33 RMotor_EN = 1;
MR_Kang 0:d429f13fe4be 34 LMotor_EN =1;
MR_Kang 0:d429f13fe4be 35 LMotor_Front.pulsewidth(0);
MR_Kang 0:d429f13fe4be 36 RMotor_Back.pulsewidth(0);
MR_Kang 0:d429f13fe4be 37 LMotor_Back.pulsewidth(0.1);
MR_Kang 0:d429f13fe4be 38 RMotor_Front.pulsewidth(0.1);
MR_Kang 0:d429f13fe4be 39 }
MR_Kang 0:d429f13fe4be 40
MR_Kang 0:d429f13fe4be 41 void Turn_L()
MR_Kang 0:d429f13fe4be 42 {
MR_Kang 0:d429f13fe4be 43 LMotor_EN = 1;
MR_Kang 0:d429f13fe4be 44 RMotor_EN = 1;
MR_Kang 0:d429f13fe4be 45 LMotor_Back.pulsewidth(0);
MR_Kang 0:d429f13fe4be 46 RMotor_Front.pulsewidth(0);
MR_Kang 0:d429f13fe4be 47 RMotor_Back.pulsewidth(0.1);
MR_Kang 0:d429f13fe4be 48 LMotor_Front.pulsewidth(0.1);
MR_Kang 0:d429f13fe4be 49 }
MR_Kang 0:d429f13fe4be 50 void Back()
MR_Kang 0:d429f13fe4be 51 {
MR_Kang 0:d429f13fe4be 52 RMotor_EN = 1;
MR_Kang 0:d429f13fe4be 53 LMotor_EN = 1;
MR_Kang 0:d429f13fe4be 54 RMotor_Front.pulsewidth(0);
MR_Kang 0:d429f13fe4be 55 LMotor_Front.pulsewidth(0);
MR_Kang 0:d429f13fe4be 56 RMotor_Back.pulsewidth(0.05);
MR_Kang 0:d429f13fe4be 57 LMotor_Back.pulsewidth(0.05);
MR_Kang 0:d429f13fe4be 58 }
MR_Kang 0:d429f13fe4be 59 void Break()
MR_Kang 0:d429f13fe4be 60 {
MR_Kang 0:d429f13fe4be 61 RMotor_Front.pulsewidth(0);
MR_Kang 0:d429f13fe4be 62 LMotor_Front.pulsewidth(0);
MR_Kang 0:d429f13fe4be 63 RMotor_Back.pulsewidth(0.05);
MR_Kang 0:d429f13fe4be 64 LMotor_Back.pulsewidth(0.05);
MR_Kang 0:d429f13fe4be 65 wait(0.1);
MR_Kang 0:d429f13fe4be 66 RMotor_EN = 0;
MR_Kang 0:d429f13fe4be 67 LMotor_EN = 0;
MR_Kang 0:d429f13fe4be 68 }
MR_Kang 0:d429f13fe4be 69 void Stop()
MR_Kang 0:d429f13fe4be 70 {
MR_Kang 0:d429f13fe4be 71 RMotor_EN = 0;
MR_Kang 0:d429f13fe4be 72 LMotor_EN = 0;
MR_Kang 0:d429f13fe4be 73 RMotor_Front.pulsewidth(0);
MR_Kang 0:d429f13fe4be 74 LMotor_Front.pulsewidth(0);
MR_Kang 0:d429f13fe4be 75 RMotor_Back.pulsewidth(0);
MR_Kang 0:d429f13fe4be 76 LMotor_Back.pulsewidth(0);
MR_Kang 0:d429f13fe4be 77 }
MR_Kang 0:d429f13fe4be 78
MR_Kang 0:d429f13fe4be 79
MR_Kang 0:d429f13fe4be 80 int dis_L,dis_R,dis_F = 80;
MR_Kang 0:d429f13fe4be 81 int F_len;
MR_Kang 0:d429f13fe4be 82 int move,measure,flag=1;
MR_Kang 0:d429f13fe4be 83
MR_Kang 0:d429f13fe4be 84 void count()
MR_Kang 0:d429f13fe4be 85 {
MR_Kang 0:d429f13fe4be 86 dis_F =100;
MR_Kang 0:d429f13fe4be 87 dis_L =100;
MR_Kang 0:d429f13fe4be 88 dis_R =100;
MR_Kang 0:d429f13fe4be 89 move =0;
MR_Kang 0:d429f13fe4be 90 measure =0;
MR_Kang 0:d429f13fe4be 91 flag=1;
MR_Kang 0:d429f13fe4be 92 }
MR_Kang 0:d429f13fe4be 93
MR_Kang 0:d429f13fe4be 94 void backcount(){
MR_Kang 0:d429f13fe4be 95 move = 1; measure =0;
MR_Kang 0:d429f13fe4be 96 }
MR_Kang 0:d429f13fe4be 97 void Rightcount(){
MR_Kang 0:d429f13fe4be 98 move = 2; measure =0;
MR_Kang 0:d429f13fe4be 99 }
MR_Kang 0:d429f13fe4be 100 void Leftcount(){
MR_Kang 0:d429f13fe4be 101 move = 3; measure =0;
MR_Kang 0:d429f13fe4be 102 }
MR_Kang 0:d429f13fe4be 103 int main()
MR_Kang 0:d429f13fe4be 104 {
MR_Kang 0:d429f13fe4be 105
MR_Kang 0:d429f13fe4be 106 while(1) {
MR_Kang 0:d429f13fe4be 107
MR_Kang 0:d429f13fe4be 108 //거리측정
MR_Kang 0:d429f13fe4be 109 if(dis_F==F_sonic_F.read()/10) {
MR_Kang 0:d429f13fe4be 110 dis_F = 80;}
MR_Kang 0:d429f13fe4be 111 if(dis_R==F_sonic_R.read()/10) {
MR_Kang 0:d429f13fe4be 112 dis_R = 90;}
MR_Kang 0:d429f13fe4be 113 if(dis_L==F_sonic_L.read()/10) {
MR_Kang 0:d429f13fe4be 114 dis_L = 90;}
MR_Kang 0:d429f13fe4be 115
MR_Kang 0:d429f13fe4be 116 if(dis_F!=F_sonic_F.read()/10) {
MR_Kang 0:d429f13fe4be 117 dis_F = F_sonic_F.read()/10;
MR_Kang 0:d429f13fe4be 118 }
MR_Kang 0:d429f13fe4be 119 if(dis_R!=F_sonic_R.read()/10) {
MR_Kang 0:d429f13fe4be 120 dis_R = F_sonic_R.read()/10;
MR_Kang 0:d429f13fe4be 121 }
MR_Kang 0:d429f13fe4be 122 if(dis_L!=F_sonic_L.read()/10) {
MR_Kang 0:d429f13fe4be 123 dis_L = F_sonic_L.read()/10;
MR_Kang 0:d429f13fe4be 124 }
MR_Kang 0:d429f13fe4be 125
MR_Kang 0:d429f13fe4be 126 printf("Front: %d\n",dis_F);
MR_Kang 0:d429f13fe4be 127 printf("Right: %d\n",dis_L);
MR_Kang 0:d429f13fe4be 128 printf("Left : %d\n",dis_R);
MR_Kang 0:d429f13fe4be 129 printf("move : %d\n",move);
MR_Kang 0:d429f13fe4be 130
MR_Kang 0:d429f13fe4be 131 /* if(dis_R <50) {
MR_Kang 0:d429f13fe4be 132 if(dis_L <50){
MR_Kang 0:d429f13fe4be 133 if(dis_F >50){
MR_Kang 0:d429f13fe4be 134 move =0;}}}
MR_Kang 0:d429f13fe4be 135 else if(dis_L <50) {
MR_Kang 0:d429f13fe4be 136 if(dis_R <50){
MR_Kang 0:d429f13fe4be 137 if(dt is_F >50){
MR_Kang 0:d429f13fe4be 138 move =0;}}}*/
MR_Kang 0:d429f13fe4be 139
MR_Kang 0:d429f13fe4be 140
MR_Kang 0:d429f13fe4be 141 switch(measure){
MR_Kang 0:d429f13fe4be 142 case 0:
MR_Kang 0:d429f13fe4be 143 if(dis_F >30) {
MR_Kang 0:d429f13fe4be 144 if(dis_R <50) {
MR_Kang 0:d429f13fe4be 145 if((dis_R + 5) < dis_L){
MR_Kang 0:d429f13fe4be 146 move =2; measure =1;}
MR_Kang 0:d429f13fe4be 147 } else if(dis_L <50) {
MR_Kang 0:d429f13fe4be 148 if((dis_L + 5) < dis_R){
MR_Kang 0:d429f13fe4be 149 move =3;measure =1;}
MR_Kang 0:d429f13fe4be 150 } else {
MR_Kang 0:d429f13fe4be 151 move =0; measure =1;
MR_Kang 0:d429f13fe4be 152 }
MR_Kang 0:d429f13fe4be 153 } else if(dis_F<30) {
MR_Kang 0:d429f13fe4be 154 move =1; measure =1;
MR_Kang 0:d429f13fe4be 155 }
MR_Kang 0:d429f13fe4be 156 break;
MR_Kang 0:d429f13fe4be 157 }
MR_Kang 0:d429f13fe4be 158
MR_Kang 0:d429f13fe4be 159
MR_Kang 0:d429f13fe4be 160 if(dis_R <10){move=5;}
MR_Kang 0:d429f13fe4be 161 else if(dis_L <10){ move =6;}
MR_Kang 0:d429f13fe4be 162 else if((dis_R <25) &&(dis_L<25)){move =4;}
MR_Kang 0:d429f13fe4be 163 else if(dis_R <35){move =2;}
MR_Kang 0:d429f13fe4be 164 else if(dis_L <35){move =3;}
MR_Kang 0:d429f13fe4be 165
MR_Kang 0:d429f13fe4be 166 switch(move) {
MR_Kang 0:d429f13fe4be 167 case 0://Front
MR_Kang 0:d429f13fe4be 168 Front();
MR_Kang 0:d429f13fe4be 169 measure =0;
MR_Kang 0:d429f13fe4be 170 break;
MR_Kang 0:d429f13fe4be 171
MR_Kang 0:d429f13fe4be 172 case 1://Back
MR_Kang 0:d429f13fe4be 173 Stop();
MR_Kang 0:d429f13fe4be 174 measure =0;
MR_Kang 0:d429f13fe4be 175 break;
MR_Kang 0:d429f13fe4be 176
MR_Kang 0:d429f13fe4be 177 case 2://Right
MR_Kang 0:d429f13fe4be 178 Turn_R();
MR_Kang 0:d429f13fe4be 179 if (flag==1) {
MR_Kang 0:d429f13fe4be 180 flag=0;
MR_Kang 0:d429f13fe4be 181 motortime.attach(&count,1);
MR_Kang 0:d429f13fe4be 182 }
MR_Kang 0:d429f13fe4be 183 wait(1);
MR_Kang 0:d429f13fe4be 184 break;
MR_Kang 0:d429f13fe4be 185
MR_Kang 0:d429f13fe4be 186 case 3://Left
MR_Kang 0:d429f13fe4be 187 Turn_L();
MR_Kang 0:d429f13fe4be 188 if (flag==1) {
MR_Kang 0:d429f13fe4be 189 flag=0;
MR_Kang 0:d429f13fe4be 190 motortime.attach(&count,1);
MR_Kang 0:d429f13fe4be 191 }
MR_Kang 0:d429f13fe4be 192 wait(1);
MR_Kang 0:d429f13fe4be 193 break;
MR_Kang 0:d429f13fe4be 194
MR_Kang 0:d429f13fe4be 195 case 4://back
MR_Kang 0:d429f13fe4be 196 Back();
MR_Kang 0:d429f13fe4be 197 backmotortime.attach(&backcount,2);
MR_Kang 0:d429f13fe4be 198 break;
MR_Kang 0:d429f13fe4be 199 case 5://Right
MR_Kang 0:d429f13fe4be 200 Back();
MR_Kang 0:d429f13fe4be 201 Rightmotortime.attach(&Rightcount,2);
MR_Kang 0:d429f13fe4be 202 break;
MR_Kang 0:d429f13fe4be 203 case 6://back
MR_Kang 0:d429f13fe4be 204 Back();
MR_Kang 0:d429f13fe4be 205 Leftmotortime.attach(&Leftcount,2);
MR_Kang 0:d429f13fe4be 206 break;
MR_Kang 0:d429f13fe4be 207 }
MR_Kang 0:d429f13fe4be 208 }
MR_Kang 0:d429f13fe4be 209
MR_Kang 0:d429f13fe4be 210 }