uyvli
Fork of PololuQik2 by
PololuQik2.cpp@4:33ca85c52dc0, 2014-04-12 (annotated)
- Committer:
- Fairy_Paolina
- Date:
- Sat Apr 12 20:25:58 2014 +0000
- Revision:
- 4:33ca85c52dc0
- Parent:
- 2:25c41766d768
modified;
Who changed what in which revision?
User | Revision | Line number | New 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 | |
Fairy_Paolina | 4:33ca85c52dc0 | 44 | uint8_t PololuQik2::GetMotor0Current() { |
Fairy_Paolina | 4:33ca85c52dc0 | 45 | unsigned char message[] = { MOTOR0CURRENTPACKET }; |
Fairy_Paolina | 4:33ca85c52dc0 | 46 | sendMessage(message , 1); |
Fairy_Paolina | 4:33ca85c52dc0 | 47 | return readByte(); |
Fairy_Paolina | 4:33ca85c52dc0 | 48 | } |
Fairy_Paolina | 4:33ca85c52dc0 | 49 | |
Fairy_Paolina | 4:33ca85c52dc0 | 50 | uint8_t PololuQik2::GetMotor1Current() { |
Fairy_Paolina | 4:33ca85c52dc0 | 51 | unsigned char message[] = { MOTOR1CURRENTPACKET }; |
Fairy_Paolina | 4:33ca85c52dc0 | 52 | sendMessage(message , 1); |
Fairy_Paolina | 4:33ca85c52dc0 | 53 | return readByte(); |
Fairy_Paolina | 4:33ca85c52dc0 | 54 | } |
Fairy_Paolina | 4:33ca85c52dc0 | 55 | |
srsmitherman | 0:511d65ef1276 | 56 | void PololuQik2::begin() { |
srsmitherman | 0:511d65ef1276 | 57 | serialConnection.baud(38400); |
srsmitherman | 0:511d65ef1276 | 58 | wait_ms(100); // Allow Serial baud rate to propagate |
srsmitherman | 0:511d65ef1276 | 59 | resetPin = 0; |
srsmitherman | 0:511d65ef1276 | 60 | wait_ms(100); |
srsmitherman | 0:511d65ef1276 | 61 | resetPin = 1; |
srsmitherman | 0:511d65ef1276 | 62 | wait_ms(10); |
srsmitherman | 0:511d65ef1276 | 63 | unsigned char message[] = { INITIALPACKET }; |
srsmitherman | 0:511d65ef1276 | 64 | sendMessage(message, 1); |
srsmitherman | 0:511d65ef1276 | 65 | wait_ms(100); |
srsmitherman | 0:511d65ef1276 | 66 | } |
srsmitherman | 0:511d65ef1276 | 67 | |
srsmitherman | 0:511d65ef1276 | 68 | void PololuQik2::setMotor0Speed(int8_t speed) { |
srsmitherman | 0:511d65ef1276 | 69 | if (speed < 0) { |
srsmitherman | 0:511d65ef1276 | 70 | unsigned char message[] = {MOTOR0REVERSEPACKET, (-1*speed)}; |
srsmitherman | 0:511d65ef1276 | 71 | sendMessage(message , 2); |
srsmitherman | 0:511d65ef1276 | 72 | } else if (speed > 0) { |
srsmitherman | 0:511d65ef1276 | 73 | unsigned char message[] = {MOTOR0FORWARDPACKET, speed}; |
srsmitherman | 0:511d65ef1276 | 74 | sendMessage(message , 2); |
srsmitherman | 0:511d65ef1276 | 75 | } else { |
srsmitherman | 0:511d65ef1276 | 76 | unsigned char message[] = {MOTOR0REVERSEPACKET, 0}; |
srsmitherman | 0:511d65ef1276 | 77 | sendMessage(message , 2); |
srsmitherman | 0:511d65ef1276 | 78 | } |
srsmitherman | 0:511d65ef1276 | 79 | } |
srsmitherman | 0:511d65ef1276 | 80 | |
srsmitherman | 0:511d65ef1276 | 81 | void PololuQik2::setMotor1Speed(int8_t speed) { |
srsmitherman | 0:511d65ef1276 | 82 | if (speed < 0) { |
srsmitherman | 0:511d65ef1276 | 83 | unsigned char message[] = {MOTOR1REVERSEPACKET, (-1*speed)}; |
srsmitherman | 0:511d65ef1276 | 84 | sendMessage(message , 2); |
srsmitherman | 0:511d65ef1276 | 85 | } else if (speed > 0) { |
srsmitherman | 0:511d65ef1276 | 86 | unsigned char message[] = {MOTOR1FORWARDPACKET, speed}; |
srsmitherman | 0:511d65ef1276 | 87 | sendMessage(message , 2); |
srsmitherman | 0:511d65ef1276 | 88 | } else { |
srsmitherman | 0:511d65ef1276 | 89 | unsigned char message[] = {MOTOR1REVERSEPACKET, 0}; |
srsmitherman | 0:511d65ef1276 | 90 | sendMessage(message , 2); |
srsmitherman | 0:511d65ef1276 | 91 | } |
srsmitherman | 0:511d65ef1276 | 92 | } |
srsmitherman | 0:511d65ef1276 | 93 | |
jjcarr2 | 2:25c41766d768 | 94 | void PololuQik2::stopBothMotors(int8_t speed) { |
jjcarr2 | 2:25c41766d768 | 95 | unsigned char message[] = {MOTOR0COASTPACKET, speed}; |
jjcarr2 | 2:25c41766d768 | 96 | sendMessage(message , 2); |
jjcarr2 | 2:25c41766d768 | 97 | unsigned char message2[] = {MOTOR1COASTPACKET, speed}; |
jjcarr2 | 2:25c41766d768 | 98 | sendMessage(message2 , 2); |
srsmitherman | 0:511d65ef1276 | 99 | } |
srsmitherman | 0:511d65ef1276 | 100 | |
srsmitherman | 0:511d65ef1276 | 101 | void PololuQik2::sendMessage(unsigned char message[], uint8_t length) { |
srsmitherman | 0:511d65ef1276 | 102 | if (enableCRC) { |
srsmitherman | 0:511d65ef1276 | 103 | uint8_t chk = CRC.CRC(message, length); |
srsmitherman | 0:511d65ef1276 | 104 | for (int8_t i = 0; i < length; i++) |
srsmitherman | 0:511d65ef1276 | 105 | sendByte(message[i]); |
srsmitherman | 0:511d65ef1276 | 106 | |
srsmitherman | 0:511d65ef1276 | 107 | sendByte(chk); |
srsmitherman | 0:511d65ef1276 | 108 | } else { |
srsmitherman | 0:511d65ef1276 | 109 | for (int8_t i = 0; i < length; i++) |
srsmitherman | 0:511d65ef1276 | 110 | sendByte(message[i]); |
srsmitherman | 0:511d65ef1276 | 111 | } |
srsmitherman | 0:511d65ef1276 | 112 | } |
srsmitherman | 0:511d65ef1276 | 113 | |
srsmitherman | 0:511d65ef1276 | 114 | uint8_t PololuQik2::readByte() { |
srsmitherman | 0:511d65ef1276 | 115 | while (!serialConnection.readable()); |
srsmitherman | 0:511d65ef1276 | 116 | return serialConnection.getc(); |
srsmitherman | 0:511d65ef1276 | 117 | } |
srsmitherman | 0:511d65ef1276 | 118 | |
srsmitherman | 0:511d65ef1276 | 119 | void PololuQik2::sendByte(uint8_t byte) { |
srsmitherman | 0:511d65ef1276 | 120 | serialConnection.putc(byte); |
srsmitherman | 0:511d65ef1276 | 121 | } |
srsmitherman | 0:511d65ef1276 | 122 | |
srsmitherman | 0:511d65ef1276 | 123 | uint8_t PololuQik2::getFirmwareVersion() { |
srsmitherman | 0:511d65ef1276 | 124 | unsigned char message[] = { FWVERSIONPACKET }; |
srsmitherman | 0:511d65ef1276 | 125 | sendMessage(message, 1); |
srsmitherman | 0:511d65ef1276 | 126 | return readByte(); |
srsmitherman | 0:511d65ef1276 | 127 | } |
srsmitherman | 0:511d65ef1276 | 128 | |
srsmitherman | 0:511d65ef1276 | 129 | bool PololuQik2::hasCRCError() { |
srsmitherman | 0:511d65ef1276 | 130 | return errorBitSet(CRCERRORBIT); |
srsmitherman | 0:511d65ef1276 | 131 | } |
srsmitherman | 0:511d65ef1276 | 132 | |
srsmitherman | 0:511d65ef1276 | 133 | bool PololuQik2::errorBitSet(uint8_t bitToCheck) { |
srsmitherman | 0:511d65ef1276 | 134 | return (1 << bitToCheck) & errByte; |
srsmitherman | 0:511d65ef1276 | 135 | } |
srsmitherman | 0:511d65ef1276 | 136 | |
srsmitherman | 0:511d65ef1276 | 137 | bool PololuQik2::hasFrameError() { |
srsmitherman | 0:511d65ef1276 | 138 | return errorBitSet(FRAMEERRORBIT); |
srsmitherman | 0:511d65ef1276 | 139 | } |
srsmitherman | 0:511d65ef1276 | 140 | |
srsmitherman | 0:511d65ef1276 | 141 | bool PololuQik2::hasDataOverrunError() { |
srsmitherman | 0:511d65ef1276 | 142 | return errorBitSet(DATAOVERRUNERRORBIT); |
srsmitherman | 0:511d65ef1276 | 143 | } |
srsmitherman | 0:511d65ef1276 | 144 | |
srsmitherman | 0:511d65ef1276 | 145 | bool PololuQik2::hasFormatError() { |
srsmitherman | 0:511d65ef1276 | 146 | return errorBitSet(FORMATERRORBIT); |
srsmitherman | 0:511d65ef1276 | 147 | } |
srsmitherman | 0:511d65ef1276 | 148 | |
srsmitherman | 0:511d65ef1276 | 149 | bool PololuQik2::hasTimeoutError() { |
srsmitherman | 0:511d65ef1276 | 150 | return errorBitSet(TIMEOUTERRORBIT); |
srsmitherman | 0:511d65ef1276 | 151 | } |
srsmitherman | 0:511d65ef1276 | 152 | |
srsmitherman | 0:511d65ef1276 | 153 | uint8_t PololuQik2::getError() { |
srsmitherman | 0:511d65ef1276 | 154 | unsigned char message[] = { ERRORPACKET }; |
srsmitherman | 0:511d65ef1276 | 155 | sendMessage(message, 1); |
srsmitherman | 0:511d65ef1276 | 156 | return readByte(); |
srsmitherman | 0:511d65ef1276 | 157 | } |