Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: QEI accelerator bit_test cyclic_io cyclic_var cylinder event_var limit mbed mecanum motor_drive pid pid_encoder rs422_put sbdbt servo
Fork of 17robo_Practice1 by
headerfile_unuse/sbdbt_.h
- Committer:
- echo_piyo
- Date:
- 2017-09-28
- Revision:
- 77:17469192b53a
- Parent:
- 66:1664ee92539d
File content as of revision 77:17469192b53a:
/*
ver1.0
Sbdbt Name(tx,rx)
sbdbtに接続するtx,rx
ver2.0
Sbdbt Name(tx,rx)
sbdbtに接続するtx,rx
begin(baudrate)
sbdbtと通信するbaudrateを決定
ボタンデータ 0/1
shikaku, l1, l2, r1, r2, start, select,
up, down, right, left, sankaku, batu, maru
アナログスティックデータ -1 ~ 1
left_x, left_y, right_x, right_y;
sbdbtから受信したデータ
open_data[data_byte];
*/
#define start_byte 0b10000000
#define Shikaku 0
#define L1 1
#define L2 2
#define R1 3
#define R2 4
#define Start 0b00000011
#define Select 0b00001100
#define Up 0
#define Down 1
#define Right 2
#define Left 3
#define Sankaku 4
#define Batu 5
#define Maru 6
#define EvenNeutral 0b01000000
#define OddNeutral 0b00111111
#define data_byte 8
#define input_byte 7
class Sbdbt
{
public :
Sbdbt(PinName mbed_tx, PinName mbed_rx, PinName pin_pairing) : SBDBT(mbed_tx,mbed_rx), pairing(pin_pairing){
}
void begin(long baudrate) {
init();
SBDBT.baud(baudrate);
SBDBT.attach(this, &Sbdbt::data_receive, Serial::RxIrq);
}
int get_pairingState(){
return pairing;
}
short shikaku;
short l1;
short l2;
short r1;
short r2;
short start;
short select;
short up;
short down;
short right;
short left;
short sankaku;
short batu;
short maru;
float left_x;
float left_y;
float right_x;
float right_y;
int open_data[data_byte];
private :
Serial SBDBT;
DigitalIn pairing;
int read, data_start, checksum, byte,
buffer[data_byte], data[data_byte];
int Left_X, Left_Y, Right_X, Right_Y ;
void init() {
open_data[0] = buffer[0] = data[0] = 128;
open_data[1] = buffer[1] = data[1] = 0;
open_data[2] = buffer[2] = data[2] = 0;
open_data[3] = buffer[3] = data[3] = 64;
open_data[4] = buffer[4] = data[4] = 64;
open_data[5] = buffer[5] = data[5] = 64;
open_data[6] = buffer[6] = data[6] = 64;
open_data[7] = buffer[7] = data[7] = 0;
}
void check() {
if (bit_test(data[1],Shikaku)) {
shikaku = 1;
} else {
shikaku = 0;
}
if (bit_test(data[1],L1)) {
l1 = 1;
} else {
l1 = 0;
}
if (bit_test(data[1],L2)) {
l2 = 1;
} else {
l2 = 0;
}
if (bit_test(data[1],R1)) {
r1 = 1;
} else {
r1 = 0;
}
if (bit_test(data[1],R2)) {
r2 = 1;
} else {
r2 = 0;
}
if ((data[2] & Start) == Start) {
start = 1;
up = 0;
down = 0;
} else {
start = 0;
if (bit_test(data[2],Up)) {
up = 1;
} else {
up = 0;
}
if (bit_test(data[2],Down)) {
down = 1;
} else {
down = 0;
}
}
if ((data[2] & Select) == Select) {
select = 1;
right = 0;
left = 0;
} else {
select = 0;
if (bit_test(data[2],Right)) {
right = 1;
} else {
right = 0;
}
if (bit_test(data[2],Left)) {
left = 1;
} else {
left = 0;
}
}
if (bit_test(data[2],Sankaku)) {
sankaku = 1;
} else {
sankaku = 0;
}
if (bit_test(data[2],Batu)) {
batu = 1;
} else {
batu = 0;
}
if (bit_test(data[2],Maru)) {
maru = 1;
} else {
maru = 0;
}
Left_X = data[3];
Left_Y = data[4];
Right_X = data[5];
Right_Y = data[6];
if (Left_X <= EvenNeutral) {
left_x = (float) EvenNeutral - Left_X;
} else {
left_x = (float) OddNeutral - Left_X;
}
if (Left_Y <= EvenNeutral) {
left_y = (float) EvenNeutral - Left_Y;
} else {
left_y = (float) OddNeutral - Left_Y;
}
if (Right_X <= EvenNeutral) {
right_x = (float) EvenNeutral - Right_X;
} else {
right_x = (float) OddNeutral - Right_X;
}
if (Right_Y <= EvenNeutral) {
right_y = (float) EvenNeutral - Right_Y;
} else {
right_y = (float) OddNeutral - Right_Y;
}
left_x = -left_x / EvenNeutral;
left_y = left_y / EvenNeutral;
right_x = -right_x / EvenNeutral;
right_y = right_y / EvenNeutral;
if (fabs(left_x) < 0.24f) {
left_x = 0;
}
if (fabs(left_y) < 0.24f) {
left_y = 0;
}
}
void data_receive () {
read = SBDBT.getc();
if (read == start_byte) {
data_start = 1;
checksum = 0;
byte = 1;
buffer[0] = read;
} else {
if (data_start == 1) {
buffer[byte] = read;
byte++;
}
if (byte > input_byte) {
int i;
for (i = 1 ; i < input_byte; i++) {
checksum = checksum + buffer[i];
}
if ((checksum & 0b01111111) == buffer[7]) {
for (i = 0; i < data_byte; i++) {
data[i] = buffer[i];
open_data[i] = data[i];
}
check();
}
byte = 0;
data_start = 0;
}
}
}
};
