Sowmiya Sivanandham / Mbed 2 deprecated Final_Sonar

seeing i robot - with all the file systems and complete code

Dependencies:   mbed SRF05 Servo CMPS03

Serializer/Serializer.cpp@1:2bac7b6f3840, 2010-12-17 (annotated)

Committer:
sowmy87
Date:
Fri Dec 17 22:55:25 2010 +0000
Revision:
1:2bac7b6f3840
Parent:
0:ee786e500a3c 
Sensor 1 edited

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sowmy87 0:ee786e500a3c 1 /**
sowmy87 0:ee786e500a3c 2 * Includes
sowmy87 0:ee786e500a3c 3 */
sowmy87 0:ee786e500a3c 4 #include "Serializer.h"
sowmy87 0:ee786e500a3c 5
sowmy87 0:ee786e500a3c 6 //Serial pcc(USBTX, USBRX);
sowmy87 0:ee786e500a3c 7
sowmy87 0:ee786e500a3c 8 Serializer::Serializer() {
sowmy87 0:ee786e500a3c 9 serial=new Serial(p28,p27);
sowmy87 0:ee786e500a3c 10 serial->baud(BAUD);
sowmy87 0:ee786e500a3c 11 Initialize();
sowmy87 0:ee786e500a3c 12 }
sowmy87 0:ee786e500a3c 13
sowmy87 0:ee786e500a3c 14 int Serializer::Initialize() {
sowmy87 0:ee786e500a3c 15 char c1=0;
sowmy87 0:ee786e500a3c 16 DigitalOut led1(LED1);
sowmy87 0:ee786e500a3c 17 DigitalOut led2(LED2);
sowmy87 0:ee786e500a3c 18 DigitalOut led3(LED3);
sowmy87 0:ee786e500a3c 19 DigitalOut led4(LED4);
sowmy87 0:ee786e500a3c 20
sowmy87 0:ee786e500a3c 21 led1=1;
sowmy87 0:ee786e500a3c 22 if (serial->writeable())
sowmy87 0:ee786e500a3c 23 serial->printf("fw\r");
sowmy87 0:ee786e500a3c 24 wait(.25);
sowmy87 0:ee786e500a3c 25 led2=1;
sowmy87 0:ee786e500a3c 26 while (serial->readable()) {
sowmy87 0:ee786e500a3c 27 c1=serial->getc();
sowmy87 0:ee786e500a3c 28 }
sowmy87 0:ee786e500a3c 29 if (c1!='>')
sowmy87 0:ee786e500a3c 30 for (int i=0;i<5;i++) {
sowmy87 0:ee786e500a3c 31 led1=led2=led3=led4=1;
sowmy87 0:ee786e500a3c 32 wait(0.1);
sowmy87 0:ee786e500a3c 33 led1=led2=led3=led4=0;
sowmy87 0:ee786e500a3c 34 wait(0.1);
sowmy87 0:ee786e500a3c 35 }
sowmy87 0:ee786e500a3c 36 wait(.25);
sowmy87 0:ee786e500a3c 37 led3=1;
sowmy87 0:ee786e500a3c 38 wait(.25);
sowmy87 0:ee786e500a3c 39 led4=1;
sowmy87 0:ee786e500a3c 40 // SetVPID(1,0,0,100);
sowmy87 0:ee786e500a3c 41 wait(0.25);
sowmy87 0:ee786e500a3c 42 led1=0;
sowmy87 0:ee786e500a3c 43 wait(.25);
sowmy87 0:ee786e500a3c 44 ClearCount();
sowmy87 0:ee786e500a3c 45 led2=0;
sowmy87 0:ee786e500a3c 46 wait(.25);
sowmy87 0:ee786e500a3c 47 led3=0;
sowmy87 0:ee786e500a3c 48 wait(.25);
sowmy87 0:ee786e500a3c 49 led4=0;
sowmy87 0:ee786e500a3c 50 _lPWM=_rPWM=0;
sowmy87 0:ee786e500a3c 51 _isBusy=0;
sowmy87 0:ee786e500a3c 52 _irqIsBusy=0;
sowmy87 0:ee786e500a3c 53 // serial->attach(this, &Serializer::InterruptHandler);
sowmy87 0:ee786e500a3c 54
sowmy87 0:ee786e500a3c 55 return 0;
sowmy87 0:ee786e500a3c 56 }
sowmy87 0:ee786e500a3c 57
sowmy87 0:ee786e500a3c 58 Serializer::~Serializer() {
sowmy87 0:ee786e500a3c 59 delete serial;
sowmy87 0:ee786e500a3c 60 }
sowmy87 0:ee786e500a3c 61
sowmy87 0:ee786e500a3c 62 void Serializer::Stop() {
sowmy87 0:ee786e500a3c 63 if (serial->writeable())
sowmy87 0:ee786e500a3c 64 serial->printf("stop\r");
sowmy87 0:ee786e500a3c 65 leftSpeed=rightSpeed=0;
sowmy87 0:ee786e500a3c 66 // pc.printf("Stop\r\n");
sowmy87 0:ee786e500a3c 67 }
sowmy87 0:ee786e500a3c 68
sowmy87 0:ee786e500a3c 69 void Serializer::ClearCountLeft() {
sowmy87 0:ee786e500a3c 70 if (serial->writeable())
sowmy87 0:ee786e500a3c 71 serial->printf("clrenc 1\r");
sowmy87 0:ee786e500a3c 72 }
sowmy87 0:ee786e500a3c 73 void Serializer::ClearCountRight() {
sowmy87 0:ee786e500a3c 74 if (serial->writeable())
sowmy87 0:ee786e500a3c 75 serial->printf("clrenc 2\r");
sowmy87 0:ee786e500a3c 76 }
sowmy87 0:ee786e500a3c 77
sowmy87 0:ee786e500a3c 78 void Serializer::ClearCount() {
sowmy87 0:ee786e500a3c 79 if (serial->writeable())
sowmy87 0:ee786e500a3c 80 serial->printf("clrenc 1 2\r");
sowmy87 0:ee786e500a3c 81 }
sowmy87 0:ee786e500a3c 82
sowmy87 0:ee786e500a3c 83 void Serializer::SetSpeedLeft(int inPsec) {
sowmy87 0:ee786e500a3c 84 if (inPsec>MAX_SPEED)inPsec=MAX_SPEED;
sowmy87 0:ee786e500a3c 85 if (inPsec<-MAX_SPEED)inPsec=-MAX_SPEED;
sowmy87 0:ee786e500a3c 86 if (serial->writeable())
sowmy87 0:ee786e500a3c 87 serial->printf("mogo 1:%i\r", -inPsec);
sowmy87 0:ee786e500a3c 88 leftSpeed=inPsec;
sowmy87 0:ee786e500a3c 89 }
sowmy87 0:ee786e500a3c 90 void Serializer::SetSpeedRight(int inPsec) {
sowmy87 0:ee786e500a3c 91 if (inPsec>MAX_SPEED)inPsec=MAX_SPEED;
sowmy87 0:ee786e500a3c 92 if (inPsec<-MAX_SPEED)inPsec=-MAX_SPEED;
sowmy87 0:ee786e500a3c 93 if (serial->writeable())
sowmy87 0:ee786e500a3c 94 serial->printf("mogo 2:%i\r", -inPsec);
sowmy87 0:ee786e500a3c 95 rightSpeed=inPsec;
sowmy87 0:ee786e500a3c 96 }
sowmy87 0:ee786e500a3c 97
sowmy87 0:ee786e500a3c 98 void Serializer::SetSpeed(int inPsec) {
sowmy87 0:ee786e500a3c 99 if (inPsec>MAX_SPEED)inPsec=MAX_SPEED;
sowmy87 0:ee786e500a3c 100 if (inPsec<-MAX_SPEED)inPsec=-MAX_SPEED;
sowmy87 0:ee786e500a3c 101 inPsec*=-1;
sowmy87 0:ee786e500a3c 102 if (serial->writeable())
sowmy87 0:ee786e500a3c 103 serial->printf("mogo 1:%i 2:%i\r", inPsec-50, inPsec);
sowmy87 0:ee786e500a3c 104 leftSpeed=rightSpeed=inPsec;
sowmy87 0:ee786e500a3c 105 }
sowmy87 0:ee786e500a3c 106
sowmy87 0:ee786e500a3c 107 void Serializer::SetVPID(int p,int i,int d,int l) {
sowmy87 0:ee786e500a3c 108 if (serial->writeable())
sowmy87 0:ee786e500a3c 109 serial->printf("vpid %i:%i:%i:%i\r", p,i,d,l);
sowmy87 0:ee786e500a3c 110 }
sowmy87 0:ee786e500a3c 111
sowmy87 0:ee786e500a3c 112 void Serializer::SetDPID(int p,int i,int d,int a) {
sowmy87 0:ee786e500a3c 113 if (serial->writeable())
sowmy87 0:ee786e500a3c 114 serial->printf("dpid %i:%i:%i:%i\r", p,i,d,a);
sowmy87 0:ee786e500a3c 115 }
sowmy87 0:ee786e500a3c 116
sowmy87 0:ee786e500a3c 117 void Serializer::DiGoLeft(int dist,int inPsec) {
sowmy87 0:ee786e500a3c 118 /* if(_isBusy)
sowmy87 0:ee786e500a3c 119 return;
sowmy87 0:ee786e500a3c 120 _isBusy=1;
sowmy87 0:ee786e500a3c 121 */
sowmy87 0:ee786e500a3c 122 if (serial->writeable())
sowmy87 0:ee786e500a3c 123 serial->printf("digo 1:%i:%i\r",dist*PULSES_PER_INCH, inPsec);
sowmy87 0:ee786e500a3c 124 }
sowmy87 0:ee786e500a3c 125 void Serializer::DiGoRight(int dist,int inPsec) {
sowmy87 0:ee786e500a3c 126 /* if(_isBusy)
sowmy87 0:ee786e500a3c 127 return;
sowmy87 0:ee786e500a3c 128 _isBusy=1;
sowmy87 0:ee786e500a3c 129 */
sowmy87 0:ee786e500a3c 130 if (serial->writeable())
sowmy87 0:ee786e500a3c 131 serial->printf("digo 2:%i:%i\r",dist*PULSES_PER_INCH, inPsec);
sowmy87 0:ee786e500a3c 132 }
sowmy87 0:ee786e500a3c 133 void Serializer::DiGo(int dist,int inPsec) {
sowmy87 0:ee786e500a3c 134 /* if(_isBusy)
sowmy87 0:ee786e500a3c 135 return;
sowmy87 0:ee786e500a3c 136 _isBusy=1;
sowmy87 0:ee786e500a3c 137 */
sowmy87 0:ee786e500a3c 138 if (serial->writeable()) {
sowmy87 0:ee786e500a3c 139 serial->printf("digo 1:%i:%i 2:%i:%i\r", \
sowmy87 0:ee786e500a3c 140 -dist*PULSES_PER_INCH, \
sowmy87 0:ee786e500a3c 141 inPsec, \
sowmy87 0:ee786e500a3c 142 -dist*PULSES_PER_INCH, \
sowmy87 0:ee786e500a3c 143 inPsec);
sowmy87 0:ee786e500a3c 144
sowmy87 0:ee786e500a3c 145 }
sowmy87 0:ee786e500a3c 146 }
sowmy87 0:ee786e500a3c 147
sowmy87 0:ee786e500a3c 148 int Serializer::IsBusy() {
sowmy87 0:ee786e500a3c 149
sowmy87 0:ee786e500a3c 150 if (_isBusy&&serial->writeable()) {
sowmy87 0:ee786e500a3c 151 serial->printf("pids\r");
sowmy87 0:ee786e500a3c 152 wait(0.01);
sowmy87 0:ee786e500a3c 153 }
sowmy87 0:ee786e500a3c 154 return _isBusy;
sowmy87 0:ee786e500a3c 155 }
sowmy87 0:ee786e500a3c 156 /*
sowmy87 0:ee786e500a3c 157 char tmp=0;
sowmy87 0:ee786e500a3c 158 if (_isBusy) {
sowmy87 0:ee786e500a3c 159 if (serial->writeable()) {
sowmy87 0:ee786e500a3c 160 serial->printf("pids\r");
sowmy87 0:ee786e500a3c 161 }
sowmy87 0:ee786e500a3c 162 wait(0.1);
sowmy87 0:ee786e500a3c 163 while (serial->readable()) {
sowmy87 0:ee786e500a3c 164 tmp=serial->getc();
sowmy87 0:ee786e500a3c 165 pcc.putc(tmp);
sowmy87 0:ee786e500a3c 166 if (tmp=='1')
sowmy87 0:ee786e500a3c 167 _isBusy=1;
sowmy87 0:ee786e500a3c 168 else if (tmp=='0')
sowmy87 0:ee786e500a3c 169 _isBusy=0;
sowmy87 0:ee786e500a3c 170 }
sowmy87 0:ee786e500a3c 171 }
sowmy87 0:ee786e500a3c 172 pcc.printf("IsBusy = %i\n\r",_isBusy);
sowmy87 0:ee786e500a3c 173 return _isBusy;
sowmy87 0:ee786e500a3c 174 */
sowmy87 0:ee786e500a3c 175
sowmy87 0:ee786e500a3c 176
sowmy87 0:ee786e500a3c 177
sowmy87 0:ee786e500a3c 178 void Serializer::TurnLeft(int deg) {
sowmy87 0:ee786e500a3c 179
sowmy87 0:ee786e500a3c 180 }
sowmy87 0:ee786e500a3c 181
sowmy87 0:ee786e500a3c 182 void Serializer::TurnRight(int deg) {
sowmy87 0:ee786e500a3c 183
sowmy87 0:ee786e500a3c 184
sowmy87 0:ee786e500a3c 185 }
sowmy87 0:ee786e500a3c 186 void Serializer::PivetLeft(int deg) {
sowmy87 0:ee786e500a3c 187
sowmy87 0:ee786e500a3c 188 Stop();
sowmy87 0:ee786e500a3c 189 deg=deg*PIVET_ADJUSTMENT;
sowmy87 0:ee786e500a3c 190 wait(0.1);
sowmy87 0:ee786e500a3c 191 if (serial->writeable())
sowmy87 0:ee786e500a3c 192 serial->printf("digo 1:%i:%i 2:%i:%i\r",deg,PIVET_SPEED,-deg,PIVET_SPEED);
sowmy87 0:ee786e500a3c 193
sowmy87 0:ee786e500a3c 194 }
sowmy87 0:ee786e500a3c 195 void Serializer::PivetRight(int deg) {
sowmy87 0:ee786e500a3c 196 this->Stop();
sowmy87 0:ee786e500a3c 197 deg=deg*PIVET_ADJUSTMENT;
sowmy87 0:ee786e500a3c 198 wait(0.1);
sowmy87 0:ee786e500a3c 199 if (serial->writeable())
sowmy87 0:ee786e500a3c 200 serial->printf("digo 1:%i:%i 2:%i:%i\r",-deg,PIVET_SPEED,deg,PIVET_SPEED);
sowmy87 0:ee786e500a3c 201
sowmy87 0:ee786e500a3c 202 }
sowmy87 0:ee786e500a3c 203
sowmy87 0:ee786e500a3c 204 void Serializer::SetLeftPWM(int pwm) {
sowmy87 0:ee786e500a3c 205 _lPWM=-pwm;
sowmy87 0:ee786e500a3c 206 if (serial->writeable())
sowmy87 0:ee786e500a3c 207 serial->printf("pwm 1:%i\r",_lPWM);
sowmy87 0:ee786e500a3c 208
sowmy87 0:ee786e500a3c 209 }
sowmy87 0:ee786e500a3c 210
sowmy87 0:ee786e500a3c 211 void Serializer::SetRightPWM(int pwm) {
sowmy87 0:ee786e500a3c 212 _rPWM=-pwm;
sowmy87 0:ee786e500a3c 213 if (serial->writeable())
sowmy87 0:ee786e500a3c 214 serial->printf("pwm 2:%i\r",_rPWM);
sowmy87 0:ee786e500a3c 215 }
sowmy87 0:ee786e500a3c 216
sowmy87 0:ee786e500a3c 217 void Serializer::SetPWM(int pwm){
sowmy87 0:ee786e500a3c 218 _rPWM=-pwm;
sowmy87 0:ee786e500a3c 219 _lPWM=-pwm;
sowmy87 0:ee786e500a3c 220 if (serial->writeable())
sowmy87 0:ee786e500a3c 221 serial->printf("pwm 1:%i 2:%i\r",_lPWM, _rPWM);
sowmy87 0:ee786e500a3c 222 }
sowmy87 0:ee786e500a3c 223
sowmy87 0:ee786e500a3c 224 void Serializer::SetPWM(int lPwm, int rPwm){
sowmy87 0:ee786e500a3c 225 _rPWM=-rPwm;
sowmy87 0:ee786e500a3c 226 _lPWM=-lPwm;
sowmy87 0:ee786e500a3c 227 if (serial->writeable())
sowmy87 0:ee786e500a3c 228 serial->printf("pwm 1:%i 2:%i\r",_lPWM, _rPWM);
sowmy87 0:ee786e500a3c 229 }
sowmy87 0:ee786e500a3c 230
sowmy87 0:ee786e500a3c 231 int Serializer::GetCountLeft() {
sowmy87 0:ee786e500a3c 232 if (serial->writeable()) {
sowmy87 0:ee786e500a3c 233 serial->printf("getenc 1\r");
sowmy87 0:ee786e500a3c 234 // pcc.printf("Sendreq\t");
sowmy87 0:ee786e500a3c 235 }
sowmy87 0:ee786e500a3c 236 wait(0.01);
sowmy87 0:ee786e500a3c 237 int ret=0;
sowmy87 0:ee786e500a3c 238 char c=0;
sowmy87 0:ee786e500a3c 239 while (serial->readable()) {
sowmy87 0:ee786e500a3c 240 // pcc.printf("Reading");
sowmy87 0:ee786e500a3c 241 c=serial->getc();
sowmy87 0:ee786e500a3c 242 // pcc.putc(c);
sowmy87 0:ee786e500a3c 243 if (c>='0'&&c<='9') {
sowmy87 0:ee786e500a3c 244 c-='0';
sowmy87 0:ee786e500a3c 245 ret*=10;
sowmy87 0:ee786e500a3c 246 ret+=c;
sowmy87 0:ee786e500a3c 247 }
sowmy87 0:ee786e500a3c 248 }
sowmy87 0:ee786e500a3c 249 return ret;
sowmy87 0:ee786e500a3c 250 }
sowmy87 0:ee786e500a3c 251
sowmy87 0:ee786e500a3c 252 int Serializer::GetCountRight() {
sowmy87 0:ee786e500a3c 253 if (serial->writeable()) {
sowmy87 0:ee786e500a3c 254 serial->printf("getenc 2\r");
sowmy87 0:ee786e500a3c 255 // pcc.printf("Sendreq\t");
sowmy87 0:ee786e500a3c 256 }
sowmy87 0:ee786e500a3c 257 wait(0.01);
sowmy87 0:ee786e500a3c 258 int ret=0;
sowmy87 0:ee786e500a3c 259 char c=0;
sowmy87 0:ee786e500a3c 260 while (serial->readable()) {
sowmy87 0:ee786e500a3c 261 // pcc.printf("Reading");
sowmy87 0:ee786e500a3c 262 c=serial->getc();
sowmy87 0:ee786e500a3c 263 // pcc.putc(c);
sowmy87 0:ee786e500a3c 264 if (c>='0'&&c<='9') {
sowmy87 0:ee786e500a3c 265 c-='0';
sowmy87 0:ee786e500a3c 266 ret*=10;
sowmy87 0:ee786e500a3c 267 ret+=c;
sowmy87 0:ee786e500a3c 268 }
sowmy87 0:ee786e500a3c 269 }
sowmy87 0:ee786e500a3c 270 return ret;
sowmy87 0:ee786e500a3c 271 }
sowmy87 0:ee786e500a3c 272 int Serializer::GetCount() {
sowmy87 0:ee786e500a3c 273 return GetCountLeft();
sowmy87 0:ee786e500a3c 274 }
sowmy87 0:ee786e500a3c 275 float Serializer::GetDistanceLeft() {
sowmy87 0:ee786e500a3c 276 return float(GetCountLeft()*PULSE_DISTANCE);
sowmy87 0:ee786e500a3c 277 }
sowmy87 0:ee786e500a3c 278 float Serializer::GetDistanceRight() {
sowmy87 0:ee786e500a3c 279 return float(GetCountRight()*PULSE_DISTANCE);
sowmy87 0:ee786e500a3c 280 }
sowmy87 0:ee786e500a3c 281 float Serializer::GetDistance() {
sowmy87 0:ee786e500a3c 282 return float(GetCountLeft()*PULSE_DISTANCE);
sowmy87 0:ee786e500a3c 283 }
sowmy87 0:ee786e500a3c 284
sowmy87 0:ee786e500a3c 285
sowmy87 0:ee786e500a3c 286 //unused and untested
sowmy87 0:ee786e500a3c 287 int Serializer::GetReply() {
sowmy87 0:ee786e500a3c 288 int returnValue=0;
sowmy87 0:ee786e500a3c 289 char c=0;
sowmy87 0:ee786e500a3c 290 char readyToReturn=0;
sowmy87 0:ee786e500a3c 291 char ack=0;
sowmy87 0:ee786e500a3c 292 char nack=0;
sowmy87 0:ee786e500a3c 293 wait(0.0001);
sowmy87 0:ee786e500a3c 294 while (serial->readable()) {
sowmy87 0:ee786e500a3c 295 c=serial->getc();
sowmy87 0:ee786e500a3c 296 if (readyToReturn)continue;
sowmy87 0:ee786e500a3c 297 if (c=='N'&&nack==0)nack=1;
sowmy87 0:ee786e500a3c 298 else nack=0;
sowmy87 0:ee786e500a3c 299 if (c=='A'&&ack==0)ack++;
sowmy87 0:ee786e500a3c 300 else ack=nack=0;
sowmy87 0:ee786e500a3c 301 if (c=='C'&&ack==1)ack++;
sowmy87 0:ee786e500a3c 302 else ack=nack=0;
sowmy87 0:ee786e500a3c 303 if (c=='K'&&ack==2)ack++;
sowmy87 0:ee786e500a3c 304 else ack=nack=0;
sowmy87 0:ee786e500a3c 305 if (ack==3)
sowmy87 0:ee786e500a3c 306 if (nack==1)
sowmy87 0:ee786e500a3c 307 returnValue=1;
sowmy87 0:ee786e500a3c 308 else
sowmy87 0:ee786e500a3c 309 returnValue=0;
sowmy87 0:ee786e500a3c 310 if (c=='>')readyToReturn=1;
sowmy87 0:ee786e500a3c 311 }
sowmy87 0:ee786e500a3c 312 return returnValue;
sowmy87 0:ee786e500a3c 313 }
sowmy87 0:ee786e500a3c 314
sowmy87 0:ee786e500a3c 315
sowmy87 0:ee786e500a3c 316 void Serializer::InterruptHandler() {
sowmy87 0:ee786e500a3c 317 char c=0;
sowmy87 0:ee786e500a3c 318 char bsy=_isBusy;
sowmy87 0:ee786e500a3c 319 while (serial->readable()) {
sowmy87 0:ee786e500a3c 320 c=serial->getc();
sowmy87 0:ee786e500a3c 321 if (!_isBusy)
sowmy87 0:ee786e500a3c 322 continue;
sowmy87 0:ee786e500a3c 323
sowmy87 0:ee786e500a3c 324 if (c=='1')
sowmy87 0:ee786e500a3c 325 bsy=1;
sowmy87 0:ee786e500a3c 326 else if (c=='0')
sowmy87 0:ee786e500a3c 327 bsy=0;
sowmy87 0:ee786e500a3c 328 }
sowmy87 0:ee786e500a3c 329 if (_isBusy&&!bsy) _isBusy=0;
sowmy87 0:ee786e500a3c 330 }