It is a library for controlling Wallbot

Dependents:   wallbot_test

Committer:
jksoft
Date:
Sun Jul 28 08:20:17 2013 +0000
Revision:
0:f8571ffd252a
First

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jksoft 0:f8571ffd252a 1 /* wallbot Library
jksoft 0:f8571ffd252a 2 *
jksoft 0:f8571ffd252a 3 * wallbot.cpp
jksoft 0:f8571ffd252a 4 *
jksoft 0:f8571ffd252a 5 * Copyright (c) 2010-2013 jksoft
jksoft 0:f8571ffd252a 6 *
jksoft 0:f8571ffd252a 7 * Permission is hereby granted, free of charge, to any person obtaining a copy
jksoft 0:f8571ffd252a 8 * of this software and associated documentation files (the "Software"), to deal
jksoft 0:f8571ffd252a 9 * in the Software without restriction, including without limitation the rights
jksoft 0:f8571ffd252a 10 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
jksoft 0:f8571ffd252a 11 * copies of the Software, and to permit persons to whom the Software is
jksoft 0:f8571ffd252a 12 * furnished to do so, subject to the following conditions:
jksoft 0:f8571ffd252a 13 *
jksoft 0:f8571ffd252a 14 * The above copyright notice and this permission notice shall be included in
jksoft 0:f8571ffd252a 15 * all copies or substantial portions of the Software.
jksoft 0:f8571ffd252a 16 *
jksoft 0:f8571ffd252a 17 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
jksoft 0:f8571ffd252a 18 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
jksoft 0:f8571ffd252a 19 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
jksoft 0:f8571ffd252a 20 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
jksoft 0:f8571ffd252a 21 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
jksoft 0:f8571ffd252a 22 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
jksoft 0:f8571ffd252a 23 * THE SOFTWARE.
jksoft 0:f8571ffd252a 24 */
jksoft 0:f8571ffd252a 25
jksoft 0:f8571ffd252a 26 #include "mbed.h"
jksoft 0:f8571ffd252a 27 #include "wallbot.h"
jksoft 0:f8571ffd252a 28
jksoft 0:f8571ffd252a 29
jksoft 0:f8571ffd252a 30 wallbot::wallbot() : _right(p22,p14,p13) , _left(p21,p12,p11) , _ain(p15, p16, p17, p18), _left_sw(p29),_right_sw(p30) {
jksoft 0:f8571ffd252a 31 PHY_PowerDown();
jksoft 0:f8571ffd252a 32
jksoft 0:f8571ffd252a 33 _right = 0.0;
jksoft 0:f8571ffd252a 34 _left = 0.0;
jksoft 0:f8571ffd252a 35
jksoft 0:f8571ffd252a 36 _left_sw.mode(PullUp);
jksoft 0:f8571ffd252a 37 _right_sw.mode(PullUp);
jksoft 0:f8571ffd252a 38
jksoft 0:f8571ffd252a 39 _floorSensorThreshold = SENSOR_THRESHOLD;
jksoft 0:f8571ffd252a 40 }
jksoft 0:f8571ffd252a 41
jksoft 0:f8571ffd252a 42 void wallbot::left_motor (float speed) {
jksoft 0:f8571ffd252a 43 _left = speed;
jksoft 0:f8571ffd252a 44 }
jksoft 0:f8571ffd252a 45
jksoft 0:f8571ffd252a 46 void wallbot::right_motor (float speed) {
jksoft 0:f8571ffd252a 47 _right = speed;
jksoft 0:f8571ffd252a 48 }
jksoft 0:f8571ffd252a 49
jksoft 0:f8571ffd252a 50 void wallbot::forward (float speed) {
jksoft 0:f8571ffd252a 51 _left = speed;
jksoft 0:f8571ffd252a 52 _right = speed;
jksoft 0:f8571ffd252a 53 }
jksoft 0:f8571ffd252a 54
jksoft 0:f8571ffd252a 55 void wallbot::backward (float speed) {
jksoft 0:f8571ffd252a 56 _left = -1.0*speed;
jksoft 0:f8571ffd252a 57 _right = -1.0*speed;
jksoft 0:f8571ffd252a 58 }
jksoft 0:f8571ffd252a 59
jksoft 0:f8571ffd252a 60 void wallbot::left (float speed) {
jksoft 0:f8571ffd252a 61 _left = -1.0*speed;
jksoft 0:f8571ffd252a 62 _right = speed;
jksoft 0:f8571ffd252a 63 }
jksoft 0:f8571ffd252a 64
jksoft 0:f8571ffd252a 65 void wallbot::right (float speed) {
jksoft 0:f8571ffd252a 66 _left = speed;
jksoft 0:f8571ffd252a 67 _right = -1.0*speed;
jksoft 0:f8571ffd252a 68 }
jksoft 0:f8571ffd252a 69
jksoft 0:f8571ffd252a 70 void wallbot::stop (void) {
jksoft 0:f8571ffd252a 71 _right = 0.0;
jksoft 0:f8571ffd252a 72 _left = 0.0;
jksoft 0:f8571ffd252a 73 }
jksoft 0:f8571ffd252a 74
jksoft 0:f8571ffd252a 75
jksoft 0:f8571ffd252a 76 float wallbot::GetLinePosition(void) {
jksoft 0:f8571ffd252a 77 float ret = 0.0;
jksoft 0:f8571ffd252a 78 int bit = 0;
jksoft 0:f8571ffd252a 79 int value[4];
jksoft 0:f8571ffd252a 80
jksoft 0:f8571ffd252a 81 value[0] = _ain.read_u16(p15);
jksoft 0:f8571ffd252a 82 value[1] = _ain.read_u16(p16);
jksoft 0:f8571ffd252a 83 value[2] = _ain.read_u16(p17);
jksoft 0:f8571ffd252a 84 value[3] = _ain.read_u16(p18);
jksoft 0:f8571ffd252a 85
jksoft 0:f8571ffd252a 86 if( value[0] > _floorSensorThreshold ) bit |= 0x01;
jksoft 0:f8571ffd252a 87 if( value[1] > _floorSensorThreshold ) bit |= 0x02;
jksoft 0:f8571ffd252a 88 if( value[2] > _floorSensorThreshold ) bit |= 0x04;
jksoft 0:f8571ffd252a 89 if( value[3] > _floorSensorThreshold ) bit |= 0x08;
jksoft 0:f8571ffd252a 90
jksoft 0:f8571ffd252a 91 switch(bit)
jksoft 0:f8571ffd252a 92 {
jksoft 0:f8571ffd252a 93 case 0x01: ret = 1.0; break;
jksoft 0:f8571ffd252a 94 case 0x03: ret = 0.66; break;
jksoft 0:f8571ffd252a 95 case 0x02: ret = 0.33; break;
jksoft 0:f8571ffd252a 96 case 0x04: ret = -0.33; break;
jksoft 0:f8571ffd252a 97 case 0x0C: ret = -0.66; break;
jksoft 0:f8571ffd252a 98 case 0x08: ret = -1.0; break;
jksoft 0:f8571ffd252a 99 default: ret = 0.0; break;
jksoft 0:f8571ffd252a 100 }
jksoft 0:f8571ffd252a 101
jksoft 0:f8571ffd252a 102 return(ret);
jksoft 0:f8571ffd252a 103 }
jksoft 0:f8571ffd252a 104
jksoft 0:f8571ffd252a 105 void wallbot::GetLinePosition(int *bit) {
jksoft 0:f8571ffd252a 106 int value[4];
jksoft 0:f8571ffd252a 107
jksoft 0:f8571ffd252a 108 *bit = 0;
jksoft 0:f8571ffd252a 109
jksoft 0:f8571ffd252a 110 value[0] = _ain.read_u16(p15);
jksoft 0:f8571ffd252a 111 value[1] = _ain.read_u16(p16);
jksoft 0:f8571ffd252a 112 value[2] = _ain.read_u16(p17);
jksoft 0:f8571ffd252a 113 value[3] = _ain.read_u16(p18);
jksoft 0:f8571ffd252a 114
jksoft 0:f8571ffd252a 115 if( value[0] > _floorSensorThreshold ) *bit |= 0x01;
jksoft 0:f8571ffd252a 116 if( value[1] > _floorSensorThreshold ) *bit |= 0x02;
jksoft 0:f8571ffd252a 117 if( value[2] > _floorSensorThreshold ) *bit |= 0x04;
jksoft 0:f8571ffd252a 118 if( value[3] > _floorSensorThreshold ) *bit |= 0x08;
jksoft 0:f8571ffd252a 119 }
jksoft 0:f8571ffd252a 120
jksoft 0:f8571ffd252a 121 int wallbot::GetLeftSw(void) {
jksoft 0:f8571ffd252a 122 return(!_left_sw);
jksoft 0:f8571ffd252a 123 }
jksoft 0:f8571ffd252a 124
jksoft 0:f8571ffd252a 125 int wallbot::GetRightSw(void) {
jksoft 0:f8571ffd252a 126 return(!_right_sw);
jksoft 0:f8571ffd252a 127 }