takahiro kambara / Mbed 2 deprecated 2016_catch_robo_left

2016 catch robo

Dependencies:   mbed WiiClassicController_kai

SerialServo.h@0:7d81654c5ce8, 2019-06-24 (annotated)

Committer:
kambara1415
Date:
Mon Jun 24 07:51:58 2019 +0000
Revision:
0:7d81654c5ce8
2016 suiheitakannsetu

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kambara1415 0:7d81654c5ce8 1 typedef struct{
kambara1415 0:7d81654c5ce8 2 int front, rear, count, size;
kambara1415 0:7d81654c5ce8 3 char *buff;
kambara1415 0:7d81654c5ce8 4 }Queue;
kambara1415 0:7d81654c5ce8 5
kambara1415 0:7d81654c5ce8 6 typedef union{
kambara1415 0:7d81654c5ce8 7 short i;
kambara1415 0:7d81654c5ce8 8 char b[2];
kambara1415 0:7d81654c5ce8 9 }int_b;
kambara1415 0:7d81654c5ce8 10
kambara1415 0:7d81654c5ce8 11 class SerialServo {
kambara1415 0:7d81654c5ce8 12 public:
kambara1415 0:7d81654c5ce8 13 SerialServo(PinName tx, PinName rx);
kambara1415 0:7d81654c5ce8 14 int start(int,int);
kambara1415 0:7d81654c5ce8 15 int readShort(char*, short*);
kambara1415 0:7d81654c5ce8 16 void sendShort(char, short);
kambara1415 0:7d81654c5ce8 17
kambara1415 0:7d81654c5ce8 18 private:
kambara1415 0:7d81654c5ce8 19 Queue *make_queue(int);
kambara1415 0:7d81654c5ce8 20 bool is_full(Queue *);
kambara1415 0:7d81654c5ce8 21 int get_count(Queue *);
kambara1415 0:7d81654c5ce8 22 bool enqueue(Queue *, char);
kambara1415 0:7d81654c5ce8 23 bool is_empty(Queue *);
kambara1415 0:7d81654c5ce8 24 char dequeue(Queue *, bool *);
kambara1415 0:7d81654c5ce8 25 void enqueueBuff();
kambara1415 0:7d81654c5ce8 26 Queue *_q;
kambara1415 0:7d81654c5ce8 27 RawSerial _servo;
kambara1415 0:7d81654c5ce8 28 Ticker _timer;
kambara1415 0:7d81654c5ce8 29 };
kambara1415 0:7d81654c5ce8 30
kambara1415 0:7d81654c5ce8 31 SerialServo::SerialServo(PinName tx, PinName rx)
kambara1415 0:7d81654c5ce8 32 :_servo(tx,rx){
kambara1415 0:7d81654c5ce8 33 }
kambara1415 0:7d81654c5ce8 34
kambara1415 0:7d81654c5ce8 35 int SerialServo::start(int baud_rate, int buff_size)
kambara1415 0:7d81654c5ce8 36 {
kambara1415 0:7d81654c5ce8 37 if((_q = make_queue(buff_size)) == NULL) return -1;
kambara1415 0:7d81654c5ce8 38 _servo.baud(baud_rate);
kambara1415 0:7d81654c5ce8 39 _servo.attach(this, &SerialServo::enqueueBuff, Serial::RxIrq);
kambara1415 0:7d81654c5ce8 40 return 0;
kambara1415 0:7d81654c5ce8 41 }
kambara1415 0:7d81654c5ce8 42
kambara1415 0:7d81654c5ce8 43 void SerialServo::sendShort(char c, short data)
kambara1415 0:7d81654c5ce8 44 {
kambara1415 0:7d81654c5ce8 45 int_b s_data;
kambara1415 0:7d81654c5ce8 46 s_data.i = data;
kambara1415 0:7d81654c5ce8 47 _servo.putc(c);
kambara1415 0:7d81654c5ce8 48 _servo.putc(s_data.b[0]);
kambara1415 0:7d81654c5ce8 49 _servo.putc(s_data.b[1]);
kambara1415 0:7d81654c5ce8 50 }
kambara1415 0:7d81654c5ce8 51
kambara1415 0:7d81654c5ce8 52 int SerialServo::readShort(char *c, short *data)
kambara1415 0:7d81654c5ce8 53 {
kambara1415 0:7d81654c5ce8 54 bool err;
kambara1415 0:7d81654c5ce8 55 if(get_count(_q) >= sizeof('A') + sizeof(short)){
kambara1415 0:7d81654c5ce8 56 *c = dequeue(_q, &err);
kambara1415 0:7d81654c5ce8 57 if(*c >= 'A' && *c <= 'Z'){
kambara1415 0:7d81654c5ce8 58 int_b x;
kambara1415 0:7d81654c5ce8 59 x.b[0] = dequeue(_q, &err);
kambara1415 0:7d81654c5ce8 60 x.b[1] = dequeue(_q, &err);
kambara1415 0:7d81654c5ce8 61 *data = x.i;
kambara1415 0:7d81654c5ce8 62 return 0;
kambara1415 0:7d81654c5ce8 63 }else readShort(c, data);
kambara1415 0:7d81654c5ce8 64 }
kambara1415 0:7d81654c5ce8 65 return -1;
kambara1415 0:7d81654c5ce8 66 }
kambara1415 0:7d81654c5ce8 67
kambara1415 0:7d81654c5ce8 68 void SerialServo::enqueueBuff()
kambara1415 0:7d81654c5ce8 69 {
kambara1415 0:7d81654c5ce8 70 if(_servo.readable() == 1) SerialServo::enqueue(_q,_servo.getc());
kambara1415 0:7d81654c5ce8 71 }
kambara1415 0:7d81654c5ce8 72
kambara1415 0:7d81654c5ce8 73
kambara1415 0:7d81654c5ce8 74 //********************
kambara1415 0:7d81654c5ce8 75 //Bufferに関する関数
kambara1415 0:7d81654c5ce8 76 //********************
kambara1415 0:7d81654c5ce8 77
kambara1415 0:7d81654c5ce8 78 Queue *SerialServo::make_queue(int n)
kambara1415 0:7d81654c5ce8 79 {
kambara1415 0:7d81654c5ce8 80 Queue *que;
kambara1415 0:7d81654c5ce8 81 que = (Queue *)malloc(sizeof(Queue));
kambara1415 0:7d81654c5ce8 82 if (que != NULL) {
kambara1415 0:7d81654c5ce8 83 que->front = 0;
kambara1415 0:7d81654c5ce8 84 que->rear = 0;
kambara1415 0:7d81654c5ce8 85 que->count = 0;
kambara1415 0:7d81654c5ce8 86 que->size = n;
kambara1415 0:7d81654c5ce8 87 que->buff = (char *)malloc(sizeof(char) * n);
kambara1415 0:7d81654c5ce8 88 if (que->buff == NULL) {
kambara1415 0:7d81654c5ce8 89 free(que);
kambara1415 0:7d81654c5ce8 90 return NULL;
kambara1415 0:7d81654c5ce8 91 }
kambara1415 0:7d81654c5ce8 92 }
kambara1415 0:7d81654c5ce8 93 return que;
kambara1415 0:7d81654c5ce8 94 }
kambara1415 0:7d81654c5ce8 95
kambara1415 0:7d81654c5ce8 96
kambara1415 0:7d81654c5ce8 97 // キューは満杯か
kambara1415 0:7d81654c5ce8 98 bool SerialServo::is_full(Queue *que)
kambara1415 0:7d81654c5ce8 99 {
kambara1415 0:7d81654c5ce8 100 return que->count == que->size;
kambara1415 0:7d81654c5ce8 101 }
kambara1415 0:7d81654c5ce8 102
kambara1415 0:7d81654c5ce8 103 // データの挿入
kambara1415 0:7d81654c5ce8 104 bool SerialServo::enqueue(Queue *que, char x)
kambara1415 0:7d81654c5ce8 105 {
kambara1415 0:7d81654c5ce8 106 if (is_full(que)) return false;
kambara1415 0:7d81654c5ce8 107 que->buff[que->rear++] = x;
kambara1415 0:7d81654c5ce8 108 que->count++;
kambara1415 0:7d81654c5ce8 109 if (que->rear == que->size)
kambara1415 0:7d81654c5ce8 110 que->rear = 0;
kambara1415 0:7d81654c5ce8 111 return true;
kambara1415 0:7d81654c5ce8 112 }
kambara1415 0:7d81654c5ce8 113
kambara1415 0:7d81654c5ce8 114 // キューは空か
kambara1415 0:7d81654c5ce8 115 bool SerialServo::is_empty(Queue *que)
kambara1415 0:7d81654c5ce8 116 {
kambara1415 0:7d81654c5ce8 117 return que->count == 0;
kambara1415 0:7d81654c5ce8 118 }
kambara1415 0:7d81654c5ce8 119
kambara1415 0:7d81654c5ce8 120 int SerialServo::get_count(Queue *que)
kambara1415 0:7d81654c5ce8 121 {
kambara1415 0:7d81654c5ce8 122 return que->count;
kambara1415 0:7d81654c5ce8 123 }
kambara1415 0:7d81654c5ce8 124
kambara1415 0:7d81654c5ce8 125 // データを取り出す
kambara1415 0:7d81654c5ce8 126 char SerialServo::dequeue(Queue *que, bool *err)
kambara1415 0:7d81654c5ce8 127 {
kambara1415 0:7d81654c5ce8 128 if (is_empty(que)) {
kambara1415 0:7d81654c5ce8 129 *err = false;
kambara1415 0:7d81654c5ce8 130 return 0;
kambara1415 0:7d81654c5ce8 131 }
kambara1415 0:7d81654c5ce8 132 char x = que->buff[que->front++];
kambara1415 0:7d81654c5ce8 133 que->count--;
kambara1415 0:7d81654c5ce8 134 *err = true;
kambara1415 0:7d81654c5ce8 135 if (que->front == que->size)
kambara1415 0:7d81654c5ce8 136 que->front = 0;
kambara1415 0:7d81654c5ce8 137 return x;
kambara1415 0:7d81654c5ce8 138 }