Sowmiya Sivanandham / Serializer

1am

Serializer.cpp@0:d2dcb47dc8e9, 2010-12-12 (annotated)

Committer:
sowmy87
Date:
Sun Dec 12 23:49:02 2010 +0000
Revision:
0:d2dcb47dc8e9
Child:
1:9a7e6c978adb
with GetCoutn

Who changed what in which revision?

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