Pavel Rybakov / Serializer

Serializer.cpp@3:cc2952c60d85, 2010-12-12 (annotated)

Committer:
gtg795y
Date:
Sun Dec 12 02:40:26 2010 +0000
Revision:
3:cc2952c60d85
Parent:
2:0a3c369d0076
Child:
4:959812c02a0c
0.001x

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gtg795y 2:0a3c369d0076 1 /**
gtg795y 2:0a3c369d0076 2 * Includes
gtg795y 2:0a3c369d0076 3 */
gtg795y 1:d0f6e726b244 4 #include "Serializer.h"
gtg795y 1:d0f6e726b244 5
gtg795y 3:cc2952c60d85 6 Serial pcc(USBTX, USBRX);
gtg795y 1:d0f6e726b244 7
gtg795y 1:d0f6e726b244 8 Serializer::Serializer() {
gtg795y 3:cc2952c60d85 9 Initialize();
gtg795y 3:cc2952c60d85 10 }
gtg795y 3:cc2952c60d85 11
gtg795y 3:cc2952c60d85 12 int Serializer::Initialize() {
gtg795y 3:cc2952c60d85 13 char c1=0;
gtg795y 3:cc2952c60d85 14 DigitalOut led1(LED1);
gtg795y 3:cc2952c60d85 15 DigitalOut led2(LED2);
gtg795y 3:cc2952c60d85 16 DigitalOut led3(LED3);
gtg795y 3:cc2952c60d85 17 DigitalOut led4(LED4);
gtg795y 1:d0f6e726b244 18 serial=new Serial(p28,p27);
gtg795y 3:cc2952c60d85 19 serial->baud(9600);
gtg795y 3:cc2952c60d85 20
gtg795y 3:cc2952c60d85 21 commandInProgress=1;
gtg795y 3:cc2952c60d85 22 led1=1;
gtg795y 3:cc2952c60d85 23 if (serial->writeable())
gtg795y 3:cc2952c60d85 24 serial->printf("fw\r");
gtg795y 3:cc2952c60d85 25 wait(.25);
gtg795y 3:cc2952c60d85 26 led2=1;
gtg795y 3:cc2952c60d85 27 while (serial->readable()) {
gtg795y 3:cc2952c60d85 28 c1=serial->getc();
gtg795y 3:cc2952c60d85 29 pcc.putc(c1);
gtg795y 3:cc2952c60d85 30 }
gtg795y 3:cc2952c60d85 31 if (c1!='>')
gtg795y 3:cc2952c60d85 32 while (1) {
gtg795y 3:cc2952c60d85 33 led1=led2=led3=led4=1;
gtg795y 3:cc2952c60d85 34 wait(0.5);
gtg795y 3:cc2952c60d85 35 led1=led2=led3=led4=0;
gtg795y 3:cc2952c60d85 36 wait(0.5);
gtg795y 3:cc2952c60d85 37 }
gtg795y 3:cc2952c60d85 38 wait(.25);
gtg795y 3:cc2952c60d85 39 led3=1;
gtg795y 3:cc2952c60d85 40 wait(.25);
gtg795y 3:cc2952c60d85 41 led4=1;
gtg795y 3:cc2952c60d85 42 wait(0.25);
gtg795y 3:cc2952c60d85 43 led1=0;
gtg795y 3:cc2952c60d85 44 wait(.25);
gtg795y 3:cc2952c60d85 45 led2=0;
gtg795y 3:cc2952c60d85 46 wait(.25);
gtg795y 3:cc2952c60d85 47 led3=0;
gtg795y 3:cc2952c60d85 48 wait(.25);
gtg795y 3:cc2952c60d85 49 led4=0;
gtg795y 3:cc2952c60d85 50
gtg795y 3:cc2952c60d85 51 serial->attach(this, &Serializer::InterruptHandler);
gtg795y 3:cc2952c60d85 52 commandInProgress=0;
gtg795y 3:cc2952c60d85 53
gtg795y 3:cc2952c60d85 54 return 0;
gtg795y 1:d0f6e726b244 55 }
gtg795y 1:d0f6e726b244 56
gtg795y 1:d0f6e726b244 57 Serializer::~Serializer() {
gtg795y 1:d0f6e726b244 58 delete serial;
gtg795y 1:d0f6e726b244 59 }
gtg795y 1:d0f6e726b244 60
gtg795y 1:d0f6e726b244 61 void Serializer::Stop() {
gtg795y 3:cc2952c60d85 62 while (commandInProgress) {}
gtg795y 1:d0f6e726b244 63 if (serial->writeable())
gtg795y 1:d0f6e726b244 64 serial->printf("stop\r");
gtg795y 3:cc2952c60d85 65 leftSpeed=rightSpeed=0;
gtg795y 3:cc2952c60d85 66 // pc.printf("Stop\r\n");
gtg795y 1:d0f6e726b244 67 }
gtg795y 1:d0f6e726b244 68
gtg795y 1:d0f6e726b244 69 void Serializer::ClearCountLeft() {
gtg795y 3:cc2952c60d85 70 while (commandInProgress) {}
gtg795y 1:d0f6e726b244 71 if (serial->writeable())
gtg795y 1:d0f6e726b244 72 serial->printf("clrenc 1\r");
gtg795y 1:d0f6e726b244 73 }
gtg795y 1:d0f6e726b244 74 void Serializer::ClearCountRight() {
gtg795y 3:cc2952c60d85 75 while (commandInProgress) {}
gtg795y 1:d0f6e726b244 76 if (serial->writeable())
gtg795y 1:d0f6e726b244 77 serial->printf("clrenc 2\r");
gtg795y 1:d0f6e726b244 78 }
gtg795y 1:d0f6e726b244 79
gtg795y 1:d0f6e726b244 80 void Serializer::ClearCount() {
gtg795y 3:cc2952c60d85 81 while (commandInProgress) {}
gtg795y 1:d0f6e726b244 82 if (serial->writeable())
gtg795y 1:d0f6e726b244 83 serial->printf("clrenc 1 2\r");
gtg795y 1:d0f6e726b244 84 }
gtg795y 1:d0f6e726b244 85
gtg795y 3:cc2952c60d85 86 void Serializer::SetSpeedLeft(int inPsec) {
gtg795y 3:cc2952c60d85 87 while (commandInProgress) {}
gtg795y 1:d0f6e726b244 88 if (serial->writeable())
gtg795y 3:cc2952c60d85 89 serial->printf("mogo 1:%i\r", inPsec);//int(-inPsec*PULSES_PER_INCH));
gtg795y 3:cc2952c60d85 90 leftSpeed=inPsec;
gtg795y 1:d0f6e726b244 91 }
gtg795y 3:cc2952c60d85 92 void Serializer::SetSpeedRight(int inPsec) {
gtg795y 3:cc2952c60d85 93 while (commandInProgress) {}
gtg795y 1:d0f6e726b244 94 if (serial->writeable())
gtg795y 3:cc2952c60d85 95 serial->printf("mogo 2:%i\r", -inPsec*PULSES_PER_INCH);
gtg795y 3:cc2952c60d85 96 rightSpeed=inPsec;
gtg795y 1:d0f6e726b244 97 }
gtg795y 3:cc2952c60d85 98 void Serializer::SetSpeed(int inPsec) {
gtg795y 3:cc2952c60d85 99 while (commandInProgress) {}
gtg795y 1:d0f6e726b244 100 if (serial->writeable())
gtg795y 3:cc2952c60d85 101 serial->printf("mogo 1:%i 2:%i\r", inPsec*PULSES_PER_INCH, inPsec*PULSES_PER_INCH);
gtg795y 3:cc2952c60d85 102 leftSpeed=rightSpeed=inPsec;
gtg795y 1:d0f6e726b244 103 }
gtg795y 1:d0f6e726b244 104
gtg795y 1:d0f6e726b244 105 void Serializer::SetVPID(int p,int i,int d,int l) {
gtg795y 3:cc2952c60d85 106 while (commandInProgress) {}
gtg795y 1:d0f6e726b244 107 if (serial->writeable())
gtg795y 1:d0f6e726b244 108 serial->printf("vpid %i:%i:%i:%i\r", p,i,d,l);
gtg795y 1:d0f6e726b244 109 }
gtg795y 1:d0f6e726b244 110
gtg795y 1:d0f6e726b244 111 void Serializer::SetDPID(int p,int i,int d,int a) {
gtg795y 3:cc2952c60d85 112 while (commandInProgress) {}
gtg795y 1:d0f6e726b244 113 if (serial->writeable())
gtg795y 1:d0f6e726b244 114 serial->printf("dpid %i:%i:%i:%i\r", p,i,d,a);
gtg795y 1:d0f6e726b244 115 }
gtg795y 1:d0f6e726b244 116
gtg795y 3:cc2952c60d85 117 void Serializer::DiGoLeft(int dist,int inPsec) {
gtg795y 3:cc2952c60d85 118 while (commandInProgress) {}
gtg795y 1:d0f6e726b244 119 if (serial->writeable())
gtg795y 3:cc2952c60d85 120 serial->printf("digo 1:%i:%i\r",-dist, inPsec*PULSES_PER_INCH);
gtg795y 1:d0f6e726b244 121 }
gtg795y 3:cc2952c60d85 122 void Serializer::DiGoRight(int dist,int inPsec) {
gtg795y 3:cc2952c60d85 123 while (commandInProgress) {}
gtg795y 1:d0f6e726b244 124 if (serial->writeable())
gtg795y 3:cc2952c60d85 125 serial->printf("digo 2:%i:%i\r",-dist*PULSES_PER_INCH, inPsec*PULSES_PER_INCH);
gtg795y 1:d0f6e726b244 126 }
gtg795y 3:cc2952c60d85 127 void Serializer::DiGo(int dist,int inPsec) {
gtg795y 3:cc2952c60d85 128 while (commandInProgress) {}
gtg795y 3:cc2952c60d85 129 if (serial->writeable()) {
gtg795y 3:cc2952c60d85 130 serial->printf("digo 1:%f:%f 2:%f:%f\r", \
gtg795y 3:cc2952c60d85 131 -dist*PULSES_PER_INCH, \
gtg795y 3:cc2952c60d85 132 inPsec*PULSES_PER_INCH, \
gtg795y 3:cc2952c60d85 133 -dist*PULSES_PER_INCH, \
gtg795y 3:cc2952c60d85 134 inPsec*PULSES_PER_INCH);
gtg795y 3:cc2952c60d85 135 /* pc.printf("digo 1:%f:%f 2:%f:%f\r", \
gtg795y 3:cc2952c60d85 136 -dist*PULSES_PER_INCH, \
gtg795y 3:cc2952c60d85 137 inPsec*PULSES_PER_INCH, \
gtg795y 3:cc2952c60d85 138 -dist*PULSES_PER_INCH, \
gtg795y 3:cc2952c60d85 139 inPsec*PULSES_PER_INCH); */
gtg795y 3:cc2952c60d85 140 }
gtg795y 1:d0f6e726b244 141 }
gtg795y 1:d0f6e726b244 142
gtg795y 3:cc2952c60d85 143 void Serializer::TurnLeft(int deg) {
gtg795y 3:cc2952c60d85 144 while (commandInProgress) {}
gtg795y 3:cc2952c60d85 145
gtg795y 3:cc2952c60d85 146 }
gtg795y 3:cc2952c60d85 147
gtg795y 3:cc2952c60d85 148 void Serializer::TurnRight(int deg) {
gtg795y 3:cc2952c60d85 149 while (commandInProgress) {}
gtg795y 3:cc2952c60d85 150
gtg795y 3:cc2952c60d85 151 }
gtg795y 3:cc2952c60d85 152 void Serializer::PivetLeft(int deg) {
gtg795y 3:cc2952c60d85 153 while (commandInProgress) {}
gtg795y 3:cc2952c60d85 154 this->Stop();
gtg795y 3:cc2952c60d85 155 deg=deg*PIVET_ADJUSTMENT + PIVET_ADJUSTMENT*10;
gtg795y 3:cc2952c60d85 156 wait(0.1);
gtg795y 3:cc2952c60d85 157 if (serial->writeable())
gtg795y 3:cc2952c60d85 158 serial->printf("digo 1:%i:%i 2:%i:%i\r",deg,PIVET_SPEED,-deg,PIVET_SPEED);
gtg795y 3:cc2952c60d85 159
gtg795y 3:cc2952c60d85 160 }
gtg795y 3:cc2952c60d85 161 void Serializer::PivetRight(int deg) {
gtg795y 3:cc2952c60d85 162 while (commandInProgress) {}
gtg795y 3:cc2952c60d85 163 this->Stop();
gtg795y 3:cc2952c60d85 164 deg=deg*PIVET_ADJUSTMENT + PIVET_ADJUSTMENT*10;
gtg795y 3:cc2952c60d85 165 wait(0.1);
gtg795y 3:cc2952c60d85 166 if (serial->writeable())
gtg795y 3:cc2952c60d85 167 serial->printf("digo 1:%i:%i 2:%i:%i\r",-deg,PIVET_SPEED,deg,PIVET_SPEED);
gtg795y 3:cc2952c60d85 168
gtg795y 3:cc2952c60d85 169 }
gtg795y 1:d0f6e726b244 170
gtg795y 1:d0f6e726b244 171
gtg795y 1:d0f6e726b244 172 //unused and untested
gtg795y 1:d0f6e726b244 173 int Serializer::GetReply() {
gtg795y 1:d0f6e726b244 174 int returnValue=0;
gtg795y 1:d0f6e726b244 175 char c=0;
gtg795y 1:d0f6e726b244 176 char readyToReturn=0;
gtg795y 1:d0f6e726b244 177 char ack=0;
gtg795y 1:d0f6e726b244 178 char nack=0;
gtg795y 1:d0f6e726b244 179 wait(0.0001);
gtg795y 1:d0f6e726b244 180 while (serial->readable()) {
gtg795y 1:d0f6e726b244 181 c=serial->getc();
gtg795y 3:cc2952c60d85 182 if (readyToReturn)continue;
gtg795y 1:d0f6e726b244 183 if (c=='N'&&nack==0)nack=1;
gtg795y 1:d0f6e726b244 184 else nack=0;
gtg795y 1:d0f6e726b244 185 if (c=='A'&&ack==0)ack++;
gtg795y 1:d0f6e726b244 186 else ack=nack=0;
gtg795y 1:d0f6e726b244 187 if (c=='C'&&ack==1)ack++;
gtg795y 1:d0f6e726b244 188 else ack=nack=0;
gtg795y 1:d0f6e726b244 189 if (c=='K'&&ack==2)ack++;
gtg795y 1:d0f6e726b244 190 else ack=nack=0;
gtg795y 1:d0f6e726b244 191 if (ack==3)
gtg795y 1:d0f6e726b244 192 if (nack==1)
gtg795y 1:d0f6e726b244 193 returnValue=1;
gtg795y 1:d0f6e726b244 194 else
gtg795y 1:d0f6e726b244 195 returnValue=0;
gtg795y 3:cc2952c60d85 196 if (c=='>')readyToReturn=1;
gtg795y 1:d0f6e726b244 197 }
gtg795y 1:d0f6e726b244 198 return returnValue;
gtg795y 1:d0f6e726b244 199 }
gtg795y 3:cc2952c60d85 200
gtg795y 3:cc2952c60d85 201
gtg795y 3:cc2952c60d85 202 void Serializer::InterruptHandler() {
gtg795y 3:cc2952c60d85 203 char c='0';
gtg795y 3:cc2952c60d85 204 while (serial->readable()) {
gtg795y 3:cc2952c60d85 205 c=serial->getc();
gtg795y 3:cc2952c60d85 206 if (c=='>')
gtg795y 3:cc2952c60d85 207 commandInProgress=0;
gtg795y 3:cc2952c60d85 208 pcc.putc(c);
gtg795y 3:cc2952c60d85 209 }
gtg795y 3:cc2952c60d85 210 }