Kazuhiro Ouchi / Mbed 2 deprecated m3pi_ledpsd

A program that connects a distance sensor to the m3pi and avoids obstacles.

Dependencies:   mbed m3pi

m3pi/m3pi.cpp@0:1c32a3ae67e1, 2011-11-23 (annotated)

Committer:
kanpapa
Date:
Wed Nov 23 14:15:24 2011 +0000
Revision:
0:1c32a3ae67e1
alpha version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kanpapa 0:1c32a3ae67e1 1 /* m3pi Library
kanpapa 0:1c32a3ae67e1 2 *
kanpapa 0:1c32a3ae67e1 3 * Copyright (c) 2007-2010 cstyles
kanpapa 0:1c32a3ae67e1 4 *
kanpapa 0:1c32a3ae67e1 5 * Permission is hereby granted, free of charge, to any person obtaining a copy
kanpapa 0:1c32a3ae67e1 6 * of this software and associated documentation files (the "Software"), to deal
kanpapa 0:1c32a3ae67e1 7 * in the Software without restriction, including without limitation the rights
kanpapa 0:1c32a3ae67e1 8 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
kanpapa 0:1c32a3ae67e1 9 * copies of the Software, and to permit persons to whom the Software is
kanpapa 0:1c32a3ae67e1 10 * furnished to do so, subject to the following conditions:
kanpapa 0:1c32a3ae67e1 11 *
kanpapa 0:1c32a3ae67e1 12 * The above copyright notice and this permission notice shall be included in
kanpapa 0:1c32a3ae67e1 13 * all copies or substantial portions of the Software.
kanpapa 0:1c32a3ae67e1 14 *
kanpapa 0:1c32a3ae67e1 15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
kanpapa 0:1c32a3ae67e1 16 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
kanpapa 0:1c32a3ae67e1 17 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
kanpapa 0:1c32a3ae67e1 18 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
kanpapa 0:1c32a3ae67e1 19 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
kanpapa 0:1c32a3ae67e1 20 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
kanpapa 0:1c32a3ae67e1 21 * THE SOFTWARE.
kanpapa 0:1c32a3ae67e1 22 */
kanpapa 0:1c32a3ae67e1 23
kanpapa 0:1c32a3ae67e1 24 #include "mbed.h"
kanpapa 0:1c32a3ae67e1 25 #include "m3pi.h"
kanpapa 0:1c32a3ae67e1 26
kanpapa 0:1c32a3ae67e1 27 m3pi::m3pi(PinName nrst, PinName tx, PinName rx) : Stream("m3pi"), _nrst(nrst), _ser(tx, rx) {
kanpapa 0:1c32a3ae67e1 28 _ser.baud(115200);
kanpapa 0:1c32a3ae67e1 29 reset();
kanpapa 0:1c32a3ae67e1 30 }
kanpapa 0:1c32a3ae67e1 31
kanpapa 0:1c32a3ae67e1 32 m3pi::m3pi() : Stream("m3pi"), _nrst(p23), _ser(p9, p10) {
kanpapa 0:1c32a3ae67e1 33 _ser.baud(115200);
kanpapa 0:1c32a3ae67e1 34 reset();
kanpapa 0:1c32a3ae67e1 35 }
kanpapa 0:1c32a3ae67e1 36
kanpapa 0:1c32a3ae67e1 37
kanpapa 0:1c32a3ae67e1 38 void m3pi::reset () {
kanpapa 0:1c32a3ae67e1 39 _nrst = 0;
kanpapa 0:1c32a3ae67e1 40 wait (0.01);
kanpapa 0:1c32a3ae67e1 41 _nrst = 1;
kanpapa 0:1c32a3ae67e1 42 wait (0.1);
kanpapa 0:1c32a3ae67e1 43 }
kanpapa 0:1c32a3ae67e1 44
kanpapa 0:1c32a3ae67e1 45 void m3pi::left_motor (float speed) {
kanpapa 0:1c32a3ae67e1 46 motor(0,speed);
kanpapa 0:1c32a3ae67e1 47 }
kanpapa 0:1c32a3ae67e1 48
kanpapa 0:1c32a3ae67e1 49 void m3pi::right_motor (float speed) {
kanpapa 0:1c32a3ae67e1 50 motor(1,speed);
kanpapa 0:1c32a3ae67e1 51 }
kanpapa 0:1c32a3ae67e1 52
kanpapa 0:1c32a3ae67e1 53 void m3pi::forward (float speed) {
kanpapa 0:1c32a3ae67e1 54 motor(0,speed);
kanpapa 0:1c32a3ae67e1 55 motor(1,speed);
kanpapa 0:1c32a3ae67e1 56 }
kanpapa 0:1c32a3ae67e1 57
kanpapa 0:1c32a3ae67e1 58 void m3pi::backward (float speed) {
kanpapa 0:1c32a3ae67e1 59 motor(0,-1.0*speed);
kanpapa 0:1c32a3ae67e1 60 motor(1,-1.0*speed);
kanpapa 0:1c32a3ae67e1 61 }
kanpapa 0:1c32a3ae67e1 62
kanpapa 0:1c32a3ae67e1 63 void m3pi::left (float speed) {
kanpapa 0:1c32a3ae67e1 64 motor(0,speed);
kanpapa 0:1c32a3ae67e1 65 motor(1,-1.0*speed);
kanpapa 0:1c32a3ae67e1 66 }
kanpapa 0:1c32a3ae67e1 67
kanpapa 0:1c32a3ae67e1 68 void m3pi::right (float speed) {
kanpapa 0:1c32a3ae67e1 69 motor(0,-1.0*speed);
kanpapa 0:1c32a3ae67e1 70 motor(1,speed);
kanpapa 0:1c32a3ae67e1 71 }
kanpapa 0:1c32a3ae67e1 72
kanpapa 0:1c32a3ae67e1 73 void m3pi::stop (void) {
kanpapa 0:1c32a3ae67e1 74 motor(0,0.0);
kanpapa 0:1c32a3ae67e1 75 motor(1,0.0);
kanpapa 0:1c32a3ae67e1 76 }
kanpapa 0:1c32a3ae67e1 77
kanpapa 0:1c32a3ae67e1 78 void m3pi::motor (int motor, float speed) {
kanpapa 0:1c32a3ae67e1 79 char opcode = 0x0;
kanpapa 0:1c32a3ae67e1 80 if (speed > 0.0) {
kanpapa 0:1c32a3ae67e1 81 if (motor==1)
kanpapa 0:1c32a3ae67e1 82 opcode = M1_FORWARD;
kanpapa 0:1c32a3ae67e1 83 else
kanpapa 0:1c32a3ae67e1 84 opcode = M2_FORWARD;
kanpapa 0:1c32a3ae67e1 85 } else {
kanpapa 0:1c32a3ae67e1 86 if (motor==1)
kanpapa 0:1c32a3ae67e1 87 opcode = M1_BACKWARD;
kanpapa 0:1c32a3ae67e1 88 else
kanpapa 0:1c32a3ae67e1 89 opcode = M2_BACKWARD;
kanpapa 0:1c32a3ae67e1 90 }
kanpapa 0:1c32a3ae67e1 91 unsigned char arg = 0x7f * abs(speed);
kanpapa 0:1c32a3ae67e1 92
kanpapa 0:1c32a3ae67e1 93 _ser.putc(opcode);
kanpapa 0:1c32a3ae67e1 94 _ser.putc(arg);
kanpapa 0:1c32a3ae67e1 95 }
kanpapa 0:1c32a3ae67e1 96
kanpapa 0:1c32a3ae67e1 97 float m3pi::battery() {
kanpapa 0:1c32a3ae67e1 98 _ser.putc(SEND_BATTERY_MILLIVOLTS);
kanpapa 0:1c32a3ae67e1 99 char lowbyte = _ser.getc();
kanpapa 0:1c32a3ae67e1 100 char hibyte = _ser.getc();
kanpapa 0:1c32a3ae67e1 101 float v = ((lowbyte + (hibyte << 8))/1000.0);
kanpapa 0:1c32a3ae67e1 102 return(v);
kanpapa 0:1c32a3ae67e1 103 }
kanpapa 0:1c32a3ae67e1 104
kanpapa 0:1c32a3ae67e1 105 float m3pi::line_position() {
kanpapa 0:1c32a3ae67e1 106 int pos = 0;
kanpapa 0:1c32a3ae67e1 107 _ser.putc(SEND_LINE_POSITION);
kanpapa 0:1c32a3ae67e1 108 pos = _ser.getc();
kanpapa 0:1c32a3ae67e1 109 pos += _ser.getc() << 8;
kanpapa 0:1c32a3ae67e1 110
kanpapa 0:1c32a3ae67e1 111 float fpos = ((float)pos - 2048.0)/2048.0;
kanpapa 0:1c32a3ae67e1 112 return(fpos);
kanpapa 0:1c32a3ae67e1 113 }
kanpapa 0:1c32a3ae67e1 114
kanpapa 0:1c32a3ae67e1 115 char m3pi::sensor_auto_calibrate() {
kanpapa 0:1c32a3ae67e1 116 _ser.putc(AUTO_CALIBRATE);
kanpapa 0:1c32a3ae67e1 117 return(_ser.getc());
kanpapa 0:1c32a3ae67e1 118 }
kanpapa 0:1c32a3ae67e1 119
kanpapa 0:1c32a3ae67e1 120
kanpapa 0:1c32a3ae67e1 121 void m3pi::calibrate(void) {
kanpapa 0:1c32a3ae67e1 122 _ser.putc(PI_CALIBRATE);
kanpapa 0:1c32a3ae67e1 123 }
kanpapa 0:1c32a3ae67e1 124
kanpapa 0:1c32a3ae67e1 125 void m3pi::reset_calibration() {
kanpapa 0:1c32a3ae67e1 126 _ser.putc(LINE_SENSORS_RESET_CALIBRATION);
kanpapa 0:1c32a3ae67e1 127 }
kanpapa 0:1c32a3ae67e1 128
kanpapa 0:1c32a3ae67e1 129 void m3pi::PID_start(int max_speed, int a, int b, int c, int d) {
kanpapa 0:1c32a3ae67e1 130 _ser.putc(max_speed);
kanpapa 0:1c32a3ae67e1 131 _ser.putc(a);
kanpapa 0:1c32a3ae67e1 132 _ser.putc(b);
kanpapa 0:1c32a3ae67e1 133 _ser.putc(c);
kanpapa 0:1c32a3ae67e1 134 _ser.putc(d);
kanpapa 0:1c32a3ae67e1 135 }
kanpapa 0:1c32a3ae67e1 136
kanpapa 0:1c32a3ae67e1 137 void m3pi::PID_stop() {
kanpapa 0:1c32a3ae67e1 138 _ser.putc(STOP_PID);
kanpapa 0:1c32a3ae67e1 139 }
kanpapa 0:1c32a3ae67e1 140
kanpapa 0:1c32a3ae67e1 141 float m3pi::pot_voltage(void) {
kanpapa 0:1c32a3ae67e1 142 int volt = 0;
kanpapa 0:1c32a3ae67e1 143 _ser.putc(SEND_TRIMPOT);
kanpapa 0:1c32a3ae67e1 144 volt = _ser.getc();
kanpapa 0:1c32a3ae67e1 145 volt += _ser.getc() << 8;
kanpapa 0:1c32a3ae67e1 146 return(volt);
kanpapa 0:1c32a3ae67e1 147 }
kanpapa 0:1c32a3ae67e1 148
kanpapa 0:1c32a3ae67e1 149
kanpapa 0:1c32a3ae67e1 150 void m3pi::leds(int val) {
kanpapa 0:1c32a3ae67e1 151
kanpapa 0:1c32a3ae67e1 152 BusOut _leds(p20,p19,p18,p17,p16,p15,p14,p13);
kanpapa 0:1c32a3ae67e1 153 _leds = val;
kanpapa 0:1c32a3ae67e1 154 }
kanpapa 0:1c32a3ae67e1 155
kanpapa 0:1c32a3ae67e1 156
kanpapa 0:1c32a3ae67e1 157 void m3pi::locate(int x, int y) {
kanpapa 0:1c32a3ae67e1 158 _ser.putc(DO_LCD_GOTO_XY);
kanpapa 0:1c32a3ae67e1 159 _ser.putc(x);
kanpapa 0:1c32a3ae67e1 160 _ser.putc(y);
kanpapa 0:1c32a3ae67e1 161 }
kanpapa 0:1c32a3ae67e1 162
kanpapa 0:1c32a3ae67e1 163 void m3pi::cls(void) {
kanpapa 0:1c32a3ae67e1 164 _ser.putc(DO_CLEAR);
kanpapa 0:1c32a3ae67e1 165 }
kanpapa 0:1c32a3ae67e1 166
kanpapa 0:1c32a3ae67e1 167 int m3pi::print (char* text, int length) {
kanpapa 0:1c32a3ae67e1 168 _ser.putc(DO_PRINT);
kanpapa 0:1c32a3ae67e1 169 _ser.putc(length);
kanpapa 0:1c32a3ae67e1 170 for (int i = 0 ; i < length ; i++) {
kanpapa 0:1c32a3ae67e1 171 _ser.putc(text[i]);
kanpapa 0:1c32a3ae67e1 172 }
kanpapa 0:1c32a3ae67e1 173 return(0);
kanpapa 0:1c32a3ae67e1 174 }
kanpapa 0:1c32a3ae67e1 175
kanpapa 0:1c32a3ae67e1 176 int m3pi::_putc (int c) {
kanpapa 0:1c32a3ae67e1 177 _ser.putc(DO_PRINT);
kanpapa 0:1c32a3ae67e1 178 _ser.putc(0x1);
kanpapa 0:1c32a3ae67e1 179 _ser.putc(c);
kanpapa 0:1c32a3ae67e1 180 wait (0.001);
kanpapa 0:1c32a3ae67e1 181 return(c);
kanpapa 0:1c32a3ae67e1 182 }
kanpapa 0:1c32a3ae67e1 183
kanpapa 0:1c32a3ae67e1 184 int m3pi::_getc (void) {
kanpapa 0:1c32a3ae67e1 185 char r = 0;
kanpapa 0:1c32a3ae67e1 186 return(r);
kanpapa 0:1c32a3ae67e1 187 }
kanpapa 0:1c32a3ae67e1 188
kanpapa 0:1c32a3ae67e1 189 int m3pi::putc (int c) {
kanpapa 0:1c32a3ae67e1 190 return(_ser.putc(c));
kanpapa 0:1c32a3ae67e1 191 }
kanpapa 0:1c32a3ae67e1 192
kanpapa 0:1c32a3ae67e1 193 int m3pi::getc (void) {
kanpapa 0:1c32a3ae67e1 194 return(_ser.getc());
kanpapa 0:1c32a3ae67e1 195 }
kanpapa 0:1c32a3ae67e1 196
kanpapa 0:1c32a3ae67e1 197
kanpapa 0:1c32a3ae67e1 198
kanpapa 0:1c32a3ae67e1 199
kanpapa 0:1c32a3ae67e1 200
kanpapa 0:1c32a3ae67e1 201 #ifdef MBED_RPC
kanpapa 0:1c32a3ae67e1 202 const rpc_method *m3pi::get_rpc_methods() {
kanpapa 0:1c32a3ae67e1 203 static const rpc_method rpc_methods[] = {{ "forward", rpc_method_caller<m3pi, float, &m3pi::forward> },
kanpapa 0:1c32a3ae67e1 204 { "backward", rpc_method_caller<m3pi, float, &m3pi::backward> },
kanpapa 0:1c32a3ae67e1 205 { "left", rpc_method_caller<m3pi, float, &m3pi::left> },
kanpapa 0:1c32a3ae67e1 206 { "right", rpc_method_caller<m3pi, float, &m3pi::right> },
kanpapa 0:1c32a3ae67e1 207 { "stop", rpc_method_caller<m3pi, &m3pi::stop> },
kanpapa 0:1c32a3ae67e1 208 { "left_motor", rpc_method_caller<m3pi, float, &m3pi::left_motor> },
kanpapa 0:1c32a3ae67e1 209 { "right_motor", rpc_method_caller<m3pi, float, &m3pi::right_motor> },
kanpapa 0:1c32a3ae67e1 210 { "battery", rpc_method_caller<float, m3pi, &m3pi::battery> },
kanpapa 0:1c32a3ae67e1 211 { "line_position", rpc_method_caller<float, m3pi, &m3pi::line_position> },
kanpapa 0:1c32a3ae67e1 212 { "sensor_auto_calibrate", rpc_method_caller<char, m3pi, &m3pi::sensor_auto_calibrate> },
kanpapa 0:1c32a3ae67e1 213
kanpapa 0:1c32a3ae67e1 214
kanpapa 0:1c32a3ae67e1 215 RPC_METHOD_SUPER(Base)
kanpapa 0:1c32a3ae67e1 216 };
kanpapa 0:1c32a3ae67e1 217 return rpc_methods;
kanpapa 0:1c32a3ae67e1 218 }
kanpapa 0:1c32a3ae67e1 219 #endif