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 uint8_t PololuQik2::GetMotor0Current() {
00045     unsigned char message[] = { MOTOR0CURRENTPACKET };
00046     sendMessage(message , 1);
00047     return readByte();
00048 }
00049 
00050 uint8_t PololuQik2::GetMotor1Current() {
00051     unsigned char message[] = { MOTOR1CURRENTPACKET };
00052     sendMessage(message , 1);
00053     return readByte();
00054 }
00055 
00056 void PololuQik2::begin() {
00057     serialConnection.baud(38400);
00058     wait_ms(100); // Allow Serial baud rate to propagate
00059     resetPin = 0;
00060     wait_ms(100);
00061     resetPin = 1;
00062     wait_ms(10);
00063     unsigned char message[] = { INITIALPACKET };
00064     sendMessage(message, 1);
00065     wait_ms(100);
00066 }
00067 
00068 void PololuQik2::setMotor0Speed(int8_t speed) {
00069     if (speed < 0) {
00070         unsigned char message[] = {MOTOR0REVERSEPACKET, (-1*speed)};
00071         sendMessage(message , 2);
00072     } else if (speed > 0) {
00073         unsigned char message[] = {MOTOR0FORWARDPACKET, speed};
00074         sendMessage(message , 2);
00075     } else {
00076         unsigned char message[] = {MOTOR0REVERSEPACKET, 0};
00077         sendMessage(message , 2);
00078     }
00079 }
00080 
00081 void PololuQik2::setMotor1Speed(int8_t speed) {
00082     if (speed < 0) {
00083         unsigned char message[] = {MOTOR1REVERSEPACKET, (-1*speed)};
00084         sendMessage(message , 2);
00085     } else if (speed > 0) {
00086         unsigned char message[] = {MOTOR1FORWARDPACKET, speed};
00087         sendMessage(message , 2);
00088     } else {
00089         unsigned char message[] = {MOTOR1REVERSEPACKET, 0};
00090         sendMessage(message , 2);
00091     }
00092 }
00093 
00094 void PololuQik2::stopBothMotors(int8_t speed) {
00095     unsigned char message[] = {MOTOR0COASTPACKET, speed};
00096     sendMessage(message , 2);
00097     unsigned char message2[] = {MOTOR1COASTPACKET, speed};
00098     sendMessage(message2 , 2);
00099 }
00100 
00101 void PololuQik2::sendMessage(unsigned char message[], uint8_t length) {
00102     if (enableCRC) {
00103         uint8_t chk = CRC.CRC(message, length);
00104         for (int8_t i = 0; i < length; i++)
00105             sendByte(message[i]);
00106 
00107         sendByte(chk);
00108     } else {
00109         for (int8_t i = 0; i < length; i++)
00110             sendByte(message[i]);
00111     }
00112 }
00113 
00114 uint8_t PololuQik2::readByte() {
00115     while (!serialConnection.readable());
00116     return serialConnection.getc();
00117 }
00118 
00119 void PololuQik2::sendByte(uint8_t byte) {
00120     serialConnection.putc(byte);
00121 }
00122 
00123 uint8_t PololuQik2::getFirmwareVersion() {
00124     unsigned char message[] = { FWVERSIONPACKET };
00125     sendMessage(message, 1);
00126     return readByte();
00127 }
00128 
00129 bool PololuQik2::hasCRCError() {
00130     return errorBitSet(CRCERRORBIT);
00131 }
00132 
00133 bool PololuQik2::errorBitSet(uint8_t bitToCheck) {
00134     return (1 << bitToCheck) & errByte;
00135 }
00136 
00137 bool PololuQik2::hasFrameError() {
00138     return errorBitSet(FRAMEERRORBIT);
00139 }
00140 
00141 bool PololuQik2::hasDataOverrunError() {
00142     return errorBitSet(DATAOVERRUNERRORBIT);
00143 }
00144 
00145 bool PololuQik2::hasFormatError() {
00146     return errorBitSet(FORMATERRORBIT);
00147 }
00148 
00149 bool PololuQik2::hasTimeoutError() {
00150     return errorBitSet(TIMEOUTERRORBIT);
00151 }
00152 
00153 uint8_t PololuQik2::getError() {
00154     unsigned char message[] = { ERRORPACKET };
00155     sendMessage(message, 1);
00156     return readByte();
00157 }