Diff: SerialServo.h

Revision:

0:7d81654c5ce8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SerialServo.h	Mon Jun 24 07:51:58 2019 +0000
@@ -0,0 +1,138 @@
+typedef struct{
+    int front, rear, count, size;
+    char *buff;
+}Queue;
+
+typedef union{
+    short i;
+    char b[2];
+}int_b;
+
+class SerialServo {
+public:
+    SerialServo(PinName tx, PinName rx);
+    int start(int,int);
+    int readShort(char*, short*);
+    void sendShort(char, short);
+    
+private:  
+    Queue *make_queue(int);
+    bool is_full(Queue *);
+    int get_count(Queue *);
+    bool enqueue(Queue *, char);
+    bool is_empty(Queue *);
+    char dequeue(Queue *, bool *);
+    void enqueueBuff();
+    Queue *_q;
+    RawSerial _servo;
+    Ticker _timer;
+};
+
+SerialServo::SerialServo(PinName tx, PinName rx)
+        :_servo(tx,rx){
+}
+
+int SerialServo::start(int baud_rate, int buff_size)
+{
+    if((_q = make_queue(buff_size)) == NULL)  return -1;
+    _servo.baud(baud_rate);
+    _servo.attach(this, &SerialServo::enqueueBuff, Serial::RxIrq);
+    return 0;
+}
+
+void SerialServo::sendShort(char c, short data)
+{
+    int_b s_data;
+    s_data.i = data;
+    _servo.putc(c);
+    _servo.putc(s_data.b[0]);
+    _servo.putc(s_data.b[1]);
+}
+
+int SerialServo::readShort(char *c, short *data)
+{
+    bool err; 
+    if(get_count(_q) >= sizeof('A') + sizeof(short)){
+        *c = dequeue(_q, &err);
+        if(*c >= 'A' && *c <= 'Z'){
+        int_b x;
+        x.b[0] = dequeue(_q, &err);
+        x.b[1] = dequeue(_q, &err);
+        *data = x.i;
+        return 0;
+        }else readShort(c, data);
+    }
+    return -1;
+}
+
+void SerialServo::enqueueBuff()
+{
+    if(_servo.readable() == 1) SerialServo::enqueue(_q,_servo.getc());
+}
+    
+    
+//********************
+//Bufferに関する関数
+//********************
+
+Queue *SerialServo::make_queue(int n)
+{
+  Queue *que;
+  que = (Queue *)malloc(sizeof(Queue));
+  if (que != NULL) {
+    que->front = 0;
+    que->rear = 0;
+    que->count = 0;
+    que->size = n;
+    que->buff = (char *)malloc(sizeof(char) * n);
+    if (que->buff == NULL) {
+      free(que);
+      return NULL;
+    }
+  }
+  return que;
+}
+
+
+// キューは満杯か
+bool SerialServo::is_full(Queue *que)
+{
+  return que->count == que->size;
+}
+
+// データの挿入
+bool SerialServo::enqueue(Queue *que, char x)
+{
+  if (is_full(que)) return false;
+  que->buff[que->rear++] = x;
+  que->count++;
+  if (que->rear == que->size)
+    que->rear = 0;
+  return true;
+}
+
+// キューは空か
+bool SerialServo::is_empty(Queue *que)
+{
+  return que->count == 0;
+}
+
+int SerialServo::get_count(Queue *que)
+{
+    return que->count;
+}
+
+// データを取り出す
+char SerialServo::dequeue(Queue *que, bool *err)
+{
+  if (is_empty(que)) {
+    *err = false;
+    return 0;
+  }
+  char x = que->buff[que->front++];
+  que->count--;
+  *err = true;
+  if (que->front == que->size)
+    que->front = 0;
+  return x;
+}
\ No newline at end of file