Pololu Quk Motor Controller Driver
Diff: PololuQik2.cpp
- Revision:
- 0:511d65ef1276
diff -r 000000000000 -r 511d65ef1276 PololuQik2.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PololuQik2.cpp Mon Mar 03 19:34:13 2014 +0000 @@ -0,0 +1,143 @@ +/** + * @file PololuQik2.cpp + * @author Edward Wilson (edwilson1989@gmail.com) + * @date Aug 6, 2010 + * @brief PololuQik2 motor drivers + */ + +/************************************************************* + PololuQik2 - basic class to control Pololu's Qik2s9v1 + motor controller (http://www.pololu.com/catalog/product/1110) + + This uses the default settings for the motor controller and the + Compact Protocol to communicate to it. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + Author: Edward Wilson (edwilson1989@gmail.com) + History: + Version: 1.0 August 6, 2010 Initial code + 1.1 December 28, 2010 Composition constructor added + 1.3 April 22, 2011 Ported for MBED +************************************************************/ + +#include "PololuQik2.h" + +PololuQik2::PololuQik2(PinName TxPin, PinName RxPin, PinName RSTPin, PinName errPin, void (*ErrorFunction)(void), bool enCRC):CRC(), resetPin(RSTPin), errorPin(errPin), serialConnection(TxPin,RxPin) { + serialConnection.baud(38400); + enableCRC = enCRC; + errorPin.rise(ErrorFunction); +} + +void PololuQik2::begin() { + serialConnection.baud(38400); + wait_ms(100); // Allow Serial baud rate to propagate + resetPin = 0; + wait_ms(100); + resetPin = 1; + wait_ms(10); + unsigned char message[] = { INITIALPACKET }; + sendMessage(message, 1); + wait_ms(100); +} + +void PololuQik2::setMotor0Speed(int8_t speed) { + if (speed < 0) { + unsigned char message[] = {MOTOR0REVERSEPACKET, (-1*speed)}; + sendMessage(message , 2); + } else if (speed > 0) { + unsigned char message[] = {MOTOR0FORWARDPACKET, speed}; + sendMessage(message , 2); + } else { + unsigned char message[] = {MOTOR0REVERSEPACKET, 0}; + sendMessage(message , 2); + } +} + +void PololuQik2::setMotor1Speed(int8_t speed) { + if (speed < 0) { + unsigned char message[] = {MOTOR1REVERSEPACKET, (-1*speed)}; + sendMessage(message , 2); + } else if (speed > 0) { + unsigned char message[] = {MOTOR1FORWARDPACKET, speed}; + sendMessage(message , 2); + } else { + unsigned char message[] = {MOTOR1REVERSEPACKET, 0}; + sendMessage(message , 2); + } +} + +void PololuQik2::stopBothMotors() { + setMotor0Speed(0); + setMotor1Speed(0); +} + +void PololuQik2::sendMessage(unsigned char message[], uint8_t length) { + if (enableCRC) { + uint8_t chk = CRC.CRC(message, length); + for (int8_t i = 0; i < length; i++) + sendByte(message[i]); + + sendByte(chk); + } else { + for (int8_t i = 0; i < length; i++) + sendByte(message[i]); + } +} + +uint8_t PololuQik2::readByte() { + while (!serialConnection.readable()); + return serialConnection.getc(); +} + +void PololuQik2::sendByte(uint8_t byte) { + serialConnection.putc(byte); +} + +uint8_t PololuQik2::getFirmwareVersion() { + unsigned char message[] = { FWVERSIONPACKET }; + sendMessage(message, 1); + return readByte(); +} + +bool PololuQik2::hasCRCError() { + return errorBitSet(CRCERRORBIT); +} + +bool PololuQik2::errorBitSet(uint8_t bitToCheck) { + return (1 << bitToCheck) & errByte; +} + +bool PololuQik2::hasFrameError() { + return errorBitSet(FRAMEERRORBIT); +} + +bool PololuQik2::hasDataOverrunError() { + return errorBitSet(DATAOVERRUNERRORBIT); +} + +bool PololuQik2::hasFormatError() { + return errorBitSet(FORMATERRORBIT); +} + +bool PololuQik2::hasTimeoutError() { + return errorBitSet(TIMEOUTERRORBIT); +} + +uint8_t PololuQik2::getError() { + unsigned char message[] = { ERRORPACKET }; + sendMessage(message, 1); + return readByte(); +}