✨
This commit is contained in:
NikolajDanger
302
ArloRobot/ArloRobot.cpp
Executable file
302
ArloRobot/ArloRobot.cpp
Executable file
@ -0,0 +1,302 @@
|
||||
/**
|
||||
@file ArloRobot.cpp
|
||||
|
||||
@author Parallax Inc.
|
||||
|
||||
@brief Arlo Robot (with DHB-10 motor controller) functions for Arduino.
|
||||
|
||||
@version 0.5
|
||||
|
||||
@copyright
|
||||
Copyright (c) Parallax Inc 2016. All rights MIT licensed;
|
||||
see license.txt.
|
||||
*/
|
||||
|
||||
#include "ArloRobot.h"
|
||||
|
||||
// Set up/tear down
|
||||
|
||||
void ArloRobot::begin(SoftwareSerial &serial)
|
||||
{
|
||||
dhb10 = &serial;
|
||||
dhb10->print("txpin ch2\r"); // Receive on different pin
|
||||
delay(20); // Wait for reply
|
||||
dhb10->flush(); // Clear input buffer
|
||||
}
|
||||
|
||||
//
|
||||
void ArloRobot::end()
|
||||
{
|
||||
dhb10->end();
|
||||
}
|
||||
//
|
||||
// Movements
|
||||
|
||||
void ArloRobot::writeCounts(long left, long right)
|
||||
{
|
||||
paramVal[0] = left;
|
||||
paramVal[1] = right;
|
||||
paramVal[2] = topSpeed;
|
||||
com("MOVE", 3, 0);
|
||||
}
|
||||
|
||||
void ArloRobot::writeSpeeds(int left, int right)
|
||||
{
|
||||
left = constrain(left, -topSpeed, topSpeed);
|
||||
right = constrain(right, -topSpeed, topSpeed);
|
||||
paramVal[0] = left;
|
||||
paramVal[1] = right;
|
||||
com("GOSPD", 2, 0);
|
||||
}
|
||||
|
||||
void ArloRobot::writeMotorPower(int left, int right)
|
||||
{
|
||||
left = constrain(left, -DHB10_MAX_MOTOR_PWR, DHB10_MAX_MOTOR_PWR);
|
||||
right = constrain(right, -DHB10_MAX_MOTOR_PWR, DHB10_MAX_MOTOR_PWR);
|
||||
paramVal[0] = left;
|
||||
paramVal[1] = right;
|
||||
com("GO", 2, 0);
|
||||
}
|
||||
|
||||
|
||||
// Measurements
|
||||
|
||||
int ArloRobot::readCountsLeft()
|
||||
{
|
||||
|
||||
#ifdef DISPLAY_ACTIVITY
|
||||
Serial.print("left strcmp ");
|
||||
Serial.println(strcmp("DIST", lastCommand));
|
||||
Serial.print("left millis ");
|
||||
Serial.println(millis() - lastReadCount);
|
||||
#endif
|
||||
|
||||
if(((millis() - lastReadCount) > DT_PREV_ENCODER_CHECK || (strcmp("DIST", lastCommand))))
|
||||
{
|
||||
com("DIST", 0, 2);
|
||||
}
|
||||
lastReadCount = millis();
|
||||
return returnVal[0];
|
||||
}
|
||||
|
||||
int ArloRobot::readCountsRight()
|
||||
{
|
||||
#ifdef DISPLAY_ACTIVITY
|
||||
Serial.print("right strcmp ");
|
||||
Serial.println(strcmp("DIST", lastCommand));
|
||||
Serial.print("right millis ");
|
||||
Serial.println(millis() - lastReadCount);
|
||||
#endif
|
||||
|
||||
if(((millis() - lastReadCount) > DT_PREV_ENCODER_CHECK || (strcmp("DIST", lastCommand))))
|
||||
{
|
||||
com("DIST", 0, 2);
|
||||
}
|
||||
lastReadCount = millis();
|
||||
return returnVal[1];
|
||||
}
|
||||
|
||||
int ArloRobot::readSpeedLeft()
|
||||
{
|
||||
if(((millis() - lastReadCount) > DT_PREV_ENCODER_CHECK || (strcmp("SPD", lastCommand))))
|
||||
{
|
||||
com("SPD", 0, 2);
|
||||
}
|
||||
lastReadCount = millis();
|
||||
return returnVal[0];
|
||||
}
|
||||
|
||||
int ArloRobot::readSpeedRight()
|
||||
{
|
||||
if(((millis() - lastReadCount) > DT_PREV_ENCODER_CHECK || (strcmp("SPD", lastCommand))))
|
||||
{
|
||||
com("SPD", 0, 2);
|
||||
}
|
||||
lastReadCount = millis();
|
||||
return returnVal[1];
|
||||
}
|
||||
|
||||
void ArloRobot::writePulseMode()
|
||||
{
|
||||
com("PULSE", 0, 0);
|
||||
}
|
||||
|
||||
|
||||
// Information
|
||||
|
||||
int ArloRobot::readFirmwareVer()
|
||||
{
|
||||
com("VER", 0, 1);
|
||||
return returnVal[0];
|
||||
}
|
||||
|
||||
int ArloRobot::readHardwareVer()
|
||||
{
|
||||
com("HWVER", 0, 1);
|
||||
return returnVal[0];
|
||||
}
|
||||
|
||||
int ArloRobot::readSpeedLimit()
|
||||
{
|
||||
return topSpeed;
|
||||
}
|
||||
|
||||
|
||||
// Configuration
|
||||
void ArloRobot::writeConfig(char *configString, int value)
|
||||
{
|
||||
paramVal[0] = value;
|
||||
com(configString, 1, 0);
|
||||
}
|
||||
|
||||
int ArloRobot::readConfig(char *configString)
|
||||
{
|
||||
com(configString, 0, 1);
|
||||
return returnVal[0];
|
||||
}
|
||||
|
||||
void ArloRobot::writeSpeedLimit(int countsPerSecond)
|
||||
{
|
||||
topSpeed = countsPerSecond;
|
||||
}
|
||||
|
||||
void ArloRobot::clearCounts()
|
||||
{
|
||||
com("RST", 0, 0);
|
||||
}
|
||||
|
||||
|
||||
//Nonvolatile Configuration Storage
|
||||
|
||||
void ArloRobot::storeConfig(char *configString)
|
||||
{
|
||||
char temp[] = {'S','T','O','R','E', ' ' ,0,0,0,0,0,0,0,0,0,0,0} ;
|
||||
strcpy(&temp[6], configString);
|
||||
com(temp, 0, 0);
|
||||
}
|
||||
|
||||
void ArloRobot::restoreConfig()
|
||||
{
|
||||
com("RESTORE", 0, 0);
|
||||
}
|
||||
|
||||
|
||||
//Private
|
||||
|
||||
char *ArloRobot::com(char *command, int paramCount, int retCount)
|
||||
{
|
||||
dhb10->listen();
|
||||
memset(c, 0, LAST_EXCG_STR_LEN);
|
||||
memset(lastExchange, ' ', LAST_EXCG_STR_LEN - 1);
|
||||
lastExchange[LAST_EXCG_STR_LEN - 1] = 0;
|
||||
int idx = 0;
|
||||
|
||||
#ifdef DISPLAY_ACTIVITY
|
||||
Serial.print(command);
|
||||
for(int i = 0; i < paramCount; i++)
|
||||
{
|
||||
Serial.print(" ");
|
||||
Serial.print(paramVal[i], DEC);
|
||||
}
|
||||
Serial.print("\\r "); // Display message
|
||||
#endif
|
||||
|
||||
dhb10->print(command); // Send message to DHB-10
|
||||
strcpy(lastExchange, command);
|
||||
idx = strlen(command);
|
||||
for(int i = 0; i < paramCount; i++)
|
||||
{
|
||||
dhb10->print(" ");
|
||||
lastExchange[idx++] = ' ';
|
||||
//lastExchange[idx] = 0;
|
||||
dhb10->print(paramVal[i], DEC);
|
||||
//idx += sprintf(&lastExchange[idx], "%d", paramVal[i]);
|
||||
char *p = itoa(paramVal[i], &lastExchange[idx], 10);
|
||||
idx += strlen(p);
|
||||
}
|
||||
dhb10->print('\r'); // Send message to DHB-10
|
||||
lastExchange[idx++] = '\\';
|
||||
lastExchange[idx++] = 'r';
|
||||
//lastExchange[idx++] = 0;
|
||||
if(idx < LAST_EXCG_TX_MIN) idx = LAST_EXCG_TX_MIN;
|
||||
|
||||
long ti = millis();
|
||||
dhb10->readBytes(c, 1); // Get first byte of reply
|
||||
long tf = millis();
|
||||
|
||||
#ifdef DISPLAY_ACTIVITY
|
||||
Serial.print("millis = ");
|
||||
Serial.println(tf - ti);
|
||||
Serial.print("Dec = ");
|
||||
Serial.println(c[0], DEC);
|
||||
#endif
|
||||
|
||||
if((tf - ti) > timeoutDHB10)
|
||||
{
|
||||
#ifdef DISPLAY_ACTIVITY
|
||||
Serial.println("No reply (timeout)"); // Display timeout message
|
||||
#endif
|
||||
strcpy(&lastExchange[idx], "No reply (timeout).");
|
||||
}
|
||||
else if(c[0] == 0)
|
||||
{
|
||||
#ifdef DISPLAY_ACTIVITY
|
||||
Serial.println("No reply"); // Display timeout message
|
||||
#endif
|
||||
strcpy(&lastExchange[idx], "No reply.");
|
||||
}
|
||||
else if(c[0] == '\r') // If DHB-10 replied
|
||||
{
|
||||
#ifdef DISPLAY_ACTIVITY
|
||||
Serial.println("\\r"); // Display terminating character
|
||||
#endif
|
||||
strcpy(&lastExchange[idx], "\\r");
|
||||
}
|
||||
else if(c[0] == 'E')
|
||||
{
|
||||
dhb10->readBytesUntil('\r', &c[1], EXCG_STR_LEN - 2);
|
||||
|
||||
#ifdef DISPLAY_ACTIVITY
|
||||
Serial.write(c);
|
||||
Serial.println("\\r");
|
||||
#endif
|
||||
strcpy(&lastExchange[idx], c);
|
||||
idx += strlen(c);
|
||||
strcpy(&lastExchange[idx], "\\r");
|
||||
}
|
||||
else
|
||||
{
|
||||
dhb10->readBytesUntil('\r', &c[1], EXCG_STR_LEN - 2);
|
||||
#ifdef DISPLAY_ACTIVITY
|
||||
Serial.write(c);
|
||||
Serial.println(">>> Else <<<");
|
||||
Serial.println("\\r");
|
||||
#endif
|
||||
strcpy(&lastExchange[idx], c);
|
||||
idx += strlen(c);
|
||||
strcpy(&lastExchange[idx], "\\r");
|
||||
}
|
||||
|
||||
for(int i = 0; i < retCount; i++) returnVal[i] = 0;
|
||||
|
||||
int j = -1;
|
||||
for(int i = 0; i < retCount; i++)
|
||||
{
|
||||
while(isAlpha(c[++j]));
|
||||
if(isdigit(c[j]) || (c[j] == '-'))
|
||||
{
|
||||
//returnVal[i] = atoi(&c[j]);
|
||||
returnVal[i] = atol(&c[j]);
|
||||
while(isDigit(c[++j]));
|
||||
}
|
||||
#ifdef DISPLAY_ACTIVITY
|
||||
Serial.print("rv[");
|
||||
Serial.print(i, DEC);
|
||||
Serial.print("] = ");
|
||||
Serial.println(returnVal[i], DEC);
|
||||
#endif
|
||||
}
|
||||
strcpy(lastCommand, command);
|
||||
//lastCommand = command;
|
||||
return c;
|
||||
}
|
||||
Reference in New Issue
Block a user