Polybot / Mbed 2 deprecated mbed_ET5_TP1

TP1

Dependencies:   mbed

m3pi.cpp@10:4276eb0b8d4c, 2020-12-09 (annotated)

Committer:
AntoineDeb
Date:
Wed Dec 09 11:31:07 2020 +0000
Revision:
10:4276eb0b8d4c
Parent:
0:89e0c9ac4a1a 
end;

Who changed what in which revision?

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