It is a library for controlling Wallbot

Dependents:   wallbot_test

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers wallbot.cpp Source File

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 }