Pavel Rybakov / Serializer

Serializer.cpp@4:959812c02a0c, 2010-12-12 (annotated)

Committer:
gtg795y
Date:
Sun Dec 12 22:12:13 2010 +0000
Revision:
4:959812c02a0c
Parent:
3:cc2952c60d85 
raw

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