Hard brake rather than coast
Fork of PololuQik2 by
PololuQik2.cpp@2:25c41766d768, 2014-04-01 (annotated)
- Committer:
- jjcarr2
- Date:
- Tue Apr 01 20:22:30 2014 +0000
- Revision:
- 2:25c41766d768
- Parent:
- 1:df9964aaa00d
stopBothMotors = hard Brake;
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 | |
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 | |
jjcarr2 | 2:25c41766d768 | 82 | void PololuQik2::stopBothMotors(int8_t speed) { |
jjcarr2 | 2:25c41766d768 | 83 | unsigned char message[] = {MOTOR0COASTPACKET, speed}; |
jjcarr2 | 2:25c41766d768 | 84 | sendMessage(message , 2); |
jjcarr2 | 2:25c41766d768 | 85 | unsigned char message2[] = {MOTOR1COASTPACKET, speed}; |
jjcarr2 | 2:25c41766d768 | 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 | } |