Diff: Koala.cpp

Revision:

0:5d64dc944fa6

Child:

1:0006a3271fa2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Koala.cpp	Tue Jun 29 11:33:00 2010 +0000
@@ -0,0 +1,56 @@
+#include "Koala.h"
+
+Koala::Koala(PinName tx, PinName rx, int baudrate) :  _serial(tx, rx){
+
+    _serial.baud(baudrate);
+    _serial.format(8,Serial::None,1);
+    set_speed(0,0);
+    set_pos_cnt(0,0);
+}
+
+
+void Koala::dummyread(){
+
+    char dummy = _serial.getc();
+}
+
+char Koala::set_speed(int speedL,int speedR) {
+
+    _serial.printf("D,%d,%d\n",speedL,speedR);
+    char reply = _serial.getc();
+    dummyread();
+    dummyread();
+    return reply;
+}
+
+
+char Koala::set_pos_cnt(int posL,int posR) {
+
+    _serial.printf("G,%d,%d\n",posL,posR);
+    char reply = _serial.getc();
+    dummyread();
+    dummyread();
+    return reply;
+}
+
+void Koala::read_position(int* nPosL, int* nPosR) {
+
+    int * PosL, * PosR;
+    PosL = (int*)nPosL;
+    PosR = (int*)nPosR;
+
+    _serial.printf("H\n");
+    _serial.scanf("h,%d,%d",PosL,PosR);
+    dummyread();
+    dummyread();
+}
+
+int Koala::read_channel(int channel) {
+
+    int reply;
+    _serial.printf("I,%d\n",channel);
+    _serial.scanf("i,%d",&reply);
+    dummyread();
+    dummyread();
+    return reply;
+}