UH Robotics / PololuQik2

uyvli

Dependents:   theRobot3

Fork of PololuQik2 by Thomas Ashworth

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?

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
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 }