Serializer.cpp

00001 /**
00002  * Includes
00003  */
00004 #include "Serializer.h"
00005 
00006 //Serial pcc(USBTX, USBRX);
00007 
00008 Serializer::Serializer() {
00009     serial=new Serial(p28,p27);
00010     serial->baud(BAUD);
00011     Initialize();
00012 }
00013 
00014 int Serializer::Initialize() {
00015     char c1=0;
00016     DigitalOut led1(LED1);
00017     DigitalOut led2(LED2);
00018     DigitalOut led3(LED3);
00019     DigitalOut led4(LED4);
00020 
00021     led1=1;
00022     if (serial->writeable())
00023         serial->printf("fw\r");
00024     wait(.25);
00025     led2=1;
00026     while (serial->readable()) {
00027         c1=serial->getc();
00028     }
00029     if (c1!='>')
00030         for (int i=0;i<5;i++) {
00031             led1=led2=led3=led4=1;
00032             wait(0.1);
00033             led1=led2=led3=led4=0;
00034             wait(0.1);
00035         }
00036     wait(.25);
00037     led3=1;
00038     wait(.25);
00039     led4=1;
00040  //   SetVPID(1,0,0,100);
00041     wait(0.25);
00042     led1=0;
00043     wait(.25);
00044     ClearCount();
00045     led2=0;
00046     wait(.25);
00047     led3=0;
00048     wait(.25);
00049     led4=0;
00050     _lPWM=_rPWM=0;
00051     _isBusy=0;
00052     _irqIsBusy=0;
00053 //    serial->attach(this, &Serializer::InterruptHandler);
00054 
00055     return 0;
00056 }
00057 
00058 Serializer::~Serializer() {
00059     delete serial;
00060 }
00061 
00062 void Serializer::Stop() {
00063     if (serial->writeable())
00064         serial->printf("stop\r");
00065     leftSpeed=rightSpeed=0;
00066 //    pc.printf("Stop\r\n");
00067 }
00068 
00069 void Serializer::ClearCountLeft() {
00070     if (serial->writeable())
00071         serial->printf("clrenc 1\r");
00072 }
00073 void Serializer::ClearCountRight() {
00074     if (serial->writeable())
00075         serial->printf("clrenc 2\r");
00076 }
00077 
00078 void Serializer::ClearCount() {
00079     if (serial->writeable())
00080         serial->printf("clrenc 1 2\r");
00081 }
00082 
00083 void Serializer::SetSpeedLeft(int inPsec) {
00084     if (inPsec>MAX_SPEED)inPsec=MAX_SPEED;
00085     if (inPsec<-MAX_SPEED)inPsec=-MAX_SPEED;
00086     if (serial->writeable())
00087         serial->printf("mogo 1:%i\r", -inPsec);
00088     leftSpeed=inPsec;
00089 }
00090 void Serializer::SetSpeedRight(int inPsec) {
00091     if (inPsec>MAX_SPEED)inPsec=MAX_SPEED;
00092     if (inPsec<-MAX_SPEED)inPsec=-MAX_SPEED;
00093     if (serial->writeable())
00094         serial->printf("mogo 2:%i\r", -inPsec);
00095     rightSpeed=inPsec;
00096 }
00097 
00098 void Serializer::SetSpeed(int inPsec) {
00099     if (inPsec>MAX_SPEED)inPsec=MAX_SPEED;
00100     if (inPsec<-MAX_SPEED)inPsec=-MAX_SPEED;
00101     inPsec*=-1;
00102     if (serial->writeable())
00103         serial->printf("mogo 1:%i 2:%i\r", inPsec-50, inPsec);
00104     leftSpeed=rightSpeed=inPsec;
00105 }
00106 
00107 void Serializer::SetVPID(int p,int i,int d,int l) {
00108     if (serial->writeable())
00109         serial->printf("vpid %i:%i:%i:%i\r", p,i,d,l);
00110 }
00111 
00112 void Serializer::SetDPID(int p,int i,int d,int a) {
00113     if (serial->writeable())
00114         serial->printf("dpid %i:%i:%i:%i\r", p,i,d,a);
00115 }
00116 
00117 void Serializer::DiGoLeft(int dist,int inPsec) {
00118 /*    if(_isBusy)
00119         return;
00120     _isBusy=1;
00121 */
00122     if (serial->writeable())
00123         serial->printf("digo 1:%i:%i\r",dist*PULSES_PER_INCH, inPsec);
00124 }
00125 void Serializer::DiGoRight(int dist,int inPsec) {
00126 /*     if(_isBusy)
00127         return;
00128     _isBusy=1;
00129 */
00130     if (serial->writeable())
00131         serial->printf("digo 2:%i:%i\r",dist*PULSES_PER_INCH, inPsec);
00132 }
00133 void Serializer::DiGo(int dist,int inPsec) {
00134 /*    if(_isBusy)
00135         return;
00136     _isBusy=1;
00137 */
00138     if (serial->writeable()) {
00139         serial->printf("digo 1:%i:%i 2:%i:%i\r", \
00140                        -dist*PULSES_PER_INCH,   \
00141                        inPsec,                  \
00142                        -dist*PULSES_PER_INCH,   \
00143                        inPsec);
00144 
00145     }
00146 }
00147 
00148 int Serializer::IsBusy() {
00149 
00150     if (_isBusy&&serial->writeable()) {
00151         serial->printf("pids\r");
00152         wait(0.01);
00153     }
00154     return _isBusy;
00155 }
00156 /*
00157       char tmp=0;
00158         if (_isBusy) {
00159             if (serial->writeable()) {
00160                 serial->printf("pids\r");
00161             }
00162             wait(0.1);
00163             while (serial->readable()) {
00164                 tmp=serial->getc();
00165                 pcc.putc(tmp);
00166                 if (tmp=='1')
00167                     _isBusy=1;
00168                 else if (tmp=='0')
00169                     _isBusy=0;
00170             }
00171         }
00172         pcc.printf("IsBusy = %i\n\r",_isBusy);
00173         return _isBusy;
00174 */
00175 
00176 
00177 
00178 void Serializer::TurnLeft(int deg) {
00179 
00180 }
00181 
00182 void Serializer::TurnRight(int deg) {
00183 
00184 
00185 }
00186 void Serializer::PivetLeft(int deg) {
00187 
00188     Stop();
00189     deg=deg*PIVET_ADJUSTMENT;
00190     wait(0.1);
00191     if (serial->writeable())
00192         serial->printf("digo 1:%i:%i 2:%i:%i\r",deg,PIVET_SPEED,-deg,PIVET_SPEED);
00193 
00194 }
00195 void Serializer::PivetRight(int deg) {
00196     this->Stop();
00197     deg=deg*PIVET_ADJUSTMENT;
00198     wait(0.1);
00199     if (serial->writeable())
00200         serial->printf("digo 1:%i:%i 2:%i:%i\r",-deg,PIVET_SPEED,deg,PIVET_SPEED);
00201 
00202 }
00203 
00204 void Serializer::SetLeftPWM(int pwm) {
00205     _lPWM=-pwm;
00206     if (serial->writeable())
00207         serial->printf("pwm 1:%i\r",_lPWM);
00208 
00209 }
00210 
00211 void Serializer::SetRightPWM(int pwm) {
00212     _rPWM=-pwm;
00213     if (serial->writeable())
00214         serial->printf("pwm 2:%i\r",_rPWM);
00215 }
00216 
00217 void Serializer::SetPWM(int pwm){
00218     _rPWM=-pwm;
00219     _lPWM=-pwm;
00220     if (serial->writeable())
00221         serial->printf("pwm 1:%i 2:%i\r",_lPWM, _rPWM);      
00222  }
00223 
00224 void Serializer::SetPWM(int lPwm, int rPwm){
00225     _rPWM=-rPwm;
00226     _lPWM=-lPwm;
00227     if (serial->writeable())
00228         serial->printf("pwm 1:%i 2:%i\r",_lPWM, _rPWM);      
00229  }
00230 
00231 int Serializer::GetCountLeft() {
00232     if (serial->writeable()) {
00233         serial->printf("getenc 1\r");
00234         //        pcc.printf("Sendreq\t");
00235     }
00236     wait(0.01);
00237     int ret=0;
00238     char c=0;
00239     while (serial->readable()) {
00240         //        pcc.printf("Reading");
00241         c=serial->getc();
00242         //        pcc.putc(c);
00243         if (c>='0'&&c<='9') {
00244             c-='0';
00245             ret*=10;
00246             ret+=c;
00247         }
00248     }
00249     return ret;
00250 }
00251 
00252 int Serializer::GetCountRight() {
00253     if (serial->writeable()) {
00254         serial->printf("getenc 2\r");
00255         //        pcc.printf("Sendreq\t");
00256     }
00257     wait(0.01);
00258     int ret=0;
00259     char c=0;
00260     while (serial->readable()) {
00261         //        pcc.printf("Reading");
00262         c=serial->getc();
00263         //        pcc.putc(c);
00264         if (c>='0'&&c<='9') {
00265             c-='0';
00266             ret*=10;
00267             ret+=c;
00268         }
00269     }
00270     return ret;
00271 }
00272 int Serializer::GetCount() {
00273     return GetCountLeft();
00274 }
00275 float Serializer::GetDistanceLeft() {
00276     return float(GetCountLeft()*PULSE_DISTANCE);
00277 }
00278 float Serializer::GetDistanceRight() {
00279     return float(GetCountRight()*PULSE_DISTANCE);
00280 }
00281 float Serializer::GetDistance() {
00282     return float(GetCountLeft()*PULSE_DISTANCE);
00283 }
00284 
00285 
00286 //unused and untested
00287 int Serializer::GetReply() {
00288     int returnValue=0;
00289     char c=0;
00290     char readyToReturn=0;
00291     char ack=0;
00292     char nack=0;
00293     wait(0.0001);
00294     while (serial->readable()) {
00295         c=serial->getc();
00296         if (readyToReturn)continue;
00297         if (c=='N'&&nack==0)nack=1;
00298         else nack=0;
00299         if (c=='A'&&ack==0)ack++;
00300         else ack=nack=0;
00301         if (c=='C'&&ack==1)ack++;
00302         else ack=nack=0;
00303         if (c=='K'&&ack==2)ack++;
00304         else ack=nack=0;
00305         if (ack==3)
00306             if (nack==1)
00307                 returnValue=1;
00308             else
00309                 returnValue=0;
00310         if (c=='>')readyToReturn=1;
00311     }
00312     return returnValue;
00313 }
00314 
00315 
00316 void Serializer::InterruptHandler() {
00317     char c=0;
00318     char bsy=_isBusy;
00319     while (serial->readable()) {
00320         c=serial->getc();
00321         if (!_isBusy)
00322             continue;
00323 
00324         if (c=='1')
00325             bsy=1;
00326         else if (c=='0')
00327             bsy=0;
00328     }
00329     if (_isBusy&&!bsy) _isBusy=0;
00330 }