2019
Dependents: wheel_test6 wheel_test7 2019NHK_A_manual_red 2019NHK_A_manual_red
OmniPosition.cpp
- Committer:
- tanabe2000
- Date:
- 2018-10-12
- Revision:
- 12:edd4217ad7a5
- Parent:
- 9:222f7fcbd05a
- Child:
- 13:913b647071a8
File content as of revision 12:edd4217ad7a5:
#include "OmniPosition.h" OmniPosition::OmniPosition(PinName serialTX, PinName serialRX) : serial(serialTX, serialRX) { // thread.start(callback(this, &OmniPosition::assembleLoop)); serial.baud(115200); serial.setHeaders(FIRST_HEDDER, SECOND_HEDDER); serial.startReceive(BUFFER_SIZE); } void OmniPosition::assembleLoop() { while(true) { serial.getData(rxdata); for(int i = 0; i < TWO_BYTE_DATA - 1; i++)data[i] = ((rxdata[2*i]<<8)|rxdata[2*i+1]) - 32768; data[2] = (rxdata[2*2] & 0xFF) | ((rxdata[2*2+1] << 8) & 0xFF00); X = data[0]; Y = data[1]; theta = data[2]; } } int16_t OmniPosition::getX() { serial.getData(rxdata); for(int i = 0; i < TWO_BYTE_DATA - 1; i++)data[i] = ((rxdata[2*i]<<8)|rxdata[2*i+1]) - 32768; data[2] = (rxdata[2*2] & 0xFF) | ((rxdata[2*2+1] << 8) & 0xFF00); X = data[0]; Y = data[1]; theta = data[2]; return X; } int16_t OmniPosition::getY() { return Y; } float OmniPosition::getTheta() { return (float)(theta / 100.0); } void OmniPosition::reset() { resetSend = true; if(resetSend) { // putc('R'); resetSend = false; } else { //putc(0); } }