ACC_COSC_2425 / Mbed 2 deprecated GroupProject

For educational purposes

Dependencies:   mbed

m3pi.cpp@1:414699543271, 2014-03-26 (annotated)

Committer:
alexGimson
Date:
Wed Mar 26 01:24:26 2014 +0000
Revision:
1:414699543271
Parent:
0:7116cced0ada 
hey

Who changed what in which revision?

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