darron nielsen / m3pi

m3pi

m3pi.cpp@0:fda628f82e5f, 2010-11-23 (annotated)

Committer:
microsat
Date:
Tue Nov 23 06:48:27 2010 +0000
Revision:
0:fda628f82e5f
added play func

Who changed what in which revision?

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