Kang hokyoung / Mbed 2 deprecated 2014_Ensoul_Capstone

2014_Ensoul_Capstone

Dependencies:   TextLCD Ultrasonic mbed BufferedSoftSerial

main.cpp@1:2859bfed20b4, 2014-07-03 (annotated)

Committer:
leejong87
Date:
Thu Jul 03 05:24:13 2014 +0000
Revision:
1:2859bfed20b4
Parent:
0:d429f13fe4be
Child:
2:c9740fccf3a7
Developer

Who changed what in which revision?

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