It is a library for controlling Wallbot
Embed:
(wiki syntax)
Show/hide line numbers
wallbot.cpp
00001 /* wallbot Library 00002 * 00003 * wallbot.cpp 00004 * 00005 * Copyright (c) 2010-2013 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 "wallbot.h" 00028 00029 00030 wallbot::wallbot() : _right(p22,p14,p13) , _left(p21,p12,p11) , _ain(p15, p16, p17, p18), _left_sw(p29),_right_sw(p30) { 00031 PHY_PowerDown(); 00032 00033 _right = 0.0; 00034 _left = 0.0; 00035 00036 _left_sw.mode(PullUp); 00037 _right_sw.mode(PullUp); 00038 00039 _floorSensorThreshold = SENSOR_THRESHOLD; 00040 } 00041 00042 void wallbot::left_motor (float speed) { 00043 _left = speed; 00044 } 00045 00046 void wallbot::right_motor (float speed) { 00047 _right = speed; 00048 } 00049 00050 void wallbot::forward (float speed) { 00051 _left = speed; 00052 _right = speed; 00053 } 00054 00055 void wallbot::backward (float speed) { 00056 _left = -1.0*speed; 00057 _right = -1.0*speed; 00058 } 00059 00060 void wallbot::left (float speed) { 00061 _left = -1.0*speed; 00062 _right = speed; 00063 } 00064 00065 void wallbot::right (float speed) { 00066 _left = speed; 00067 _right = -1.0*speed; 00068 } 00069 00070 void wallbot::stop (void) { 00071 _right = 0.0; 00072 _left = 0.0; 00073 } 00074 00075 00076 float wallbot::GetLinePosition(void) { 00077 float ret = 0.0; 00078 int bit = 0; 00079 int value[4]; 00080 00081 value[0] = _ain.read_u16(p15); 00082 value[1] = _ain.read_u16(p16); 00083 value[2] = _ain.read_u16(p17); 00084 value[3] = _ain.read_u16(p18); 00085 00086 if( value[0] > _floorSensorThreshold ) bit |= 0x01; 00087 if( value[1] > _floorSensorThreshold ) bit |= 0x02; 00088 if( value[2] > _floorSensorThreshold ) bit |= 0x04; 00089 if( value[3] > _floorSensorThreshold ) bit |= 0x08; 00090 00091 switch(bit) 00092 { 00093 case 0x01: ret = 1.0; break; 00094 case 0x03: ret = 0.66; break; 00095 case 0x02: ret = 0.33; break; 00096 case 0x04: ret = -0.33; break; 00097 case 0x0C: ret = -0.66; break; 00098 case 0x08: ret = -1.0; break; 00099 default: ret = 0.0; break; 00100 } 00101 00102 return(ret); 00103 } 00104 00105 void wallbot::GetLinePosition(int *bit) { 00106 int value[4]; 00107 00108 *bit = 0; 00109 00110 value[0] = _ain.read_u16(p15); 00111 value[1] = _ain.read_u16(p16); 00112 value[2] = _ain.read_u16(p17); 00113 value[3] = _ain.read_u16(p18); 00114 00115 if( value[0] > _floorSensorThreshold ) *bit |= 0x01; 00116 if( value[1] > _floorSensorThreshold ) *bit |= 0x02; 00117 if( value[2] > _floorSensorThreshold ) *bit |= 0x04; 00118 if( value[3] > _floorSensorThreshold ) *bit |= 0x08; 00119 } 00120 00121 int wallbot::GetLeftSw(void) { 00122 return(!_left_sw); 00123 } 00124 00125 int wallbot::GetRightSw(void) { 00126 return(!_right_sw); 00127 }
Generated on Sun Jul 17 2022 09:53:56 by 1.7.2