卒研
Fork of ds3_si by
ds3_si.cpp
- Committer:
- fujikenac
- Date:
- 2017-09-12
- Revision:
- 1:3fbf08dc60b6
- Parent:
- 0:4ac4c7635035
- Child:
- 2:fabf8b58b40c
File content as of revision 1:3fbf08dc60b6:
#include "mbed.h" #include "ds3_si.h" ds3_si::ds3_si(PinName tx, PinName rx):serial(tx, rx), pc(USBTX, USBRX) { in[0] = 128; in[1] = 0; in[2] = 0; in[3] = 64; in[4] = 64; in[5] = 64; in[6] = 64; in[7] = 0; serial.baud(2400); serial.attach(this, &ds3_si::serialin, Serial::RxIrq); } ds3_si::ds3_si(PinName tx, PinName rx, int baudrate):serial(tx, rx), pc(USBTX, USBRX) { in[0] = 128; in[1] = 0; in[2] = 0; in[3] = 64; in[4] = 64; in[5] = 64; in[6] = 64; in[7] = 0; serial.baud(baudrate); serial.attach(this, &ds3_si::serialin, Serial::RxIrq); } void ds3_si::serialin() { if(serial.readable()) { while(serial.getc() != 128) {} for(short i = 1; i < 8; i++) { *(in + i) = serial.getc(); } } } bool ds3_si::buttonstate(short order) { return in[(order >> 7) + 1] & order; } int ds3_si::analogstate(short order) { return (order % 2) ? in[order] - 64 : 64 - in[order]; } int ds3_si::getinputdata(short order) { return in[order]; } double ds3_si::getangle(short order) { int x = analogstate(order * 2 + 1); int y = analogstate(order * 2 + 2); if(x || y) return atan2(double(y), double(x)); else return NONE_angle; } void ds3_si::usb_debug() { for(short i = 0; i < 8; i++) pc.printf("%4d", in[i]); pc.printf(" %4.1lf %4.1lf", getangle(L) * 180 / PI, getangle(R) * 180 / PI); if(buttonstate(square)) pc.printf(" 1"); else pc.printf(" 0"); pc.printf("\n"); }