David Lopes / m3pi

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.

Fork of m3pi by Chris Styles

m3pi.cpp@2:330eb028e85b, 2010-11-01 (annotated)

Committer:
chris
Date:
Mon Nov 01 16:18:55 2010 +0000
Revision:
2:330eb028e85b
Parent:
1:816a80dcc1a3
Child:
3:c38d2f980494
Corrected code line_position function

Who changed what in which revision?

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