00001
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include "PololuQik2.h"
00039
00040 /
00041 void PololuQik2::motor1Reverse(uint8_t speed) {
00042 unsigned char message[] = { MOTOR1REVERSEPACKET, speed };
00043 sendMessage(message, 2);
00044 }
00045
00046 void PololuQik2::motor1Forward(uint8_t speed) {
00047 unsigned char message[] = { MOTOR1FORWARDPACKET, speed };
00048 sendMessage(message, 2);
00049 }
00050
00051 void PololuQik2::motor0Coast() {
00052 unsigned char message[] = { MOTOR0COASTPACKET };
00053 sendMessage(message, 1);
00054 }
00055
00056 void PololuQik2::motor0Forward(uint8_t speed) {
00057 unsigned char message[] = { MOTOR0FORWARDPACKET, speed };
00058 sendMessage(message, 2);
00059 }
00060
00061 void PololuQik2::begin() {
00062 digitalWrite(_resetPin, LOW);
00063 delay(100);
00064 digitalWrite(_resetPin, HIGH);
00065 delay(10);
00066
00067 unsigned char message[] = { INITIALPACKET };
00068 sendMessage(message, 1);
00069
00070 delay(100);
00071 }
00072
00073 void PololuQik2::sendMessage(unsigned char message[], unsigned int length) {
00074 if (_enCRC) {
00075 uint8_t chk = _crc->CRC(message, length);
00076 for (int i = 0; i < length; i++)
00077 sendByte(message[i]);
00078
00079 sendByte(chk);
00080 } else {
00081 for (int i = 0; i < length; i++)
00082 sendByte(message[i]);
00083 }
00084 }
00085
00086 void PololuQik2::stopMotor0() {
00087 unsigned char message[] = { MOTOR1REVERSEPACKET, 0 };
00088 sendMessage(message, 2);
00089 }
00090
00091 void PololuQik2::stopBothMotors() {
00092 stopMotor0();
00093 stopMotor1();
00094 }
00095
00096 unsigned int PololuQik2::getFirmwareVersion() {
00097 unsigned char message[] = { FWVERSIONPACKET };
00098 sendMessage(message, 1);
00099
00100 return readByte();
00101 }
00102
00103 PololuQik2::PololuQik2(NewSoftSerial *serial, uint8_t resetPin, uint8_t errPin, bool enCRC, CRC7 *crc) {
00104 _serial = serial;
00105 _resetPin = resetPin;
00106 _errByte = -1;
00107 _errPin = errPin;
00108 _enCRC = enCRC;
00109 _crc = crc;
00110
00111 _crc.init();
00112 pinMode(_resetPin, OUTPUT);
00113
00114
00115 if(errPin == 2 || errPin ==3){
00116
00117 }
00118
00119 }
00120
00121 void PololuQik2::motor1Coast() {
00122 unsigned char message[] = { MOTOR1COASTPACKET };
00123 sendMessage(message, 1);
00124 }
00125
00126 void PololuQik2::stopMotor1() {
00127 unsigned char message[] = { MOTOR1REVERSEPACKET, 0 };
00128 sendMessage(message, 2);
00129 }
00130
00131 void PololuQik2::motor0Reverse(uint8_t speed) {
00132 unsigned char message[] = { MOTOR0REVERSEPACKET, speed };
00133 sendMessage(message, 2);
00134 }
00135
00136 bool PololuQik2::hasCRCError() {
00137 getError();
00138 return errorBitSet(CRCERRORBIT);
00139 }
00140
00141 bool PololuQik2::errorBitSet(uint8_t bitToCheck) {
00142 getError();
00143 return (1 << bitToCheck) & _errByte;
00144 }
00145
00146 bool PololuQik2::hasFrameError() {
00147 getError();
00148 return errorBitSet(FRAMEERRORBIT);
00149 }
00150
00151 bool PololuQik2::hasDataOverrunError() {
00152 getError();
00153 return errorBitSet(DATAOVERRUNERRORBIT);
00154 }
00155
00156 bool PololuQik2::hasFormatError() {
00157 getError();
00158 return errorBitSet(FORMATERRORBIT);
00159 }
00160
00161 bool PololuQik2::hasTimeoutError() {
00162 getError();
00163 return errorBitSet(TIMEOUTERRORBIT);
00164 }
00165
00166 uint8_t PololuQik2::getError() {
00167 unsigned char message[] = { ERRORPACKET };
00168 sendMessage(message, 1);
00169 _errByte = readByte();
00170 return _errByte;
00171 }
00172
00173 void PololuQik2::error(){
00174 Serial.print("An Error has Occurred: ");
00175 Serial.println(getError());
00176 }
00177
00178 uint8_t PololuQik2::readByte(){
00179 return _serial->read();
00180 }
00181
00182 void PololuQik2::sendByte(uint8_t byte){
00183 _serial->print(byte, BYTE);
00184 }