Koala.cpp

00001 #include "Koala.h"
00002 
00003 Koala::Koala(PinName tx, PinName rx, int baudrate) :  Serial(tx, rx) {
00004 
00005     this->baud(baudrate);
00006     this->format(8,Serial::None,1);
00007     set_speed(0,0);
00008     set_pos_cnt(0,0);
00009 }
00010 
00011 
00012 void Koala::dummyread() {
00013 
00014     char dummy = this->getc();
00015 }
00016 
00017 char Koala::set_speed(int speedL,int speedR) {
00018 
00019     this->printf("D,%d,%d\n",speedL,speedR);
00020     char reply = this->getc();
00021     dummyread();
00022     dummyread();
00023     return reply;
00024 }
00025 
00026 
00027 char Koala::set_pos_cnt(int posL,int posR) {
00028 
00029     this->printf("G,%d,%d\n",posL,posR);
00030     char reply = this->getc();
00031     dummyread();
00032     dummyread();
00033     return reply;
00034 }
00035 
00036 void Koala::read_position(int* nPosL, int* nPosR) {
00037 
00038     int * PosL, * PosR;
00039     PosL = (int*)nPosL;
00040     PosR = (int*)nPosR;
00041 
00042     this->printf("H\n");
00043     this->scanf("h,%d,%d",PosL,PosR);
00044     dummyread();
00045     dummyread();
00046 }
00047 
00048 int Koala::read_channel(int channel) {
00049 
00050     int reply;
00051     this->printf("I,%d\n",channel);
00052     this->scanf("i,%d",&reply);
00053     dummyread();
00054     dummyread();
00055     return reply;
00056 }