Paolina Povolotskaya / PololuQik2

Pololu Quk Motor Controller Driver

Dependents:   NavigationTest

Fork of PololuQik2 by stephen smitherman

PololuQik2.cpp@2:8d650b072fa8, 2014-04-03 (annotated)

Committer:
Fairy_Paolina
Date:
Thu Apr 03 15:24:59 2014 +0000
Revision:
2:8d650b072fa8
Parent:
1:df9964aaa00d 
new;

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
Fairy_Paolina 2:8d650b072fa8 82 void PololuQik2::stopBothMotors(int8_t speed) {
Fairy_Paolina 2:8d650b072fa8 83 unsigned char message[] = {MOTOR0COASTPACKET, speed};
Fairy_Paolina 2:8d650b072fa8 84 sendMessage(message , 2);
Fairy_Paolina 2:8d650b072fa8 85 unsigned char message2[] = {MOTOR1COASTPACKET, speed};
Fairy_Paolina 2:8d650b072fa8 86 sendMessage(message2 , 2);
srsmitherman 0:511d65ef1276 87 }
srsmitherman 0:511d65ef1276 88
srsmitherman 0:511d65ef1276 89 void PololuQik2::sendMessage(unsigned char message[], uint8_t length) {
srsmitherman 0:511d65ef1276 90 if (enableCRC) {
srsmitherman 0:511d65ef1276 91 uint8_t chk = CRC.CRC(message, length);
srsmitherman 0:511d65ef1276 92 for (int8_t i = 0; i < length; i++)
srsmitherman 0:511d65ef1276 93 sendByte(message[i]);
srsmitherman 0:511d65ef1276 94
srsmitherman 0:511d65ef1276 95 sendByte(chk);
srsmitherman 0:511d65ef1276 96 } else {
srsmitherman 0:511d65ef1276 97 for (int8_t i = 0; i < length; i++)
srsmitherman 0:511d65ef1276 98 sendByte(message[i]);
srsmitherman 0:511d65ef1276 99 }
srsmitherman 0:511d65ef1276 100 }
srsmitherman 0:511d65ef1276 101
srsmitherman 0:511d65ef1276 102 uint8_t PololuQik2::readByte() {
srsmitherman 0:511d65ef1276 103 while (!serialConnection.readable());
srsmitherman 0:511d65ef1276 104 return serialConnection.getc();
srsmitherman 0:511d65ef1276 105 }
srsmitherman 0:511d65ef1276 106
srsmitherman 0:511d65ef1276 107 void PololuQik2::sendByte(uint8_t byte) {
srsmitherman 0:511d65ef1276 108 serialConnection.putc(byte);
srsmitherman 0:511d65ef1276 109 }
srsmitherman 0:511d65ef1276 110
srsmitherman 0:511d65ef1276 111 uint8_t PololuQik2::getFirmwareVersion() {
srsmitherman 0:511d65ef1276 112 unsigned char message[] = { FWVERSIONPACKET };
srsmitherman 0:511d65ef1276 113 sendMessage(message, 1);
srsmitherman 0:511d65ef1276 114 return readByte();
srsmitherman 0:511d65ef1276 115 }
srsmitherman 0:511d65ef1276 116
srsmitherman 0:511d65ef1276 117 bool PololuQik2::hasCRCError() {
srsmitherman 0:511d65ef1276 118 return errorBitSet(CRCERRORBIT);
srsmitherman 0:511d65ef1276 119 }
srsmitherman 0:511d65ef1276 120
srsmitherman 0:511d65ef1276 121 bool PololuQik2::errorBitSet(uint8_t bitToCheck) {
srsmitherman 0:511d65ef1276 122 return (1 << bitToCheck) & errByte;
srsmitherman 0:511d65ef1276 123 }
srsmitherman 0:511d65ef1276 124
srsmitherman 0:511d65ef1276 125 bool PololuQik2::hasFrameError() {
srsmitherman 0:511d65ef1276 126 return errorBitSet(FRAMEERRORBIT);
srsmitherman 0:511d65ef1276 127 }
srsmitherman 0:511d65ef1276 128
srsmitherman 0:511d65ef1276 129 bool PololuQik2::hasDataOverrunError() {
srsmitherman 0:511d65ef1276 130 return errorBitSet(DATAOVERRUNERRORBIT);
srsmitherman 0:511d65ef1276 131 }
srsmitherman 0:511d65ef1276 132
srsmitherman 0:511d65ef1276 133 bool PololuQik2::hasFormatError() {
srsmitherman 0:511d65ef1276 134 return errorBitSet(FORMATERRORBIT);
srsmitherman 0:511d65ef1276 135 }
srsmitherman 0:511d65ef1276 136
srsmitherman 0:511d65ef1276 137 bool PololuQik2::hasTimeoutError() {
srsmitherman 0:511d65ef1276 138 return errorBitSet(TIMEOUTERRORBIT);
srsmitherman 0:511d65ef1276 139 }
srsmitherman 0:511d65ef1276 140
srsmitherman 0:511d65ef1276 141 uint8_t PololuQik2::getError() {
srsmitherman 0:511d65ef1276 142 unsigned char message[] = { ERRORPACKET };
srsmitherman 0:511d65ef1276 143 sendMessage(message, 1);
srsmitherman 0:511d65ef1276 144 return readByte();
srsmitherman 0:511d65ef1276 145 }