PololuQik2.cpp

00001 /**
00002  *    @file     PololuQik2.cpp
00003  *    @author   Edward Wilson (edwilson1989@gmail.com)
00004  *    @date     Aug 6, 2010
00005  *    @brief    PololuQik2 motor drivers
00006  */
00007 
00008 /*************************************************************
00009  PololuQik2 - basic class to control Pololu's Qik2s9v1
00010  motor controller (http://www.pololu.com/catalog/product/1110)
00011 
00012  This uses the default settings for the motor controller and the
00013  Compact Protocol to communicate to it.
00014 
00015  This library is free software; you can redistribute it and/or
00016  modify it under the terms of the GNU Lesser General Public
00017  License as published by the Free Software Foundation; either
00018  version 2.1 of the License, or (at your option) any later version.
00019 
00020  This library is distributed in the hope that it will be useful,
00021  but WITHOUT ANY WARRANTY; without even the implied warranty of
00022  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00023  Lesser General Public License for more details.
00024 
00025  You should have received a copy of the GNU Lesser General Public
00026  License along with this library; if not, write to the Free Software
00027  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00028 
00029  Author:  Edward Wilson (edwilson1989@gmail.com)
00030  History:
00031  Version: 1.0    August 6, 2010      Initial code
00032           1.1    December 28, 2010   Composition constructor added
00033           1.3    April 22, 2011      Ported for MBED
00034 ************************************************************/
00035 
00036 #include "PololuQik2.h"
00037 
00038 PololuQik2::PololuQik2(PinName TxPin, PinName RxPin, PinName RSTPin, PinName errPin, void (*ErrorFunction)(void), bool enCRC):CRC(), resetPin(RSTPin), errorPin(errPin), serialConnection(TxPin,RxPin) {
00039     serialConnection.baud(38400);
00040     enableCRC = enCRC;
00041     errorPin.rise(ErrorFunction);
00042 }
00043 
00044 void PololuQik2::begin() {
00045     serialConnection.baud(38400);
00046     wait_ms(100); // Allow Serial baud rate to propagate
00047     resetPin = 0;
00048     wait_ms(100);
00049     resetPin = 1;
00050     wait_ms(10);
00051     unsigned char message[] = { INITIALPACKET };
00052     sendMessage(message, 1);
00053     wait_ms(100);
00054 }
00055 
00056 void PololuQik2::setMotor0Speed(int8_t speed) {
00057     if (speed < 0) {
00058         unsigned char message[] = {MOTOR0REVERSEPACKET, (-1*speed)};
00059         sendMessage(message , 2);
00060     } else if (speed > 0) {
00061         unsigned char message[] = {MOTOR0FORWARDPACKET, speed};
00062         sendMessage(message , 2);
00063     } else {
00064         unsigned char message[] = {MOTOR0REVERSEPACKET, 0};
00065         sendMessage(message , 2);
00066     }
00067 }
00068 
00069 void PololuQik2::setMotor1Speed(int8_t speed) {
00070     if (speed < 0) {
00071         unsigned char message[] = {MOTOR1REVERSEPACKET, (-1*speed)};
00072         sendMessage(message , 2);
00073     } else if (speed > 0) {
00074         unsigned char message[] = {MOTOR1FORWARDPACKET, speed};
00075         sendMessage(message , 2);
00076     } else {
00077         unsigned char message[] = {MOTOR1REVERSEPACKET, 0};
00078         sendMessage(message , 2);
00079     }
00080 }
00081 
00082 void PololuQik2::stopBothMotors() {
00083     setMotor0Speed(0);
00084     setMotor1Speed(0);
00085 }
00086 
00087 void PololuQik2::sendMessage(unsigned char message[], uint8_t length) {
00088     if (enableCRC) {
00089         uint8_t chk = CRC.CRC(message, length);
00090         for (int8_t i = 0; i < length; i++)
00091             sendByte(message[i]);
00092 
00093         sendByte(chk);
00094     } else {
00095         for (int8_t i = 0; i < length; i++)
00096             sendByte(message[i]);
00097     }
00098 }
00099 
00100 uint8_t PololuQik2::readByte() {
00101     while (!serialConnection.readable());
00102     return serialConnection.getc();
00103 }
00104 
00105 void PololuQik2::sendByte(uint8_t byte) {
00106     serialConnection.putc(byte);
00107 }
00108 
00109 uint8_t PololuQik2::getFirmwareVersion() {
00110     unsigned char message[] = { FWVERSIONPACKET };
00111     sendMessage(message, 1);
00112     return readByte();
00113 }
00114 
00115 bool PololuQik2::hasCRCError() {
00116     return errorBitSet(CRCERRORBIT);
00117 }
00118 
00119 bool PololuQik2::errorBitSet(uint8_t bitToCheck) {
00120     return (1 << bitToCheck) & errByte;
00121 }
00122 
00123 bool PololuQik2::hasFrameError() {
00124     return errorBitSet(FRAMEERRORBIT);
00125 }
00126 
00127 bool PololuQik2::hasDataOverrunError() {
00128     return errorBitSet(DATAOVERRUNERRORBIT);
00129 }
00130 
00131 bool PololuQik2::hasFormatError() {
00132     return errorBitSet(FORMATERRORBIT);
00133 }
00134 
00135 bool PololuQik2::hasTimeoutError() {
00136     return errorBitSet(TIMEOUTERRORBIT);
00137 }
00138 
00139 uint8_t PololuQik2::getError() {
00140     unsigned char message[] = { ERRORPACKET };
00141     sendMessage(message, 1);
00142     return readByte();
00143 }