wallbotble.cpp

00001 /* JKsoft Wallbot BLE Library
00002  *
00003  * wallbotble.cpp
00004  *
00005  * Copyright (c) 2010-2014 jksoft
00006  *
00007  * Permission is hereby granted, free of charge, to any person obtaining a copy
00008  * of this software and associated documentation files (the "Software"), to deal
00009  * in the Software without restriction, including without limitation the rights
00010  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00011  * copies of the Software, and to permit persons to whom the Software is
00012  * furnished to do so, subject to the following conditions:
00013  *
00014  * The above copyright notice and this permission notice shall be included in
00015  * all copies or substantial portions of the Software.
00016  *
00017  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00018  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00019  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00020  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00021  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00022  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00023  * THE SOFTWARE.
00024  */
00025 
00026 #include "mbed.h"
00027 #include "wallbotble.h"
00028 
00029 
00030 wallbotble::wallbotble() :      _right(P0_28,P0_30,P0_0) , _left(P0_29,P0_23,P0_24) ,
00031                                         _sw(P0_16,P0_17),
00032                                         _outlow(P0_20),_statusled(P0_18,P0_19),
00033                                         _f_sensor1(P0_2),_f_sensor2(P0_3),_f_sensor3(P0_4),_f_sensor4(P0_5)  {
00034     _right = 0.0;
00035     _left = 0.0;
00036     _outlow = 0;    
00037     _statusled = 0x3;
00038     _sensor_gain = SENSOR_VAL_GAIN_DEFAULT;
00039     _sw.mode(PullUp);
00040 }
00041 
00042 void wallbotble::left_motor (float speed) {
00043     _left = speed;
00044 }
00045 
00046 void wallbotble::right_motor (float speed) {
00047     _right = speed;
00048 }
00049 
00050 void wallbotble::forward (float speed) {
00051     _left = speed;
00052     _right = speed;
00053 }
00054 
00055 void wallbotble::backward (float speed) {
00056     _left = -speed;
00057     _right = -speed;
00058 }
00059 
00060 void wallbotble::left_turn (float speed) {
00061     _left = -speed;
00062     _right = speed;
00063 }
00064 
00065 void wallbotble::right_turn (float speed) {
00066     _left = speed;
00067     _right = -speed;
00068 }
00069 
00070 void wallbotble::stop (void) {
00071     _right = 0.0;
00072     _left = 0.0;
00073 }
00074 
00075 void wallbotble::set_led(int bit) {
00076     bit |= bit >> 2;
00077     _statusled = (bit ^ 0x03) & 0x03;
00078 }
00079 
00080 void wallbotble::set_led1(int bit) {
00081     if( bit )
00082     {
00083         bit = _statusled ^ 0x03;
00084         bit = bit | 0x1;
00085     }
00086     else
00087     {
00088         bit = _statusled ^ 0x03;
00089         bit = bit & 0x2;
00090     }
00091     set_led(bit);
00092 }
00093 
00094 void wallbotble::set_led2(int bit) {
00095     if( bit )
00096     {
00097         bit = _statusled ^ 0x03;
00098         bit = bit | 0x2;
00099     }
00100     else
00101     {
00102         bit = _statusled ^ 0x03;
00103         bit = bit & 0x1;
00104     }
00105     set_led(bit);
00106 }
00107 
00108 void wallbotble::set_f_sensor_gain(int gain) {
00109     _sensor_gain = gain;
00110 }
00111 
00112 void wallbotble::f_sensor_calibrate(void) {
00113 
00114     for(int i = 0 ; i < 4 ; i++)
00115     {
00116         _sensor_def[i] = 0;
00117     }
00118 
00119     for(int j = 0 ; j < 12 ; j++)
00120     {
00121         int in[4] = { _f_sensor1.read_u16(),
00122                      _f_sensor2.read_u16(),
00123                      _f_sensor3.read_u16(),
00124                      _f_sensor4.read_u16()
00125                    };
00126         set_led(1<<(j%2));
00127         for(int i = 0 ; i < 4 ; i++)
00128         {
00129             _sensor_def[i] += in[i];
00130         }
00131         wait_ms(200);
00132     }
00133     for(int i = 0 ; i < 4 ; i++)
00134     {
00135         _sensor_def[i] /= 12;
00136     }
00137     set_led(0);
00138 }
00139 
00140 int wallbotble::GetLinePosition(void) {
00141     int in[4] = { _f_sensor1.read_u16(),
00142                  _f_sensor2.read_u16(),
00143                  _f_sensor3.read_u16(),
00144                  _f_sensor4.read_u16()
00145                };
00146     int stat = 0;
00147     
00148     // printf("ad[0]:%05d ad[1]:%05d ad[2]:%05d ad[3]:%05d \n\r",in[0],in[1],in[2],in[3]);
00149     
00150     for(int i = 0 ; i < 4 ; i++)
00151     {       
00152         if( (in[i] > (_sensor_def[i] + _sensor_gain))
00153           ||(in[i] < (_sensor_def[i] - _sensor_gain)) )
00154         {
00155             stat |= 1 << i;
00156         }
00157     }
00158     
00159     return(stat);
00160 }
00161 
00162 
00163 int wallbotble::GetSw(void) {
00164     return(_sw ^ 0x3);
00165 }
00166 
00167