taka yamanouchi / MotorDrivers

first_library

Dependents:   2019_tourobo_upper minirobo_upper_reserve minirobo_under_reserve serial_RTX_NUCLEA

Sabertooth_Serial.cpp

Committer:
sink
Date:
2019-02-04
Revision:
1:9dd052490f0a
Parent:
0:03e99bd9339e

File content as of revision 1:9dd052490f0a:

#include "Sabertooth_Serial.h"

SaberSerial::SaberSerial(int baudrate, PinName tx, PinName rx) : _Sabertooth(tx, rx){
    _Sabertooth.baud(baudrate);
}

void SaberSerial::Serial(int address, int direction, int cmd){
    _Sabertooth.putc(address);
    _Sabertooth.putc(direction); 
    _Sabertooth.putc(abs(cmd));
    _Sabertooth.putc((address + direction + abs(cmd)) & 0b01111111);
}