Junichi Katsu / wallbotmini

It is a library for controlling Wallbot mini

Dependents:   wallbotmini_test

wallbotmini.cpp@0:a11da93ea3f6, 2013-11-02 (annotated)

Committer:
jksoft
Date:
Sat Nov 02 01:04:00 2013 +0000
Revision:
0:a11da93ea3f6
Rev1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jksoft 0:a11da93ea3f6 1 /* wallbot mini Library
jksoft 0:a11da93ea3f6 2 *
jksoft 0:a11da93ea3f6 3 * wallbotmini.cpp
jksoft 0:a11da93ea3f6 4 *
jksoft 0:a11da93ea3f6 5 * Copyright (c) 2010-2013 jksoft
jksoft 0:a11da93ea3f6 6 *
jksoft 0:a11da93ea3f6 7 * Permission is hereby granted, free of charge, to any person obtaining a copy
jksoft 0:a11da93ea3f6 8 * of this software and associated documentation files (the "Software"), to deal
jksoft 0:a11da93ea3f6 9 * in the Software without restriction, including without limitation the rights
jksoft 0:a11da93ea3f6 10 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
jksoft 0:a11da93ea3f6 11 * copies of the Software, and to permit persons to whom the Software is
jksoft 0:a11da93ea3f6 12 * furnished to do so, subject to the following conditions:
jksoft 0:a11da93ea3f6 13 *
jksoft 0:a11da93ea3f6 14 * The above copyright notice and this permission notice shall be included in
jksoft 0:a11da93ea3f6 15 * all copies or substantial portions of the Software.
jksoft 0:a11da93ea3f6 16 *
jksoft 0:a11da93ea3f6 17 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
jksoft 0:a11da93ea3f6 18 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
jksoft 0:a11da93ea3f6 19 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
jksoft 0:a11da93ea3f6 20 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
jksoft 0:a11da93ea3f6 21 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
jksoft 0:a11da93ea3f6 22 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
jksoft 0:a11da93ea3f6 23 * THE SOFTWARE.
jksoft 0:a11da93ea3f6 24 */
jksoft 0:a11da93ea3f6 25
jksoft 0:a11da93ea3f6 26 #include "mbed.h"
jksoft 0:a11da93ea3f6 27 #include "wallbotmini.h"
jksoft 0:a11da93ea3f6 28
jksoft 0:a11da93ea3f6 29
jksoft 0:a11da93ea3f6 30 wallbotmini::wallbotmini() : _right(dp2,dp17,dp25) , _left(dp1,dp28,dp26) , _sw(dp24),
jksoft 0:a11da93ea3f6 31 _statusled(dp13,dp6,dp14,dp18), _sensor1(dp9),_sensor2(dp10),_sensor3(dp11) {
jksoft 0:a11da93ea3f6 32 _right = 0.0;
jksoft 0:a11da93ea3f6 33 _left = 0.0;
jksoft 0:a11da93ea3f6 34
jksoft 0:a11da93ea3f6 35 _statusled = 0;
jksoft 0:a11da93ea3f6 36 }
jksoft 0:a11da93ea3f6 37
jksoft 0:a11da93ea3f6 38 void wallbotmini::left_motor (float speed) {
jksoft 0:a11da93ea3f6 39 _left = speed;
jksoft 0:a11da93ea3f6 40 }
jksoft 0:a11da93ea3f6 41
jksoft 0:a11da93ea3f6 42 void wallbotmini::right_motor (float speed) {
jksoft 0:a11da93ea3f6 43 _right = speed;
jksoft 0:a11da93ea3f6 44 }
jksoft 0:a11da93ea3f6 45
jksoft 0:a11da93ea3f6 46 void wallbotmini::forward (float speed) {
jksoft 0:a11da93ea3f6 47 _left = speed;
jksoft 0:a11da93ea3f6 48 _right = speed;
jksoft 0:a11da93ea3f6 49 }
jksoft 0:a11da93ea3f6 50
jksoft 0:a11da93ea3f6 51 void wallbotmini::backward (float speed) {
jksoft 0:a11da93ea3f6 52 _left = -1.0*speed;
jksoft 0:a11da93ea3f6 53 _right = -1.0*speed;
jksoft 0:a11da93ea3f6 54 }
jksoft 0:a11da93ea3f6 55
jksoft 0:a11da93ea3f6 56 void wallbotmini::left (float speed) {
jksoft 0:a11da93ea3f6 57 _left = -1.0*speed;
jksoft 0:a11da93ea3f6 58 _right = speed;
jksoft 0:a11da93ea3f6 59 }
jksoft 0:a11da93ea3f6 60
jksoft 0:a11da93ea3f6 61 void wallbotmini::right (float speed) {
jksoft 0:a11da93ea3f6 62 _left = speed;
jksoft 0:a11da93ea3f6 63 _right = -1.0*speed;
jksoft 0:a11da93ea3f6 64 }
jksoft 0:a11da93ea3f6 65
jksoft 0:a11da93ea3f6 66 void wallbotmini::stop (void) {
jksoft 0:a11da93ea3f6 67 _right = 0.0;
jksoft 0:a11da93ea3f6 68 _left = 0.0;
jksoft 0:a11da93ea3f6 69 }
jksoft 0:a11da93ea3f6 70
jksoft 0:a11da93ea3f6 71 void wallbotmini::set_led(int bit) {
jksoft 0:a11da93ea3f6 72 _statusled = bit;
jksoft 0:a11da93ea3f6 73 }
jksoft 0:a11da93ea3f6 74
jksoft 0:a11da93ea3f6 75 void wallbotmini::sensor_calibrate(void) {
jksoft 0:a11da93ea3f6 76
jksoft 0:a11da93ea3f6 77
jksoft 0:a11da93ea3f6 78 for(int i = 0 ; i < 3 ; i++)
jksoft 0:a11da93ea3f6 79 {
jksoft 0:a11da93ea3f6 80 _sensor_def[i] = 0;
jksoft 0:a11da93ea3f6 81 }
jksoft 0:a11da93ea3f6 82
jksoft 0:a11da93ea3f6 83 for(int j = 0 ; j < 12 ; j++)
jksoft 0:a11da93ea3f6 84 {
jksoft 0:a11da93ea3f6 85 int in[3] = { _sensor1.read_u16(),
jksoft 0:a11da93ea3f6 86 _sensor2.read_u16(),
jksoft 0:a11da93ea3f6 87 _sensor3.read_u16()
jksoft 0:a11da93ea3f6 88 };
jksoft 0:a11da93ea3f6 89 set_led(1<<(j%4));
jksoft 0:a11da93ea3f6 90 for(int i = 0 ; i < 3 ; i++)
jksoft 0:a11da93ea3f6 91 {
jksoft 0:a11da93ea3f6 92 _sensor_def[i] += in[i];
jksoft 0:a11da93ea3f6 93 }
jksoft 0:a11da93ea3f6 94 wait_ms(200);
jksoft 0:a11da93ea3f6 95 }
jksoft 0:a11da93ea3f6 96 for(int i = 0 ; i < 3 ; i++)
jksoft 0:a11da93ea3f6 97 {
jksoft 0:a11da93ea3f6 98 _sensor_def[i] /= 12;
jksoft 0:a11da93ea3f6 99 }
jksoft 0:a11da93ea3f6 100 set_led(0);
jksoft 0:a11da93ea3f6 101 }
jksoft 0:a11da93ea3f6 102
jksoft 0:a11da93ea3f6 103 int wallbotmini::GetLinePosition(void) {
jksoft 0:a11da93ea3f6 104 int in[3] = { _sensor1.read_u16(),
jksoft 0:a11da93ea3f6 105 _sensor2.read_u16(),
jksoft 0:a11da93ea3f6 106 _sensor3.read_u16()
jksoft 0:a11da93ea3f6 107 };
jksoft 0:a11da93ea3f6 108 int stat = 0;
jksoft 0:a11da93ea3f6 109
jksoft 0:a11da93ea3f6 110 // printf("ad[0]:%05d ad[1]:%05d ad[2]:%05d \n\r",in[0],in[1],in[2]);
jksoft 0:a11da93ea3f6 111
jksoft 0:a11da93ea3f6 112 for(int i = 0 ; i < 3 ; i++)
jksoft 0:a11da93ea3f6 113 {
jksoft 0:a11da93ea3f6 114 if( (in[i] > (_sensor_def[i] + SENSOR_VAL_WIDTH))
jksoft 0:a11da93ea3f6 115 ||(in[i] < (_sensor_def[i] - SENSOR_VAL_WIDTH)) )
jksoft 0:a11da93ea3f6 116 {
jksoft 0:a11da93ea3f6 117 stat |= 1 << i;
jksoft 0:a11da93ea3f6 118 }
jksoft 0:a11da93ea3f6 119 }
jksoft 0:a11da93ea3f6 120
jksoft 0:a11da93ea3f6 121 return(stat);
jksoft 0:a11da93ea3f6 122 }
jksoft 0:a11da93ea3f6 123
jksoft 0:a11da93ea3f6 124
jksoft 0:a11da93ea3f6 125 int wallbotmini::GetSw(void) {
jksoft 0:a11da93ea3f6 126 return(!_sw);
jksoft 0:a11da93ea3f6 127 }
jksoft 0:a11da93ea3f6 128
jksoft 0:a11da93ea3f6 129