Code:
#include "RP6RobotBaseLib.h"
// returns max value from to integer
int max(int number1, int number2)
{
if ( number1 > number2) return number1;
else return number2;
}
/*****************************************************************************/
// Main function - The program starts here:
int main(void)
{
int DIR_LEFT = 0; // 0 nothing, 1 FWD, -1 BWD
int DIR_RIGHT = 0; // 0 nothing, 1 FWD, -1 BWD
int M_SPEED_L = 0;
int M_SPEED_R = 0;
int SPEED_STEP = 10;
initRobotBase();
powerON();
setACSPwrHigh();
clearReceptionBuffer();
// ---------------------------------------
// Main loop:
uint8_t buffer_pos = 0;
int charsToReceive = 50;
char receiveBuffer[charsToReceive+1];
int send = 0;
while(true)
{
if(getBufferLength()) // Check if we still have data (means getBufferLength() is not zero)
{
receiveBuffer[buffer_pos] = readChar(); // get next character from reception buffer
if(receiveBuffer[buffer_pos]=='&') // End of line detected!
{
receiveBuffer[buffer_pos+1]='\0'; // Terminate String with a 0, so other routines can determine where it ends!
buffer_pos = -1;
clearReceptionBuffer();
}
else if(buffer_pos >= charsToReceive) // IMPORTANT: We can not receive more characters than "charsToReceive"
{
receiveBuffer[charsToReceive]='\0'; // So if we receive more characters, we just stop reception and terminate the String.
writeString_P("$You entered more characters than possible!%\n");
}
buffer_pos++;
}
// try to find something useful information in the
// received string
// TODO: check if string starts with $ and ends with &
// read all values
else if (strncmp("$Q|ALL",receiveBuffer,6) == 0) {
int BL = getBumperLeft();
int BR = getBumperRight();
int OL = isObstacleLeft();
int OR = isObstacleRight();
char sendValues[85];
memset(sendValues, '\0', sizeof(sendValues) );
sprintf(sendValues,"$M|BL:%d|BR:%d|LDRL:%d|LDRR:%d|UBAT:%d|OL:%d|OR:%d|ML:%d|MR:%d&",BL,BR,adcLSL,adcLSR,adcBat,OL,OR,M_SPEED_L,M_SPEED_R);
writeString(sendValues);
send = 1;
}
// move forward
else if (strncmp("$D|FWD", receiveBuffer,6) == 0) {
if ( DIR_LEFT == -1 || DIR_RIGHT == -1 )
stop();
setMotorDir(FWD,FWD);
if ( M_SPEED_L != M_SPEED_R) {
M_SPEED_L = max(M_SPEED_L,M_SPEED_R);
M_SPEED_R = max(M_SPEED_L,M_SPEED_R);
}
moveAtSpeed(M_SPEED_L,M_SPEED_R);
DIR_LEFT = 1; DIR_RIGHT = 1;
writeString_P("$OK&");
send = 1;
}
// move backwards
else if (strncmp("$D|BWD", receiveBuffer,6) == 0) {
if ( DIR_LEFT == 1 || DIR_RIGHT == 1 )
stop();
setMotorDir(BWD,BWD);
if ( M_SPEED_L != M_SPEED_R) {
M_SPEED_L = max(M_SPEED_L,M_SPEED_R);
M_SPEED_R = max(M_SPEED_L,M_SPEED_R);
}
moveAtSpeed(M_SPEED_L,M_SPEED_R);
DIR_LEFT = -1; DIR_RIGHT = -1;
writeString_P("$OK&");
send = 1;
}
// turn left
else if (strncmp("$D|LEFT", receiveBuffer,7) == 0) {
if ( DIR_LEFT == 1 || DIR_RIGHT == -1 )
stop();
setMotorDir(BWD,FWD);
moveAtSpeed(60,60);
DIR_LEFT = -1; DIR_RIGHT = 1;
writeString_P("$OK&");
send = 1;
}
// turn right
else if (strncmp("$D|RIGHT", receiveBuffer,8) == 0) {
if ( DIR_LEFT == -1 || DIR_RIGHT == 1 )
stop();
setMotorDir(FWD,BWD);
moveAtSpeed(60,60);
DIR_LEFT = 1; DIR_RIGHT = -1;
writeString_P("$OK&");
send = 1;
}
// stop
else if (strncmp("$D|STOP", receiveBuffer,7) == 0) {
stop();
M_SPEED_L = 0;
M_SPEED_R = 0;
writeString_P("$OK&");
send = 1;
}
// increase motorspeed
else if (strncmp("$D|BU", receiveBuffer, 5) == 0) {
if (M_SPEED_L <= (200 - SPEED_STEP) && M_SPEED_R <= (200 - SPEED_STEP)) {
M_SPEED_L += SPEED_STEP;
M_SPEED_R += SPEED_STEP;
}
moveAtSpeed(M_SPEED_L,M_SPEED_R);
writeString_P("$OK&");
send = 1;
}
// decrease motorspeed
else if (strncmp("$D|BD", receiveBuffer, 5) == 0) {
if (M_SPEED_L >= SPEED_STEP && M_SPEED_R >= SPEED_STEP) {
M_SPEED_L -= SPEED_STEP;
M_SPEED_R -= SPEED_STEP;
}
moveAtSpeed(M_SPEED_L,M_SPEED_R);
writeString_P("$OK&");
send = 1;
}
// increase left motorspeed
else if (strncmp("$D|LU", receiveBuffer, 5) == 0) {
if (M_SPEED_L <= (200 - SPEED_STEP)) M_SPEED_L += SPEED_STEP;
moveAtSpeed(M_SPEED_L, M_SPEED_R);
writeString_P("$OK&");
send = 1;
}
// increase right motorspeed
else if (strncmp("$D|RU", receiveBuffer, 5) == 0) {
if (M_SPEED_R <= (200 - SPEED_STEP)) M_SPEED_R += SPEED_STEP;
moveAtSpeed(M_SPEED_L, M_SPEED_R);
writeString_P("$OK&");
send = 1;
}
// decrease left motorspeed
else if (strncmp("$D|LD", receiveBuffer, 5) == 0) {
if (M_SPEED_L >= SPEED_STEP) M_SPEED_L -= SPEED_STEP;
moveAtSpeed(M_SPEED_L, M_SPEED_R);
writeString_P("$OK&");
send = 1;
}
// decrease right motorspeed
else if (strncmp("$D|RD", receiveBuffer, 5) == 0) {
if (M_SPEED_R >= SPEED_STEP) M_SPEED_R -= SPEED_STEP;
moveAtSpeed(M_SPEED_L, M_SPEED_R);
writeString_P("$OK&");
send = 1;
}
// set motorspeed to default value
else if (strncmp("$D|DEFAULT", receiveBuffer, 10) == 0) {
M_SPEED_L = 80;
M_SPEED_R = 80;
writeString_P("$OK&");
send = 1;
}
// nothing detected, set send to 0
else send = 0;
// if something was send, clear receiveBuffer
if (send) {
memset(receiveBuffer, '\0', charsToReceive);
for(uint8_t cnt = 0; cnt < charsToReceive; cnt++)
receiveBuffer[cnt]=0;
}
task_RP6System();
}
// End of main loop!
// ---------------------------------------
return 0;
}
Lesezeichen