While I was typing out my last post I had a though to constrain Xspeed. Xspeed = constrain(Xspeed, 0, Yspeed);
And it worked. The bot runs perfectly. Thank you very much for all your help and your Example 5 sketch for serial communication.
Here is the final Slave sketch:
#include <SoftwareSerial.h>
SoftwareSerial BTserial(2, 3); // RX | TX
// Connect the HC-05 TX to Arduino pin 2 RX.
// Connect the HC-05 RX to Arduino pin 3 TX through a voltage divider.
const byte numChars = 32;
char receivedChars[numChars];
char tempChars[numChars]; // temporary array for use when parsing
// variables to hold the parsed data
char Go[numChars] = {0};
int RightvalY = 0;
int RightvalX = 0;
int LeftvalY = 0;
int LeftvalX = 0;
boolean newData = false;
const byte MotorApwmPin = 5;
const byte MotorA1Pin = 6;
const byte MotorA2Pin = 7;
const byte MotorBpwmPin = 10;
const byte MotorB1Pin = 8;
const byte MotorB2Pin = 9;
const byte StndbyPin = 11;
const int DeadBand = 30;
unsigned int MidPoint = 508;
unsigned int Yspeed = 0;
unsigned int Xspeed = 0;
//============
void setup() {
BTserial.begin(9600);
Serial.begin(9600);
Serial.println("Arduino Ready ");
Serial.println();
pinMode(MotorA1Pin, OUTPUT);
pinMode(MotorA2Pin, OUTPUT);
pinMode(MotorB1Pin, OUTPUT);
pinMode(MotorB2Pin, OUTPUT);
pinMode(MotorApwmPin, OUTPUT);
pinMode(MotorBpwmPin, OUTPUT);
pinMode(StndbyPin, OUTPUT);
digitalWrite(MotorA1Pin, LOW);
digitalWrite(MotorA2Pin, LOW);
digitalWrite(MotorB1Pin, LOW);
digitalWrite(MotorB2Pin, LOW);
digitalWrite(MotorApwmPin, LOW);
digitalWrite(MotorBpwmPin, LOW);
digitalWrite(StndbyPin, LOW);
}
//============
void loop() {
recvWithStartEndMarkers();
if (newData == true) {
strcpy(tempChars, receivedChars);
// this temporary copy is necessary to protect the original data
// because strtok() used in parseData() replaces the commas with \0
parseData();
showParsedData();
newData = false;
Move();
}
}
//============
void recvWithStartEndMarkers() {
static boolean recvInProgress = false;
static byte ndx = 0;
char startMarker = '<';
char endMarker = '>';
char rc;
while (BTserial.available() > 0 && newData == false) {
rc = BTserial.read();
if (recvInProgress == true) {
if (rc != endMarker) {
receivedChars[ndx] = rc;
ndx++;
if (ndx >= numChars) {
ndx = numChars - 1;
}
}
else {
receivedChars[ndx] = '\0'; // terminate the string
recvInProgress = false;
ndx = 0;
newData = true;
}
}
else if (rc == startMarker) {
recvInProgress = true;
}
}
}
//============
void parseData() { // split the data into its parts
char * strtokIndx; // this is used by strtok() as an index
strtokIndx = strtok(tempChars,","); // get the first part - the string
strcpy(Go, strtokIndx); // copy it to messageFromPC
strtokIndx = strtok(NULL, ","); // this continues where the previous call left off
RightvalY = atoi(strtokIndx); // convert this part to an integer
strtokIndx = strtok(NULL, ","); // this continues where the previous call left off
RightvalX = atoi(strtokIndx); // convert this part to an integer
/* strtokIndx = strtok(NULL, ","); // this continues where the previous call left off
LeftvalY = atoi(strtokIndx); // convert this part to an integer
strtokIndx = strtok(NULL, ","); // this continues where the previous call left off
LeftvalX = atoi(strtokIndx); // convert this part to an integer
*/
}
//============
void showParsedData() {
// Serial.print(Go);
// Serial.print(LeftvalY);
// Serial.print(LeftvalX);
// Serial.print(RightvalY);
// Serial.println(RightvalX);
}
void Move(){
//Reverse
if(RightvalY > MidPoint + DeadBand){
Yspeed = (RightvalY - 514) /2;
if(RightvalX > (MidPoint + DeadBand)){ //Adjusts Left Motor Speed for Turning
Xspeed = (RightvalX - 514) /2;
Xspeed = constrain(Xspeed, 0, Yspeed);
analogWrite(MotorApwmPin, Yspeed);
analogWrite(MotorBpwmPin, Yspeed - Xspeed);
Reverse();
}
else if(RightvalX < (512 - DeadBand)){ //Adjusts Right Motor Speed for Turning
Xspeed = (511 - RightvalX) / 2;
Xspeed = constrain(Xspeed, 0, Yspeed);
analogWrite(MotorApwmPin, Yspeed - Xspeed);
analogWrite(MotorBpwmPin, Yspeed);
Reverse();
}
else{
analogWrite(MotorApwmPin, Yspeed); //Straight Backwards
analogWrite(MotorBpwmPin, Yspeed);
Reverse();
}
}
//-----------------------------
//Forward
else if(RightvalY <= (512 - DeadBand)){
Yspeed = (511 - RightvalY) / 2;
if(RightvalX > (512 + DeadBand)){ //Adjusts Left Motor Speed for Turning
Xspeed = (RightvalX - 514) / 2;
Xspeed = constrain(Xspeed, 0, Yspeed);
analogWrite(MotorApwmPin, Yspeed);
analogWrite(MotorBpwmPin, Yspeed - Xspeed);
Forward();
}
else if(RightvalX < (512 - DeadBand)){ //Adjusts Right Motor Speed for Turning
Xspeed = (511 - RightvalX) / 2;
Xspeed = constrain(Xspeed, 0, Yspeed);
analogWrite(MotorApwmPin, Yspeed- Xspeed);
analogWrite(MotorBpwmPin, Yspeed);
Forward();
}
else{
analogWrite(MotorApwmPin, Yspeed); //Straight Forwards
analogWrite(MotorBpwmPin, Yspeed);
Forward();
}
}
//------------------------------
else{
// Zero Point Turning
if(RightvalX > (512 + DeadBand)){ // zero point turn Left
Xspeed = (RightvalX - 512) / 2;
analogWrite(MotorApwmPin, Xspeed);
analogWrite(MotorBpwmPin, Xspeed);
ZeroTurnLeft();
}
else if(RightvalX < (512 - DeadBand)){// zero point turn Right
Xspeed = (510 - RightvalX) / 2;
analogWrite(MotorApwmPin, Xspeed);
analogWrite(MotorBpwmPin, Xspeed);
ZeroTurnRight();
}
else{
FullStop();
}
}
/* Serial.print("Yspeed ");
Serial.print(Yspeed);
Serial.print(" Xspeed ");
Serial.println(Xspeed); */
}
void Forward(){
digitalWrite(MotorA1Pin, HIGH);
digitalWrite(MotorA2Pin, LOW);
digitalWrite(MotorB1Pin, LOW);
digitalWrite(MotorB2Pin, HIGH);
digitalWrite(StndbyPin, HIGH);
}
void Reverse(){
digitalWrite(MotorA1Pin, LOW);
digitalWrite(MotorA2Pin, HIGH);
digitalWrite(MotorB1Pin, HIGH);
digitalWrite(MotorB2Pin, LOW);
digitalWrite(StndbyPin, HIGH);
}
void ZeroTurnLeft(){
digitalWrite(MotorA1Pin, HIGH);
digitalWrite(MotorA2Pin, LOW);
digitalWrite(MotorB1Pin, HIGH);
digitalWrite(MotorB2Pin, LOW);
digitalWrite(StndbyPin, HIGH);
}
void ZeroTurnRight(){
digitalWrite(MotorA1Pin, LOW);
digitalWrite(MotorA2Pin, HIGH);
digitalWrite(MotorB1Pin, LOW);
digitalWrite(MotorB2Pin, HIGH);
digitalWrite(StndbyPin, HIGH);
}
void FullStop(){
digitalWrite(MotorApwmPin,LOW);
digitalWrite(MotorBpwmPin,LOW);
digitalWrite(MotorA1Pin, LOW);
digitalWrite(MotorA2Pin, LOW);
digitalWrite(MotorB1Pin, LOW);
digitalWrite(MotorB2Pin, LOW);
digitalWrite(StndbyPin, LOW);
}