It is a library for controlling Wallbot mini
wallbotmini.cpp
- Committer:
- jksoft
- Date:
- 2013-11-02
- Revision:
- 0:a11da93ea3f6
File content as of revision 0:a11da93ea3f6:
/* wallbot mini Library * * wallbotmini.cpp * * Copyright (c) 2010-2013 jksoft * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ #include "mbed.h" #include "wallbotmini.h" wallbotmini::wallbotmini() : _right(dp2,dp17,dp25) , _left(dp1,dp28,dp26) , _sw(dp24), _statusled(dp13,dp6,dp14,dp18), _sensor1(dp9),_sensor2(dp10),_sensor3(dp11) { _right = 0.0; _left = 0.0; _statusled = 0; } void wallbotmini::left_motor (float speed) { _left = speed; } void wallbotmini::right_motor (float speed) { _right = speed; } void wallbotmini::forward (float speed) { _left = speed; _right = speed; } void wallbotmini::backward (float speed) { _left = -1.0*speed; _right = -1.0*speed; } void wallbotmini::left (float speed) { _left = -1.0*speed; _right = speed; } void wallbotmini::right (float speed) { _left = speed; _right = -1.0*speed; } void wallbotmini::stop (void) { _right = 0.0; _left = 0.0; } void wallbotmini::set_led(int bit) { _statusled = bit; } void wallbotmini::sensor_calibrate(void) { for(int i = 0 ; i < 3 ; i++) { _sensor_def[i] = 0; } for(int j = 0 ; j < 12 ; j++) { int in[3] = { _sensor1.read_u16(), _sensor2.read_u16(), _sensor3.read_u16() }; set_led(1<<(j%4)); for(int i = 0 ; i < 3 ; i++) { _sensor_def[i] += in[i]; } wait_ms(200); } for(int i = 0 ; i < 3 ; i++) { _sensor_def[i] /= 12; } set_led(0); } int wallbotmini::GetLinePosition(void) { int in[3] = { _sensor1.read_u16(), _sensor2.read_u16(), _sensor3.read_u16() }; int stat = 0; // printf("ad[0]:%05d ad[1]:%05d ad[2]:%05d \n\r",in[0],in[1],in[2]); for(int i = 0 ; i < 3 ; i++) { if( (in[i] > (_sensor_def[i] + SENSOR_VAL_WIDTH)) ||(in[i] < (_sensor_def[i] - SENSOR_VAL_WIDTH)) ) { stat |= 1 << i; } } return(stat); } int wallbotmini::GetSw(void) { return(!_sw); }