Stephan Willis / m3pi

za

Fork of m3pi by Chris Styles

m3pi.cpp@14:4739a7ef4251, 2013-11-16 (annotated)

Committer:
s4stevo
Date:
Sat Nov 16 01:24:57 2013 +0000
Revision:
14:4739a7ef4251
Parent:
10:30aaa3c1910a 
25

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chris 0:e6020bd04b45 1
chris 0:e6020bd04b45 2 #include "mbed.h"
chris 0:e6020bd04b45 3 #include "m3pi.h"
chris 0:e6020bd04b45 4
s4stevo 9:08cc708cee52 5
s4stevo 9:08cc708cee52 6 mbed::AnalogIn pRight(p19);
s4stevo 9:08cc708cee52 7
s4stevo 9:08cc708cee52 8
chris 7:9b128cebb3c2 9 m3pi::m3pi(PinName nrst, PinName tx, PinName rx) : Stream("m3pi"), _nrst(nrst), _ser(tx, rx) {
chris 0:e6020bd04b45 10 _ser.baud(115200);
chris 0:e6020bd04b45 11 reset();
chris 0:e6020bd04b45 12 }
chris 0:e6020bd04b45 13
chris 7:9b128cebb3c2 14 m3pi::m3pi() : Stream("m3pi"), _nrst(p23), _ser(p9, p10) {
chris 7:9b128cebb3c2 15 _ser.baud(115200);
chris 7:9b128cebb3c2 16 reset();
chris 7:9b128cebb3c2 17 }
chris 7:9b128cebb3c2 18
chris 7:9b128cebb3c2 19
s4stevo 9:08cc708cee52 20
chris 0:e6020bd04b45 21 void m3pi::reset () {
chris 0:e6020bd04b45 22 _nrst = 0;
chris 0:e6020bd04b45 23 wait (0.01);
chris 0:e6020bd04b45 24 _nrst = 1;
chris 6:62ee1486ecb9 25 wait (0.1);
chris 0:e6020bd04b45 26 }
chris 0:e6020bd04b45 27
chris 0:e6020bd04b45 28 void m3pi::left_motor (float speed) {
chris 0:e6020bd04b45 29 motor(0,speed);
chris 0:e6020bd04b45 30 }
chris 0:e6020bd04b45 31
chris 0:e6020bd04b45 32 void m3pi::right_motor (float speed) {
chris 0:e6020bd04b45 33 motor(1,speed);
chris 0:e6020bd04b45 34 }
chris 0:e6020bd04b45 35
chris 0:e6020bd04b45 36 void m3pi::forward (float speed) {
chris 0:e6020bd04b45 37 motor(0,speed);
chris 0:e6020bd04b45 38 motor(1,speed);
chris 0:e6020bd04b45 39 }
chris 0:e6020bd04b45 40
chris 0:e6020bd04b45 41 void m3pi::backward (float speed) {
chris 0:e6020bd04b45 42 motor(0,-1.0*speed);
chris 0:e6020bd04b45 43 motor(1,-1.0*speed);
chris 0:e6020bd04b45 44 }
chris 0:e6020bd04b45 45
chris 0:e6020bd04b45 46 void m3pi::left (float speed) {
chris 0:e6020bd04b45 47 motor(0,speed);
chris 0:e6020bd04b45 48 motor(1,-1.0*speed);
chris 0:e6020bd04b45 49 }
chris 0:e6020bd04b45 50
chris 0:e6020bd04b45 51 void m3pi::right (float speed) {
chris 0:e6020bd04b45 52 motor(0,-1.0*speed);
chris 0:e6020bd04b45 53 motor(1,speed);
chris 0:e6020bd04b45 54 }
chris 0:e6020bd04b45 55
chris 0:e6020bd04b45 56 void m3pi::stop (void) {
chris 0:e6020bd04b45 57 motor(0,0.0);
chris 0:e6020bd04b45 58 motor(1,0.0);
chris 0:e6020bd04b45 59 }
chris 0:e6020bd04b45 60
chris 0:e6020bd04b45 61 void m3pi::motor (int motor, float speed) {
chris 0:e6020bd04b45 62 char opcode = 0x0;
chris 0:e6020bd04b45 63 if (speed > 0.0) {
chris 0:e6020bd04b45 64 if (motor==1)
chris 0:e6020bd04b45 65 opcode = M1_FORWARD;
chris 0:e6020bd04b45 66 else
chris 0:e6020bd04b45 67 opcode = M2_FORWARD;
chris 0:e6020bd04b45 68 } else {
chris 0:e6020bd04b45 69 if (motor==1)
chris 0:e6020bd04b45 70 opcode = M1_BACKWARD;
chris 0:e6020bd04b45 71 else
chris 0:e6020bd04b45 72 opcode = M2_BACKWARD;
chris 0:e6020bd04b45 73 }
chris 0:e6020bd04b45 74 unsigned char arg = 0x7f * abs(speed);
chris 0:e6020bd04b45 75
chris 0:e6020bd04b45 76 _ser.putc(opcode);
chris 0:e6020bd04b45 77 _ser.putc(arg);
chris 0:e6020bd04b45 78 }
s4stevo 9:08cc708cee52 79 //Battery Function
chris 0:e6020bd04b45 80 float m3pi::battery() {
chris 0:e6020bd04b45 81 _ser.putc(SEND_BATTERY_MILLIVOLTS);
chris 0:e6020bd04b45 82 char lowbyte = _ser.getc();
chris 0:e6020bd04b45 83 char hibyte = _ser.getc();
chris 0:e6020bd04b45 84 float v = ((lowbyte + (hibyte << 8))/1000.0);
chris 0:e6020bd04b45 85 return(v);
chris 0:e6020bd04b45 86 }
chris 0:e6020bd04b45 87
s4stevo 9:08cc708cee52 88
s4stevo 9:08cc708cee52 89 float m3pi::position() {
chris 0:e6020bd04b45 90 int pos = 0;
s4stevo 9:08cc708cee52 91
chris 0:e6020bd04b45 92
s4stevo 9:08cc708cee52 93 return(pos);
s4stevo 9:08cc708cee52 94
chris 0:e6020bd04b45 95 }
chris 0:e6020bd04b45 96
s4stevo 9:08cc708cee52 97 //Don't need this.
s4stevo 9:08cc708cee52 98 /*
chris 0:e6020bd04b45 99 char m3pi::sensor_auto_calibrate() {
chris 0:e6020bd04b45 100 _ser.putc(AUTO_CALIBRATE);
chris 0:e6020bd04b45 101 return(_ser.getc());
s4stevo 9:08cc708cee52 102 }*/
chris 0:e6020bd04b45 103
s4stevo 9:08cc708cee52 104 //Don't need this but need something similar.
s4stevo 9:08cc708cee52 105 /*
chris 0:e6020bd04b45 106 void m3pi::calibrate(void) {
chris 0:e6020bd04b45 107 _ser.putc(PI_CALIBRATE);
chris 0:e6020bd04b45 108 }
chris 0:e6020bd04b45 109
s4stevo 9:08cc708cee52 110 //Don't need this.
s4stevo 9:08cc708cee52 111
chris 0:e6020bd04b45 112 void m3pi::reset_calibration() {
chris 0:e6020bd04b45 113 _ser.putc(LINE_SENSORS_RESET_CALIBRATION);
chris 0:e6020bd04b45 114 }
s4stevo 9:08cc708cee52 115 */
chris 0:e6020bd04b45 116
chris 0:e6020bd04b45 117 void m3pi::PID_start(int max_speed, int a, int b, int c, int d) {
chris 0:e6020bd04b45 118 _ser.putc(max_speed);
chris 0:e6020bd04b45 119 _ser.putc(a);
chris 0:e6020bd04b45 120 _ser.putc(b);
chris 0:e6020bd04b45 121 _ser.putc(c);
chris 0:e6020bd04b45 122 _ser.putc(d);
chris 0:e6020bd04b45 123 }
chris 0:e6020bd04b45 124
chris 0:e6020bd04b45 125 void m3pi::PID_stop() {
chris 0:e6020bd04b45 126 _ser.putc(STOP_PID);
chris 0:e6020bd04b45 127 }
chris 0:e6020bd04b45 128
s4stevo 9:08cc708cee52 129 //Don't need.
s4stevo 9:08cc708cee52 130
s4stevo 9:08cc708cee52 131 /*float m3pi::pot_voltage(void) {
chris 0:e6020bd04b45 132 int volt = 0;
chris 0:e6020bd04b45 133 _ser.putc(SEND_TRIMPOT);
chris 0:e6020bd04b45 134 volt = _ser.getc();
chris 0:e6020bd04b45 135 volt += _ser.getc() << 8;
chris 0:e6020bd04b45 136 return(volt);
chris 0:e6020bd04b45 137 }
s4stevo 9:08cc708cee52 138 */
s4stevo 9:08cc708cee52 139 //Print to LEDs. Probably wont use, possibly for voltage level.
chris 5:09fb0636207b 140 void m3pi::leds(int val) {
chris 7:9b128cebb3c2 141
chris 7:9b128cebb3c2 142 BusOut _leds(p20,p19,p18,p17,p16,p15,p14,p13);
chris 5:09fb0636207b 143 _leds = val;
chris 5:09fb0636207b 144 }
chris 5:09fb0636207b 145
s4stevo 9:08cc708cee52 146 //LCD Function.
chris 0:e6020bd04b45 147 void m3pi::locate(int x, int y) {
chris 0:e6020bd04b45 148 _ser.putc(DO_LCD_GOTO_XY);
chris 0:e6020bd04b45 149 _ser.putc(x);
chris 0:e6020bd04b45 150 _ser.putc(y);
chris 0:e6020bd04b45 151 }
s4stevo 9:08cc708cee52 152 //Clear LCD
chris 0:e6020bd04b45 153 void m3pi::cls(void) {
chris 0:e6020bd04b45 154 _ser.putc(DO_CLEAR);
chris 0:e6020bd04b45 155 }
s4stevo 10:30aaa3c1910a 156
s4stevo 10:30aaa3c1910a 157 void m3pi::play(void) {
s4stevo 10:30aaa3c1910a 158 _ser.putc(DO_PLAY);
s4stevo 10:30aaa3c1910a 159 }
s4stevo 10:30aaa3c1910a 160 //Print function for LCD
chris 0:e6020bd04b45 161 int m3pi::print (char* text, int length) {
chris 0:e6020bd04b45 162 _ser.putc(DO_PRINT);
chris 0:e6020bd04b45 163 _ser.putc(length);
chris 0:e6020bd04b45 164 for (int i = 0 ; i < length ; i++) {
chris 0:e6020bd04b45 165 _ser.putc(text[i]);
chris 0:e6020bd04b45 166 }
chris 0:e6020bd04b45 167 return(0);
chris 0:e6020bd04b45 168 }
chris 0:e6020bd04b45 169
s4stevo 9:08cc708cee52 170 //Print function for LCD
chris 0:e6020bd04b45 171 int m3pi::_putc (int c) {
chris 0:e6020bd04b45 172 _ser.putc(DO_PRINT);
chris 0:e6020bd04b45 173 _ser.putc(0x1);
chris 0:e6020bd04b45 174 _ser.putc(c);
chris 0:e6020bd04b45 175 wait (0.001);
chris 0:e6020bd04b45 176 return(c);
chris 0:e6020bd04b45 177 }
chris 0:e6020bd04b45 178
chris 0:e6020bd04b45 179 int m3pi::_getc (void) {
chris 0:e6020bd04b45 180 char r = 0;
chris 0:e6020bd04b45 181 return(r);
chris 0:e6020bd04b45 182 }
chris 0:e6020bd04b45 183
chris 0:e6020bd04b45 184 int m3pi::putc (int c) {
chris 0:e6020bd04b45 185 return(_ser.putc(c));
chris 0:e6020bd04b45 186 }
chris 0:e6020bd04b45 187
chris 0:e6020bd04b45 188 int m3pi::getc (void) {
chris 0:e6020bd04b45 189 return(_ser.getc());
chris 0:e6020bd04b45 190 }
chris 0:e6020bd04b45 191
chris 0:e6020bd04b45 192
chris 0:e6020bd04b45 193
chris 0:e6020bd04b45 194
chris 0:e6020bd04b45 195
chris 0:e6020bd04b45 196 #ifdef MBED_RPC
chris 0:e6020bd04b45 197 const rpc_method *m3pi::get_rpc_methods() {
chris 0:e6020bd04b45 198 static const rpc_method rpc_methods[] = {{ "forward", rpc_method_caller<m3pi, float, &m3pi::forward> },
chris 1:816a80dcc1a3 199 { "backward", rpc_method_caller<m3pi, float, &m3pi::backward> },
chris 0:e6020bd04b45 200 { "left", rpc_method_caller<m3pi, float, &m3pi::left> },
chris 0:e6020bd04b45 201 { "right", rpc_method_caller<m3pi, float, &m3pi::right> },
chris 0:e6020bd04b45 202 { "stop", rpc_method_caller<m3pi, &m3pi::stop> },
chris 0:e6020bd04b45 203 { "left_motor", rpc_method_caller<m3pi, float, &m3pi::left_motor> },
chris 0:e6020bd04b45 204 { "right_motor", rpc_method_caller<m3pi, float, &m3pi::right_motor> },
chris 0:e6020bd04b45 205 { "battery", rpc_method_caller<float, m3pi, &m3pi::battery> },
s4stevo 9:08cc708cee52 206 { "position", rpc_method_caller<float, m3pi, &m3pi::position> },
s4stevo 9:08cc708cee52 207 //{ "sensor_auto_calibrate", rpc_method_caller<char, m3pi, &m3pi::sensor_auto_calibrate> },
chris 0:e6020bd04b45 208
chris 0:e6020bd04b45 209
chris 0:e6020bd04b45 210 RPC_METHOD_SUPER(Base)
chris 0:e6020bd04b45 211 };
chris 0:e6020bd04b45 212 return rpc_methods;
chris 0:e6020bd04b45 213 }
chris 0:e6020bd04b45 214 #endif