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 /*
00121 * @param    speed_left, speed_right: 0-100.
00122 * @return   none.
00123 */
00124 void move_down(uint8_t speed_left, uint8_t speed_right)
00125 {
00126     DIR_L = 1;
00127     g_pwm_left = speed_left;
00128     DIR_R = 1;
00129     g_pwm_right = speed_right;
00130     g_sts_car = DOWN;
00131 }
00132 
00133 //////////////////////////////////////////////////
00134 //////////////////////////////////////////////////
00135 /*
00136 * @param    none.
00137 * @return   Distance (cm), 1-80 cm.
00138 */
00139 uint16_t sensor_ir(void)
00140 {
00141     uint16_t    adc;
00142     uint16_t    cm;
00143     uint16_t    sensor = 0, i;
00144 
00145     for(i=0; i<NOISE; i++) {
00146         adc = analog_ir.read_u16();
00147         adc = 750-adc;
00148         if      (adc > 60000) cm = 1;
00149         else if (adc > 600) cm = 0;
00150         else if (adc > 550) cm = adc/8;
00151         else if (adc > 500) cm = adc/10;
00152         else if (adc > 450) cm = adc/12;
00153         else if (adc > 400) cm = adc/14;
00154         else if (adc > 350) cm = adc/16;
00155         else if (adc > 300) cm = adc/18;
00156         else if (adc > 200) cm = adc/16;
00157         else if (adc > 200) cm = adc/14;
00158         else if (adc > 150) cm = adc/12;
00159         else if (adc > 100) cm = adc/10;
00160         else if (adc >  60) cm = adc/9;
00161         else if (adc >  30) cm = adc/8;
00162         else if (adc >   0) cm = adc/7;
00163 
00164         wait(0.001);
00165         sensor = sensor + cm;
00166         if(cm == 0) break;
00167         cm = sensor/NOISE;
00168     }
00169 
00170 #ifdef DEBUG
00171     pc.printf("\r\n %d adc, %d cm", adc, cm);
00172 #endif
00173     return cm;
00174 }
00175 
00176 //////////////////////////////////////////////////
00177 /*
00178 * @param    none.
00179 * @return   ON or OFF.
00180 */
00181 uint8_t sensor_ir2(void)
00182 {
00183     uint16_t i, sensor = 0;
00184 
00185     for(i=0; i<NOISE; i++) {
00186         wait(0.001);
00187         sensor = sensor + SEN_IR;
00188     }
00189     if(sensor > NOISE/2)   return OFF;
00190     else                   return ON;
00191 }
00192 
00193 //////////////////////////////////////////////////
00194 /*
00195 * @param    none.
00196 * @return   ON or OFF.
00197 */
00198 uint8_t sensor_up_left(void)
00199 {
00200     uint16_t i, sensor = 0;
00201 
00202     for(i=0; i<NOISE; i++) {
00203         wait(0.001);
00204         sensor = sensor + SEN_UP_L;
00205     }
00206     if(sensor > NOISE/2)   return OFF;
00207     else                   return ON;
00208 }
00209 //////////////////////////////////////////////////
00210 /*
00211 * @param    none.
00212 * @return   ON or OFF.
00213 */
00214 uint8_t sensor_up_right(void)
00215 {
00216     uint16_t i, sensor = 0;
00217 
00218     for(i=0; i<NOISE; i++) {
00219         wait(0.001);
00220         sensor = sensor + SEN_UP_R;
00221     }
00222     if(sensor > NOISE/2)   return OFF;
00223     else                   return ON;
00224 }
00225 //////////////////////////////////////////////////
00226 /*
00227 * @param    none.
00228 * @return   ON or OFF.
00229 */
00230 uint8_t sensor_down(void)
00231 {
00232     uint16_t i, sensor = 0;
00233 
00234     for(i=0; i<NOISE; i++) {
00235         wait(0.001);
00236         sensor = sensor + SEN_DOWN;
00237     }
00238     if(sensor > NOISE/2)   return OFF;
00239     else                   return ON;
00240 }
00241 //////////////////////////////////////////////////////////////////////////////////////////////////
00242 //////////////////////////////////////////////////////////////////////////////////////////////////
00243 //////////////////////////////////////////////////////////////////////////////////////////////////
00244 void periodicCallback(void)
00245 {
00246     if(g_time_pwm < g_pwm_left)    PWM_L = 1; else PWM_L = 0;
00247     if(g_time_pwm < g_pwm_right)   PWM_R = 1; else PWM_R = 0;
00248     g_time_pwm++; if(g_time_pwm >= 100) {g_time_pwm = 0;}
00249 
00250     g_time_led++; if(g_time_led >= 1000) {g_time_led = 0; led1 = !led1;}   //DIR_L = !DIR_L; DIR_R = !DIR_R;
00251 
00252 }
00253 
00254 //////////////////////////////////////////////////////////////////////////////////////////////////
00255 //////////////////////////////////////////////////////////////////////////////////////////////////
00256 //////////////////////////////////////////////////////////////////////////////////////////////////
00257 int main(void)
00258 {
00259     wait(0.1);
00260 //Init Hardware
00261     SEN_UP_L.mode(PullUp);
00262     SEN_UP_R.mode(PullUp);
00263     SEN_DOWN.mode(PullUp);
00264     SEN_IR.mode(PullUp);
00265     led1 = 1; DIR_L = 0; DIR_R = 0; PWM_L = 0;  PWM_R = 0;
00266 #ifdef DEBUG
00267     pc.baud(9600);
00268     pc.printf("\n\r# Sumo Car\n\r");
00269 #endif
00270 //Init interupt Timer
00271     ticker.attach(periodicCallback, 0.00015);
00272 //    wait(0.5);
00273 
00274 //#ifdef DEBUG    //sensor
00275 //    while (true) {
00276 //        rotator_right(20);
00277 //        wait(0.5);
00278 //        sensor_ir();
00279 //    }
00280 //#endif
00281     move_up(100, 100);
00282     wait(0.7);
00283     uint16_t tick = 0;
00284     bool right = true;
00285     while (true) {
00286         uint8_t sen_in = sensor_ir();
00287         if( (sen_in > 0) && (sen_in < 40) )
00288 //        uint8_t sen_in = sensor_ir2();
00289 //        if(sen_in == ON)
00290         {
00291             right = !right;
00292             move_up(100, 100);
00293             //move_up(30, 30);
00294             while( (sensor_up_left() == ON) && (sensor_up_right() == ON) );
00295             move_down(70, 70);
00296             wait(0.1);
00297             //move_down(0, 0);
00298             //while(1);
00299         } else {
00300             while((sensor_up_left() == OFF) && (sensor_up_right() == OFF)) {
00301                 move_down(100, 100);
00302             }
00303             if ((sensor_up_left() == OFF) && (sensor_up_right() == ON) && (sensor_down() == OFF)) {
00304                 move_up(40,100);
00305                 wait(0.5);
00306             } else if ((sensor_up_left() == ON) && (sensor_up_right() == OFF) && (sensor_down() == OFF)) {
00307                 move_up(100,40);
00308                 wait(0.5);
00309             } else if ((sensor_up_left() == ON) && (sensor_up_right() == ON) && (sensor_down() == OFF)) {
00310                 move_up(100,100);
00311                 wait(0.5);
00312             } else if ((sensor_up_left() == ON) && (sensor_up_right() == OFF)) {
00313                 rotator_left(50);
00314                 wait(0.4);
00315                 move_up(70,40);
00316                 wait(0.5);
00317             } else if ((sensor_up_left() == OFF) && (sensor_up_right() == ON)) {
00318                 rotator_right(50);
00319                 wait(0.5);
00320                 move_up(40,70);
00321                 wait(0.5);
00322             } else if ((sensor_up_left() == ON) && (sensor_up_right() == ON) && (sensor_down() == ON)) {
00323                 if (right) {
00324                     rotator_right(40);
00325                 } else {
00326                     rotator_left(40);    
00327                 }
00328                 //tick++;
00329 //                if (tick % 20 == 0) {
00330 //                    right = !right;
00331 //                }
00332             }
00333         }
00334     }
00335 }
00336