Masa Itou / Mbed 2 deprecated test2

test

Dependencies:   mbed

m3pi/m3pi.cpp@0:ad3a16d8dbc8, 2013-05-16 (annotated)

Committer:
itoumasa
Date:
Thu May 16 05:58:24 2013 +0000
Revision:
0:ad3a16d8dbc8
jh

Who changed what in which revision?

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