stephen smitherman / PololuQik2

Pololu Quk Motor Controller Driver

Dependents:   NavigationTest

PololuQik2.cpp@0:511d65ef1276, 2014-03-03 (annotated)

Committer:
srsmitherman
Date:
Mon Mar 03 19:34:13 2014 +0000
Revision:
0:511d65ef1276
Pololu Qik Motor Controller;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
srsmitherman 0:511d65ef1276 1 /**
srsmitherman 0:511d65ef1276 2 * @file PololuQik2.cpp
srsmitherman 0:511d65ef1276 3 * @author Edward Wilson (edwilson1989@gmail.com)
srsmitherman 0:511d65ef1276 4 * @date Aug 6, 2010
srsmitherman 0:511d65ef1276 5 * @brief PololuQik2 motor drivers
srsmitherman 0:511d65ef1276 6 */
srsmitherman 0:511d65ef1276 7
srsmitherman 0:511d65ef1276 8 /*************************************************************
srsmitherman 0:511d65ef1276 9 PololuQik2 - basic class to control Pololu's Qik2s9v1
srsmitherman 0:511d65ef1276 10 motor controller (http://www.pololu.com/catalog/product/1110)
srsmitherman 0:511d65ef1276 11
srsmitherman 0:511d65ef1276 12 This uses the default settings for the motor controller and the
srsmitherman 0:511d65ef1276 13 Compact Protocol to communicate to it.
srsmitherman 0:511d65ef1276 14
srsmitherman 0:511d65ef1276 15 This library is free software; you can redistribute it and/or
srsmitherman 0:511d65ef1276 16 modify it under the terms of the GNU Lesser General Public
srsmitherman 0:511d65ef1276 17 License as published by the Free Software Foundation; either
srsmitherman 0:511d65ef1276 18 version 2.1 of the License, or (at your option) any later version.
srsmitherman 0:511d65ef1276 19
srsmitherman 0:511d65ef1276 20 This library is distributed in the hope that it will be useful,
srsmitherman 0:511d65ef1276 21 but WITHOUT ANY WARRANTY; without even the implied warranty of
srsmitherman 0:511d65ef1276 22 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
srsmitherman 0:511d65ef1276 23 Lesser General Public License for more details.
srsmitherman 0:511d65ef1276 24
srsmitherman 0:511d65ef1276 25 You should have received a copy of the GNU Lesser General Public
srsmitherman 0:511d65ef1276 26 License along with this library; if not, write to the Free Software
srsmitherman 0:511d65ef1276 27 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
srsmitherman 0:511d65ef1276 28
srsmitherman 0:511d65ef1276 29 Author: Edward Wilson (edwilson1989@gmail.com)
srsmitherman 0:511d65ef1276 30 History:
srsmitherman 0:511d65ef1276 31 Version: 1.0 August 6, 2010 Initial code
srsmitherman 0:511d65ef1276 32 1.1 December 28, 2010 Composition constructor added
srsmitherman 0:511d65ef1276 33 1.3 April 22, 2011 Ported for MBED
srsmitherman 0:511d65ef1276 34 ************************************************************/
srsmitherman 0:511d65ef1276 35
srsmitherman 0:511d65ef1276 36 #include "PololuQik2.h"
srsmitherman 0:511d65ef1276 37
srsmitherman 0:511d65ef1276 38 PololuQik2::PololuQik2(PinName TxPin, PinName RxPin, PinName RSTPin, PinName errPin, void (*ErrorFunction)(void), bool enCRC):CRC(), resetPin(RSTPin), errorPin(errPin), serialConnection(TxPin,RxPin) {
srsmitherman 0:511d65ef1276 39 serialConnection.baud(38400);
srsmitherman 0:511d65ef1276 40 enableCRC = enCRC;
srsmitherman 0:511d65ef1276 41 errorPin.rise(ErrorFunction);
srsmitherman 0:511d65ef1276 42 }
srsmitherman 0:511d65ef1276 43
srsmitherman 0:511d65ef1276 44 void PololuQik2::begin() {
srsmitherman 0:511d65ef1276 45 serialConnection.baud(38400);
srsmitherman 0:511d65ef1276 46 wait_ms(100); // Allow Serial baud rate to propagate
srsmitherman 0:511d65ef1276 47 resetPin = 0;
srsmitherman 0:511d65ef1276 48 wait_ms(100);
srsmitherman 0:511d65ef1276 49 resetPin = 1;
srsmitherman 0:511d65ef1276 50 wait_ms(10);
srsmitherman 0:511d65ef1276 51 unsigned char message[] = { INITIALPACKET };
srsmitherman 0:511d65ef1276 52 sendMessage(message, 1);
srsmitherman 0:511d65ef1276 53 wait_ms(100);
srsmitherman 0:511d65ef1276 54 }
srsmitherman 0:511d65ef1276 55
srsmitherman 0:511d65ef1276 56 void PololuQik2::setMotor0Speed(int8_t speed) {
srsmitherman 0:511d65ef1276 57 if (speed < 0) {
srsmitherman 0:511d65ef1276 58 unsigned char message[] = {MOTOR0REVERSEPACKET, (-1*speed)};
srsmitherman 0:511d65ef1276 59 sendMessage(message , 2);
srsmitherman 0:511d65ef1276 60 } else if (speed > 0) {
srsmitherman 0:511d65ef1276 61 unsigned char message[] = {MOTOR0FORWARDPACKET, speed};
srsmitherman 0:511d65ef1276 62 sendMessage(message , 2);
srsmitherman 0:511d65ef1276 63 } else {
srsmitherman 0:511d65ef1276 64 unsigned char message[] = {MOTOR0REVERSEPACKET, 0};
srsmitherman 0:511d65ef1276 65 sendMessage(message , 2);
srsmitherman 0:511d65ef1276 66 }
srsmitherman 0:511d65ef1276 67 }
srsmitherman 0:511d65ef1276 68
srsmitherman 0:511d65ef1276 69 void PololuQik2::setMotor1Speed(int8_t speed) {
srsmitherman 0:511d65ef1276 70 if (speed < 0) {
srsmitherman 0:511d65ef1276 71 unsigned char message[] = {MOTOR1REVERSEPACKET, (-1*speed)};
srsmitherman 0:511d65ef1276 72 sendMessage(message , 2);
srsmitherman 0:511d65ef1276 73 } else if (speed > 0) {
srsmitherman 0:511d65ef1276 74 unsigned char message[] = {MOTOR1FORWARDPACKET, speed};
srsmitherman 0:511d65ef1276 75 sendMessage(message , 2);
srsmitherman 0:511d65ef1276 76 } else {
srsmitherman 0:511d65ef1276 77 unsigned char message[] = {MOTOR1REVERSEPACKET, 0};
srsmitherman 0:511d65ef1276 78 sendMessage(message , 2);
srsmitherman 0:511d65ef1276 79 }
srsmitherman 0:511d65ef1276 80 }
srsmitherman 0:511d65ef1276 81
srsmitherman 0:511d65ef1276 82 void PololuQik2::stopBothMotors() {
srsmitherman 0:511d65ef1276 83 setMotor0Speed(0);
srsmitherman 0:511d65ef1276 84 setMotor1Speed(0);
srsmitherman 0:511d65ef1276 85 }
srsmitherman 0:511d65ef1276 86
srsmitherman 0:511d65ef1276 87 void PololuQik2::sendMessage(unsigned char message[], uint8_t length) {
srsmitherman 0:511d65ef1276 88 if (enableCRC) {
srsmitherman 0:511d65ef1276 89 uint8_t chk = CRC.CRC(message, length);
srsmitherman 0:511d65ef1276 90 for (int8_t i = 0; i < length; i++)
srsmitherman 0:511d65ef1276 91 sendByte(message[i]);
srsmitherman 0:511d65ef1276 92
srsmitherman 0:511d65ef1276 93 sendByte(chk);
srsmitherman 0:511d65ef1276 94 } else {
srsmitherman 0:511d65ef1276 95 for (int8_t i = 0; i < length; i++)
srsmitherman 0:511d65ef1276 96 sendByte(message[i]);
srsmitherman 0:511d65ef1276 97 }
srsmitherman 0:511d65ef1276 98 }
srsmitherman 0:511d65ef1276 99
srsmitherman 0:511d65ef1276 100 uint8_t PololuQik2::readByte() {
srsmitherman 0:511d65ef1276 101 while (!serialConnection.readable());
srsmitherman 0:511d65ef1276 102 return serialConnection.getc();
srsmitherman 0:511d65ef1276 103 }
srsmitherman 0:511d65ef1276 104
srsmitherman 0:511d65ef1276 105 void PololuQik2::sendByte(uint8_t byte) {
srsmitherman 0:511d65ef1276 106 serialConnection.putc(byte);
srsmitherman 0:511d65ef1276 107 }
srsmitherman 0:511d65ef1276 108
srsmitherman 0:511d65ef1276 109 uint8_t PololuQik2::getFirmwareVersion() {
srsmitherman 0:511d65ef1276 110 unsigned char message[] = { FWVERSIONPACKET };
srsmitherman 0:511d65ef1276 111 sendMessage(message, 1);
srsmitherman 0:511d65ef1276 112 return readByte();
srsmitherman 0:511d65ef1276 113 }
srsmitherman 0:511d65ef1276 114
srsmitherman 0:511d65ef1276 115 bool PololuQik2::hasCRCError() {
srsmitherman 0:511d65ef1276 116 return errorBitSet(CRCERRORBIT);
srsmitherman 0:511d65ef1276 117 }
srsmitherman 0:511d65ef1276 118
srsmitherman 0:511d65ef1276 119 bool PololuQik2::errorBitSet(uint8_t bitToCheck) {
srsmitherman 0:511d65ef1276 120 return (1 << bitToCheck) & errByte;
srsmitherman 0:511d65ef1276 121 }
srsmitherman 0:511d65ef1276 122
srsmitherman 0:511d65ef1276 123 bool PololuQik2::hasFrameError() {
srsmitherman 0:511d65ef1276 124 return errorBitSet(FRAMEERRORBIT);
srsmitherman 0:511d65ef1276 125 }
srsmitherman 0:511d65ef1276 126
srsmitherman 0:511d65ef1276 127 bool PololuQik2::hasDataOverrunError() {
srsmitherman 0:511d65ef1276 128 return errorBitSet(DATAOVERRUNERRORBIT);
srsmitherman 0:511d65ef1276 129 }
srsmitherman 0:511d65ef1276 130
srsmitherman 0:511d65ef1276 131 bool PololuQik2::hasFormatError() {
srsmitherman 0:511d65ef1276 132 return errorBitSet(FORMATERRORBIT);
srsmitherman 0:511d65ef1276 133 }
srsmitherman 0:511d65ef1276 134
srsmitherman 0:511d65ef1276 135 bool PololuQik2::hasTimeoutError() {
srsmitherman 0:511d65ef1276 136 return errorBitSet(TIMEOUTERRORBIT);
srsmitherman 0:511d65ef1276 137 }
srsmitherman 0:511d65ef1276 138
srsmitherman 0:511d65ef1276 139 uint8_t PololuQik2::getError() {
srsmitherman 0:511d65ef1276 140 unsigned char message[] = { ERRORPACKET };
srsmitherman 0:511d65ef1276 141 sendMessage(message, 1);
srsmitherman 0:511d65ef1276 142 return readByte();
srsmitherman 0:511d65ef1276 143 }