Christian Burri / Mbed 2 deprecated autonomous Robot Android

This program is for an autonomous robot for the competition at the Hochschule Luzern. http://cruisingcrepe.wordpress.com/ We are one of the 32 teams. http://cruisingcrepe.wordpress.com/ The postition control is based on this Documentation: Control of Wheeled Mobile Robots: An Experimental Overview from Alessandro De Luca, Giuseppe Oriolo, Marilena Vendittelli. For more information see here: http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf

Dependencies:   mbed

Fork of autonomous Robot Android by Christian Burri

AndroidADKTerm/android.cpp@14:6a45a9f940a8, 2013-04-11 (annotated)

Committer:
chrigelburri
Date:
Thu Apr 11 09:22:35 2013 +0000
Revision:
14:6a45a9f940a8
Parent:
12:235e318a414f
Child:
15:cb1337567ad4
android 2.0 implemented (untested!)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chrigelburri 11:775ebb69d5e1 1 #include "android.h"
chrigelburri 14:6a45a9f940a8 2 //#define PI 3.141592654f
chrigelburri 11:775ebb69d5e1 3 using namespace std;
chrigelburri 11:775ebb69d5e1 4
chrigelburri 14:6a45a9f940a8 5 AdkTerm::AdkTerm(RobotControl* robotControl,
chrigelburri 14:6a45a9f940a8 6 state_t* s,
chrigelburri 14:6a45a9f940a8 7 float period)
chrigelburri 14:6a45a9f940a8 8 : AndroidAccessory(INBL,
chrigelburri 14:6a45a9f940a8 9 OUTL,
chrigelburri 14:6a45a9f940a8 10 "ARM",
chrigelburri 14:6a45a9f940a8 11 "mbed",
chrigelburri 14:6a45a9f940a8 12 "mbed Terminal",
chrigelburri 14:6a45a9f940a8 13 "0.1",
chrigelburri 14:6a45a9f940a8 14 "http://www.mbed.org",
chrigelburri 14:6a45a9f940a8 15 "0000000012345678"),
chrigelburri 14:6a45a9f940a8 16 Task(period)
chrigelburri 11:775ebb69d5e1 17 {
chrigelburri 14:6a45a9f940a8 18 this->robotControl = robotControl;
chrigelburri 14:6a45a9f940a8 19 this->period = period;
chrigelburri 14:6a45a9f940a8 20 this->s = s;
chrigelburri 11:775ebb69d5e1 21 };
chrigelburri 11:775ebb69d5e1 22
chrigelburri 11:775ebb69d5e1 23 void AdkTerm::setupDevice()
chrigelburri 11:775ebb69d5e1 24 {
chrigelburri 11:775ebb69d5e1 25 for (int i = 0; i<OUTL; i++) {
chrigelburri 11:775ebb69d5e1 26 buffer[i] = 0;
chrigelburri 11:775ebb69d5e1 27 }
chrigelburri 11:775ebb69d5e1 28 bcount = 0;
chrigelburri 11:775ebb69d5e1 29 }
chrigelburri 11:775ebb69d5e1 30
chrigelburri 14:6a45a9f940a8 31 float AdkTerm::getDesiredX()
chrigelburri 11:775ebb69d5e1 32 {
chrigelburri 14:6a45a9f940a8 33 return desiredX/1000;
chrigelburri 11:775ebb69d5e1 34 }
chrigelburri 11:775ebb69d5e1 35
chrigelburri 14:6a45a9f940a8 36 float AdkTerm::getDesiredY()
chrigelburri 11:775ebb69d5e1 37 {
chrigelburri 14:6a45a9f940a8 38 return desiredY/1000;
chrigelburri 11:775ebb69d5e1 39 }
chrigelburri 11:775ebb69d5e1 40
chrigelburri 14:6a45a9f940a8 41 float AdkTerm::getDesiredTheta()
chrigelburri 11:775ebb69d5e1 42 {
chrigelburri 14:6a45a9f940a8 43 return desiredTheta * PI / 180;
chrigelburri 11:775ebb69d5e1 44 }
chrigelburri 11:775ebb69d5e1 45
chrigelburri 11:775ebb69d5e1 46
chrigelburri 14:6a45a9f940a8 47 void AdkTerm::run()
chrigelburri 14:6a45a9f940a8 48 {
chrigelburri 11:775ebb69d5e1 49
chrigelburri 14:6a45a9f940a8 50 u8* wbuf = _writebuff;
chrigelburri 11:775ebb69d5e1 51
chrigelburri 14:6a45a9f940a8 52 // floats to string
chrigelburri 14:6a45a9f940a8 53 char str[32];
chrigelburri 14:6a45a9f940a8 54
chrigelburri 14:6a45a9f940a8 55 //send to android
chrigelburri 14:6a45a9f940a8 56 sprintf( str, "%f;%f;%f;%i;%i;%i;%f", robotControl->getxActualPosition(),
chrigelburri 14:6a45a9f940a8 57 robotControl->getyActualPosition(),
chrigelburri 14:6a45a9f940a8 58 robotControl->getActualTheta(),
chrigelburri 14:6a45a9f940a8 59 s->state&STATE_UNDER == 0 ? 0 : 1,
chrigelburri 14:6a45a9f940a8 60 s->state&STATE_LEFT == 0 ? 0 : 1,
chrigelburri 14:6a45a9f940a8 61 s->state&STATE_RIGHT == 0 ? 0 : 1,
chrigelburri 14:6a45a9f940a8 62 s->voltageBattery);
chrigelburri 11:775ebb69d5e1 63
chrigelburri 14:6a45a9f940a8 64 //copy to wbuf
chrigelburri 14:6a45a9f940a8 65 strcpy( (char*) wbuf, str );
chrigelburri 11:775ebb69d5e1 66
chrigelburri 14:6a45a9f940a8 67 this->write(wbuf,32);
chrigelburri 14:6a45a9f940a8 68
chrigelburri 14:6a45a9f940a8 69 }
chrigelburri 11:775ebb69d5e1 70
chrigelburri 11:775ebb69d5e1 71 void AdkTerm::resetDevice()
chrigelburri 11:775ebb69d5e1 72 {
chrigelburri 11:775ebb69d5e1 73 for (int i = 0; i<OUTL; i++) {
chrigelburri 11:775ebb69d5e1 74 buffer[i] = 0;
chrigelburri 11:775ebb69d5e1 75 }
chrigelburri 11:775ebb69d5e1 76 bcount = 0;
chrigelburri 11:775ebb69d5e1 77 }
chrigelburri 11:775ebb69d5e1 78
chrigelburri 11:775ebb69d5e1 79 void AdkTerm::Tokenize(const string& str,
chrigelburri 11:775ebb69d5e1 80 vector<string>& tokens,
chrigelburri 11:775ebb69d5e1 81 const string& delimiters /*= " "*/)
chrigelburri 11:775ebb69d5e1 82 {
chrigelburri 11:775ebb69d5e1 83 // Skip delimiters at beginning.
chrigelburri 11:775ebb69d5e1 84 string::size_type lastPos = str.find_first_not_of(delimiters, 0);
chrigelburri 11:775ebb69d5e1 85 // Find first "non-delimiter".
chrigelburri 11:775ebb69d5e1 86 string::size_type pos = str.find_first_of(delimiters, lastPos);
chrigelburri 11:775ebb69d5e1 87
chrigelburri 11:775ebb69d5e1 88 while (string::npos != pos || string::npos != lastPos) {
chrigelburri 11:775ebb69d5e1 89 // Found a token, add it to the vector.
chrigelburri 11:775ebb69d5e1 90 tokens.push_back(str.substr(lastPos, pos - lastPos));
chrigelburri 11:775ebb69d5e1 91 // Skip delimiters. Note the "not_of"
chrigelburri 11:775ebb69d5e1 92 lastPos = str.find_first_not_of(delimiters, pos);
chrigelburri 11:775ebb69d5e1 93 // Find next "non-delimiter"
chrigelburri 11:775ebb69d5e1 94 pos = str.find_first_of(delimiters, lastPos);
chrigelburri 11:775ebb69d5e1 95 }
chrigelburri 11:775ebb69d5e1 96 }
chrigelburri 11:775ebb69d5e1 97
chrigelburri 11:775ebb69d5e1 98 int AdkTerm::callbackRead(u8 *buf, int len)
chrigelburri 11:775ebb69d5e1 99 {
chrigelburri 11:775ebb69d5e1 100 // convert buffer (unsigned char) to string
chrigelburri 11:775ebb69d5e1 101 std::string str(reinterpret_cast<char*>(buf), len);
chrigelburri 11:775ebb69d5e1 102
chrigelburri 11:775ebb69d5e1 103 // new vector of strings
chrigelburri 11:775ebb69d5e1 104 vector<string> tokens;
chrigelburri 11:775ebb69d5e1 105
chrigelburri 11:775ebb69d5e1 106 // tokenize the string with the semicolon separator
chrigelburri 11:775ebb69d5e1 107 Tokenize(str, tokens, ";");
chrigelburri 11:775ebb69d5e1 108 copy(tokens.begin(), tokens.end(), ostream_iterator<string>(cout, ", "));
chrigelburri 11:775ebb69d5e1 109
chrigelburri 11:775ebb69d5e1 110
chrigelburri 11:775ebb69d5e1 111 if(tokens.size() > 2) {
chrigelburri 11:775ebb69d5e1 112
chrigelburri 11:775ebb69d5e1 113 //string to float
chrigelburri 14:6a45a9f940a8 114 desiredX = ::atof(tokens.at(0).c_str());
chrigelburri 14:6a45a9f940a8 115 desiredY = ::atof(tokens.at(1).c_str());
chrigelburri 14:6a45a9f940a8 116 desiredTheta = ::atof(tokens.at(2).c_str());
chrigelburri 11:775ebb69d5e1 117 }
chrigelburri 11:775ebb69d5e1 118
chrigelburri 11:775ebb69d5e1 119 return 0;
chrigelburri 11:775ebb69d5e1 120 }
chrigelburri 11:775ebb69d5e1 121
chrigelburri 11:775ebb69d5e1 122 int AdkTerm::callbackWrite()
chrigelburri 11:775ebb69d5e1 123 {
chrigelburri 11:775ebb69d5e1 124 ind = false;
chrigelburri 11:775ebb69d5e1 125 return 0;
chrigelburri 11:775ebb69d5e1 126 }
chrigelburri 11:775ebb69d5e1 127
chrigelburri 11:775ebb69d5e1 128 void AdkTerm::serialIRQ()
chrigelburri 11:775ebb69d5e1 129 {
chrigelburri 11:775ebb69d5e1 130 //buffer[bcount] = pc.getc();
chrigelburri 11:775ebb69d5e1 131
chrigelburri 11:775ebb69d5e1 132
chrigelburri 11:775ebb69d5e1 133 if (buffer[bcount] == '\n' || buffer[bcount] == '\r') {
chrigelburri 11:775ebb69d5e1 134 u8* wbuf = _writebuff;
chrigelburri 11:775ebb69d5e1 135 for (int i = 0; i<OUTL; i++) {
chrigelburri 11:775ebb69d5e1 136 wbuf[i] = buffer[i];
chrigelburri 11:775ebb69d5e1 137 buffer[i] = 0;
chrigelburri 11:775ebb69d5e1 138 }
chrigelburri 11:775ebb69d5e1 139 //pc.printf("Sending: %s\n\r",wbuf);
chrigelburri 11:775ebb69d5e1 140 ind = true;
chrigelburri 11:775ebb69d5e1 141 this->write(wbuf,bcount);
chrigelburri 11:775ebb69d5e1 142 bcount = 0;
chrigelburri 11:775ebb69d5e1 143 } else {
chrigelburri 11:775ebb69d5e1 144 if (buffer[bcount] != 0x08 && buffer[bcount] != 0x7F ) {
chrigelburri 11:775ebb69d5e1 145 bcount++;
chrigelburri 11:775ebb69d5e1 146 if (bcount == OUTL) {
chrigelburri 11:775ebb69d5e1 147 bcount = 0;
chrigelburri 11:775ebb69d5e1 148 }
chrigelburri 11:775ebb69d5e1 149 } else {
chrigelburri 11:775ebb69d5e1 150 bcount--;
chrigelburri 11:775ebb69d5e1 151 }
chrigelburri 11:775ebb69d5e1 152 }
chrigelburri 11:775ebb69d5e1 153
chrigelburri 14:6a45a9f940a8 154 }