Zachary Sunberg / Mbed 2 deprecated SAILORSbot

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

m3pi/m3pi.cpp@0:9f058fe8cab5, 2015-06-25 (annotated)

Committer:
zsunberg
Date:
Thu Jun 25 00:10:50 2015 +0000
Revision:
0:9f058fe8cab5
before erasing old led stuff

Who changed what in which revision?

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