main.cpp

00001 /* mbed Microcontroller Library
00002  * Copyright (c) 2006-2015 ARM Limited
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #include "mbed.h"
00018 //#include "ble/BLE.h"
00019 //#include "ble/services/iBeacon.h"
00020 //#include "ble/services/UARTService.h"
00021 
00022 DigitalOut  led1(LED1, 1);
00023 DigitalOut  DIR_L(p25);
00024 DigitalOut  PWM_L(p23);
00025 DigitalOut  DIR_R(p28);
00026 DigitalOut  PWM_R(p24);
00027 
00028 //// Sensor
00029 #define NOISE 5
00030 //#define DEBUG
00031 
00032 #ifdef DEBUG
00033 Serial      pc(p10, p11);
00034 #endif
00035 Ticker      ticker;
00036 AnalogIn    analog_ir(p1);
00037 DigitalIn   SEN_UP_R(p2);
00038 DigitalIn   SEN_UP_L(p3);
00039 DigitalIn   SEN_DOWN(p4);
00040 DigitalIn   SEN_IR(p6);
00041 
00042 #define ON      0
00043 #define OFF     1
00044 #define VREF    3300.0  //mV
00045 
00046 //// Motor
00047 #define STOP    0
00048 #define UP      1
00049 #define DOWN    2
00050 #define LEFT    3
00051 #define RIGHT   4
00052 uint8_t     g_time_pwm = 0,     g_pwm_left = 0,     g_pwm_right = 0;
00053 uint16_t    g_time_led = 0;
00054 uint8_t     g_sts_car = STOP;
00055 
00056 
00057 
00058 //////////////////////////////////////////////////////////////////////////////////////////////////
00059 //////////////////////////////////////////////////////////////////////////////////////////////////
00060 //////////////////////////////////////////////////////////////////////////////////////////////////
00061 /*
00062  * @param    dir: UP or DOWN; speed: 0-100.
00063  * @return   none.
00064  */
00065 void motor_left(uint8_t dir, uint8_t speed)
00066 {
00067     DIR_R = dir;
00068     g_pwm_left = speed;
00069 }
00070 //////////////////////////////////////////////////
00071 /*
00072  * @param    dir: UP or DOWN; speed: 0-100.
00073  * @return   none.
00074  */
00075 void motor_right(uint8_t dir, uint8_t speed)
00076 {
00077     DIR_R = dir;
00078     g_pwm_right = speed;
00079 }
00080 //////////////////////////////////////////////////
00081 /*
00082  * @param    speed: 0-100.
00083  * @return   none.
00084  */
00085 void rotator_left(uint8_t speed)
00086 {
00087     DIR_L = 0;
00088     g_pwm_left = speed;
00089     DIR_R = 1;
00090     g_pwm_right = speed;
00091     g_sts_car = LEFT;
00092 }
00093 //////////////////////////////////////////////////
00094 /*
00095  * @param    speed: 0-100.
00096  * @return   none.
00097  */
00098 void rotator_right(uint8_t speed)
00099 {
00100     DIR_L = 1;
00101     g_pwm_left = speed;
00102     DIR_R = 0;
00103     g_pwm_right = speed;
00104     g_sts_car = RIGHT;
00105 }
00106 //////////////////////////////////////////////////
00107 /*
00108  * @param    speed_left, speed_right: 0-100.
00109  * @return   none.
00110  */
00111 void move_up(uint8_t speed_left, uint8_t speed_right)
00112 {
00113     DIR_L = 0;
00114     g_pwm_left = speed_left;
00115     DIR_R = 0;
00116     g_pwm_right = speed_right;
00117     g_sts_car = UP;
00118 }
00119 
00120 void move_around(uint8_t speed_left, uint8_t speed_right)
00121 {
00122     DIR_L = 0;
00123     g_pwm_left = speed_left;
00124     DIR_R = 0;
00125     g_pwm_right = speed_right;
00126     g_sts_car = UP;
00127 }
00128 
00129 //////////////////////////////////////////////////
00130 /*
00131  * @param    speed_left, speed_right: 0-100.
00132  * @return   none.
00133  */
00134 void move_down(uint8_t speed_left, uint8_t speed_right)
00135 {
00136     DIR_L = 1;
00137     g_pwm_left = speed_left;
00138     DIR_R = 1;
00139     g_pwm_right = speed_right;
00140     g_sts_car = DOWN;
00141 }
00142 
00143 void move_left_down (uint8_t speed_left, uint8_t speed_right)
00144 {
00145     DIR_L = 1;
00146     g_pwm_left = speed_left;
00147     DIR_R = 0;
00148     g_pwm_right = speed_right;
00149     g_sts_car = DOWN;
00150 }
00151 
00152 void move_right_down (uint8_t speed_left, uint8_t speed_right)
00153 {
00154     DIR_L = 0;
00155     g_pwm_left = speed_left;
00156     DIR_R = 1;
00157     g_pwm_right = speed_right;
00158     g_sts_car = DOWN;
00159 }
00160 
00161 //////////////////////////////////////////////////
00162 //////////////////////////////////////////////////
00163 /*
00164  * @param    none.
00165  * @return   Distance (cm), 1-80 cm.
00166  */
00167 uint16_t sensor_ir(void)
00168 {
00169     uint16_t    adc;
00170     uint16_t    cm;
00171     uint16_t    sensor = 0, i;
00172     
00173     for(i=0; i<NOISE; i++) {
00174         adc = analog_ir.read_u16();
00175         adc = 750-adc;
00176         if      (adc > 60000) cm = 1;
00177         else if (adc > 600) cm = 0;
00178         else if (adc > 550) cm = adc/8;
00179         else if (adc > 500) cm = adc/10;
00180         else if (adc > 450) cm = adc/12;
00181         else if (adc > 400) cm = adc/14;
00182         else if (adc > 350) cm = adc/16;
00183         else if (adc > 300) cm = adc/18;
00184         else if (adc > 200) cm = adc/16;
00185         else if (adc > 200) cm = adc/14;
00186         else if (adc > 150) cm = adc/12;
00187         else if (adc > 100) cm = adc/10;
00188         else if (adc >  60) cm = adc/9;
00189         else if (adc >  30) cm = adc/8;
00190         else if (adc >   0) cm = adc/7;
00191         
00192         //wait(0.001);
00193         sensor = sensor + cm;
00194         if(cm == 0) break;
00195         cm = sensor/NOISE;
00196     }
00197     
00198 #ifdef DEBUG
00199     pc.printf("\r\n %d adc, %d cm", adc, cm);
00200 #endif
00201     return cm;
00202 }
00203 
00204 //////////////////////////////////////////////////
00205 /*
00206  * @param    none.
00207  * @return   ON or OFF.
00208  */
00209 uint8_t sensor_ir2(void)
00210 {
00211     uint16_t i, sensor = 0;
00212     
00213     for(i=0; i<NOISE; i++) {
00214         //wait(0.001);
00215         sensor = sensor + SEN_IR;
00216     }
00217     if(sensor > NOISE/2)   return OFF;
00218     else                   return ON;
00219 }
00220 
00221 //////////////////////////////////////////////////
00222 /*
00223  * @param    none.
00224  * @return   ON or OFF.
00225  */
00226 uint8_t sensor_up_left(void)
00227 {
00228     uint16_t i, sensor = 0;
00229     
00230     for(i=0; i<NOISE; i++) {
00231         //wait(0.001);
00232         sensor = sensor + SEN_UP_L;
00233     }
00234     if(sensor > NOISE/2)   return OFF;
00235     else                   return ON;
00236 }
00237 //////////////////////////////////////////////////
00238 /*
00239  * @param    none.
00240  * @return   ON or OFF.
00241  */
00242 uint8_t sensor_up_right(void)
00243 {
00244     uint16_t i, sensor = 0;
00245     
00246     for(i=0; i<NOISE; i++) {
00247         //wait(0.001);
00248         sensor = sensor + SEN_UP_R;
00249     }
00250     if(sensor > NOISE/2)   return OFF;
00251     else                   return ON;
00252 }
00253 //////////////////////////////////////////////////
00254 /*
00255  * @param    none.
00256  * @return   ON or OFF.
00257  */
00258 uint8_t sensor_down(void)
00259 {
00260     uint16_t i, sensor = 0;
00261     
00262     for(i=0; i<NOISE; i++) {
00263         //wait(0.001);
00264         sensor = sensor + SEN_DOWN;
00265     }
00266     if(sensor > NOISE/2)   return OFF;
00267     else                   return ON;
00268 }
00269 //////////////////////////////////////////////////////////////////////////////////////////////////
00270 //////////////////////////////////////////////////////////////////////////////////////////////////
00271 //////////////////////////////////////////////////////////////////////////////////////////////////
00272 void periodicCallback(void)
00273 {
00274     if(g_time_pwm < g_pwm_left)    PWM_L = 1; else PWM_L = 0;
00275     if(g_time_pwm < g_pwm_right)   PWM_R = 1; else PWM_R = 0;
00276     g_time_pwm++; if(g_time_pwm >= 100) {g_time_pwm = 0;}
00277     
00278     g_time_led++; if(g_time_led >= 1000) {g_time_led = 0; led1 = !led1;}   //DIR_L = !DIR_L; DIR_R = !DIR_R;
00279     
00280 }
00281 
00282 //////////////////////////////////////////////////////////////////////////////////////////////////
00283 //////////////////////////////////////////////////////////////////////////////////////////////////
00284 //////////////////////////////////////////////////////////////////////////////////////////////////
00285 
00286 bool randomBool() {
00287     return rand() % 2 == 1;
00288 }
00289 
00290 bool isInDanger(void){
00291     return ((sensor_up_left() == OFF) || (sensor_up_right() == OFF) || (sensor_down() == OFF));
00292 }
00293 
00294 int main(void)
00295 {
00296     //wait(0.1);
00297     //Init Hardware
00298     SEN_UP_L.mode(PullUp);
00299     SEN_UP_R.mode(PullUp);
00300     SEN_DOWN.mode(PullUp);
00301     SEN_IR.mode(PullUp);
00302     led1 = 1; DIR_L = 0; DIR_R = 0; PWM_L = 0;  PWM_R = 0;
00303     //Init interupt Timer
00304     ticker.attach(periodicCallback, 0.00015);
00305     //wait(0.5);
00306     uint8_t sen_in = sensor_ir();
00307     while (true) {
00308         sen_in = sensor_ir();
00309         if(isInDanger()){
00310             g_sts_car = STOP;
00311              //if (sensor_down() == OFF && sensor_up_left() == OFF){
00312 //                rotator_right(30);
00313 //                while(sensor_down() == OFF && sensor_up_left() == OFF);
00314 //                 uint8_t timer = 0;
00315 //                 while(!isInDanger() && timer < 50){
00316 //                     //wait(0.01);
00317 //                     timer++;
00318 //                 }
00319 //                 move_up(70,70);
00320 //                 while(!isInDanger() && timer < 100){
00321 //                     //wait(0.01);
00322 //                     timer++;
00323 //                 }
00324 //                g_sts_car = STOP;     
00325 //             }
00326 //             else if(sensor_down() == OFF && sensor_up_right() == OFF){
00327 //                rotator_left(30);
00328 //                while(sensor_down() == OFF && sensor_up_right() == OFF);
00329 //                 uint8_t timer = 0;
00330 //                 while(!isInDanger() && timer < 50){
00331 //                     //wait(0.01);
00332 //                     timer = timer + 1;
00333 //                 }
00334 //                 move_up(70,70);
00335 //                 while(!isInDanger() && timer < 100){
00336 //                     //wait(0.01);
00337 //                     timer = timer + 1;
00338 //                 }
00339 //                g_sts_car = STOP;  
00340 //             }
00341 //             else
00342               if(sensor_up_left() == OFF && sensor_up_right() == OFF){
00343                 move_down(100,100);
00344                 while(sensor_up_left() == OFF && sensor_up_right() == OFF);
00345                  uint8_t timer = 0;
00346                  move_down(30,100);
00347                   while((isInDanger() == false) && timer < 150){
00348                      wait(0.001);
00349                      timer++;
00350                   }
00351                  g_sts_car = STOP;
00352              }
00353              else if(sensor_up_left() == OFF){
00354                  move_down(30,100);
00355                  while(sensor_up_left() == OFF);
00356                   uint8_t timer = 0;
00357                   while((isInDanger() == false) && timer < 150){
00358                      wait(0.001);
00359                      timer++;
00360                   }
00361                  // move_up(70,70);
00362 //                  while((isInDanger() == false) && timer < 40){
00363 //                     timer++;
00364 //                 } 
00365                 g_sts_car = STOP;    
00366              }
00367              else if(sensor_up_right() == OFF){
00368                  move_down(100,30);
00369                   while(sensor_up_right() == OFF);
00370                   uint8_t timer = 0;
00371                   while((isInDanger() == false) && timer < 150){
00372                      wait(0.001);
00373                      timer++;
00374                   }
00375 //                  move_up(70,70);
00376 //                  while((isInDanger() == false) && timer < 40){
00377 //                     timer++;
00378 //                 }
00379                 g_sts_car = STOP;  
00380              }
00381              else if (sensor_down() == OFF) {
00382                  move_up(100,100);
00383                  while(sensor_down() == OFF);
00384                  g_sts_car = STOP;
00385              }
00386         }
00387         else{
00388             g_sts_car = STOP;
00389             if(sen_in > 0 && sen_in <30){
00390                 move_up(100,100);
00391                 while((isInDanger() == false) && sensor_ir() > 0 && sensor_ir() <30);
00392                 g_sts_car = STOP;
00393             }
00394             else {
00395                 rotator_right(20);
00396             }
00397         }
00398         
00399     }
00400 }