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